/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      121213    R.C.A. Boon       Creation of code.
 *      130117    R.C.A. Boon       Added solved boolean flag to applicable member functions (with
 *                                  help from S. Billemont). Moved constructor to header file.
 *                                  Moved debugged getMaximumNumberOfRevolutions to source file.
 *      130211    R.C.A. Boon       Added hasSolution flag to root finder.
 *      120227    S. Billemont      Removed hasSolution in favor of an exception.
 *      130325    R.C.A. Boon       Removed superfluous sanity check of number of revolutions in
 *                                  execute() function, fixed bug in computation of maximumNumberOf-
 *                                  Revolutions.
 *
 *    References
 *      PyKEP toolbox, Dario Izzo, ESA Advanced Concepts Team.
 *      Richard H. An Introduction to the Mathematics and Methods of Astrodynamics, Revised
 *          Edition.
 *      Battin, AIAA Education Series.
 *
 *    Notes
 *
 */

#include <cmath>

#include <boost/format.hpp>
#include <boost/math/special_functions.hpp> // for asinh and acosh

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/multiRevolutionLambertTargeterIzzo.h"
#include "Tudat/Mathematics/BasicMathematics/convergenceException.h"

namespace tudat
{
namespace mission_segments
{

//! Compute solution for N revolutions and branch.
void MultiRevolutionLambertTargeterIzzo::computeForRevolutionsAndBranch(
        const int aNumberOfRevolutions, const bool aIsRightBranch )
{
    // Adjust parameters for new solution
    numberOfRevolutions = aNumberOfRevolutions;
    isRightBranch = aIsRightBranch;

    // Check whether number of revolutions is possible
    sanityCheckNumberOfRevolutions( );

    // Execute problem solving for new solution
    execute( );
}

//! Get maximum number of revolutions calculated.
int MultiRevolutionLambertTargeterIzzo::getMaximumNumberOfRevolutions( )
{
    if ( !solved )
    {
        transformDimensions( );
        sanityCheckNumberOfRevolutions( );
    }

    return maximumNumberOfRevolutions;
}

//! Sanity check number of revolutions.
void MultiRevolutionLambertTargeterIzzo::sanityCheckNumberOfRevolutions( )
{
    // If not yet defined, calculate number of revolutions possible.
    if ( maximumNumberOfRevolutions == NO_MAXIMUM_REVOLUTIONS )
    {
        // Temporarily store specified number, as numberOfRevolutions is needed to calculate max
        // (this is a tricky way to work, but on the other hand this makes this approach decidedly
        // different from PyKEP routines and it also happens only once per object).
        int copyOfOriginalNumberOfRevolutions = numberOfRevolutions;

        // Calculate first guess of maximum, by dividing the time of flight of the minimum energy
        // ellipse by the normalized time of flight.
        numberOfRevolutions = static_cast< int >(
                    normalizedTimeOfFlight / (
                        mathematical_constants::PI / 2.0
                        * std::sqrt( 2.0 * normalizedSemiPerimeter
                                     * normalizedSemiPerimeter
                                     * normalizedSemiPerimeter ) ) );

        // If the current guess for the maximum is non-zero, then additional analysis is required to
        // determine the correct maximum.
        if( numberOfRevolutions != 0)
        {
            // The following try-block is meant to check whether the solution converges or not. If
            // the current guess for the maximum number of revolutions is correct, then the problem
            // will converge. If it does not, an exception will be thrown stating that it did not
            // converge. Catching this exception allows to decrease the guess only when the
            // exception occurs, and not under other circumstances.
            try
            {
                // Compute root (no further information is required)
                computeRootTimeOfFlight();
            }
            catch( basic_mathematics::ConvergenceException )
            {
                // If the rootfinder did not converge, then the current guess is wrong and needs to
                // be decreased
                numberOfRevolutions--;
            }
        }
        // No further analysis is needed of the current guess is equal to zero.

        // Maximum is now found.
        maximumNumberOfRevolutions = numberOfRevolutions;

        // Reinstating original number of revolutions specified.
        numberOfRevolutions = copyOfOriginalNumberOfRevolutions;
    }

    // Default: compare maximum with specified number of revolutions.
    // If specified is larger than maximum, no solution is possible.
    if ( numberOfRevolutions > maximumNumberOfRevolutions )
    {
        // Throw exception.
        BOOST_THROW_EXCEPTION( std::runtime_error(
            ( boost::format(
                "Number of revolutions specified in Lambert problem is larger than possible.\n"
                "Specified number of revolutions %d while the maximum is %d"
            ) % numberOfRevolutions % maximumNumberOfRevolutions).str( )
        ) );
    }
    // Else, nothing wrong.
}

//! Execute solving procedure (for multiple revolutions).
void MultiRevolutionLambertTargeterIzzo::execute( )
{
    // Sanity checks.
    sanityCheckTimeOfFlight( );
    sanityCheckGravitationalParameter( );

    // Transform dimensions.
    transformDimensions( );

    /*// Sanity check for number of revolutions (must be after dimension removal).
    sanityCheckNumberOfRevolutions( );*/

    if ( numberOfRevolutions == 0 )
    {
        // call base class function that works on zero revolutions.
        ZeroRevolutionLambertTargeterIzzo::execute( );
    }
    else
    {
        // Solve multi-rev root.
        double multipleRevolutionXParameter = computeRootTimeOfFlight( );

        // Reconstruct velocities.
        computeVelocities( multipleRevolutionXParameter );
    }

    solved = true;
}

//! Compute time-of-flight using Lagrange's equation (for multiple revolutions).
double MultiRevolutionLambertTargeterIzzo::computeTimeOfFlight( const double xParameter )
{
    // Determine semi-major axis.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;

        // If long transfer arc.
        if ( isLongway )
        {
            betaParameter = -2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc.
        else
        {
            betaParameter = 2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange including multiple revolutions.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) )
                  + 2.0 * mathematical_constants::PI
                  * numberOfRevolutions );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;

        // If long transfer arc.
        if ( isLongway )
        {
            betaParameter = -2.0 * boost::math::asinh( std::sqrt( ( normalizedSemiPerimeter
                                                                    - normalizedChord )
                                                                  / ( -2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * boost::math::asinh( std::sqrt( ( normalizedSemiPerimeter
                                                                   - normalizedChord )
                                                                 / ( -2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlightLagrange = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlightLagrange;
    }
}

//! Solve the time of flight equation for x (for multiple revolutions).
double MultiRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( )
{
    using mathematical_constants::PI;

    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1, x2;

    if ( isRightBranch )
    { // right branch solution.
        x1 = std::tan( .7234 * PI / 2.0 );
        x2 = std::tan( .5234 * PI / 2.0 );
    }
    else
    { // left branch solution.
        x1 = std::tan( -.5234 * PI / 2.0 );
        x2 = std::tan( -.2234 * PI / 2.0 );
    }

    double y1 = computeTimeOfFlight( std::atan( x1 ) * 2.0 / PI ) - normalizedTimeOfFlight;

    double y2 = computeTimeOfFlight( std::atan( x2 ) * 2.0 / PI ) - normalizedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && ( y1 != y2 )
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = computeTimeOfFlight( std::atan( xNew ) * 2.0 / PI ) - normalizedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        BOOST_THROW_EXCEPTION( basic_mathematics::ConvergenceException(
            ( boost::format(
                "Multi-Revolution Lambert targeter failed to converge to a solution.\n"
                "Reached the maximum number of iterations: %d"
            ) % maximumNumberOfIterations).str( )
        ) );
    }

    // Revert to x parameter.
    double xParameter = std::atan( xNew ) * 2.0 / PI;
    return xParameter;
}

// Add compute maximum number of revolutions routine?

} // namespace mission_segments
} // namespace tudat


//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

// clang-format off

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include "PyImathFun.h"
#include "PyImathFunOperators.h"
#include "PyImathDecorators.h"
#include "PyImathExport.h"
#include "PyImathAutovectorize.h"

namespace PyImath {

using namespace boost::python;

namespace
{

struct RegisterFloatDoubleOps
{
    template <typename T>
    void operator() (T)
    {
        // nb: MSVC gets confused about which arg we want (it thinks it
        // might be boost::arg), so telling it which one explicitly here.
        typedef boost::python::arg arg;

        generate_bindings<abs_op<T>,boost::mpl::true_>(
            "abs",
            "return the absolute value of 'value'",
            (arg("value")));

        generate_bindings<sign_op<T>,boost::mpl::true_>(
            "sign",
            "return 1 or -1 based on the sign of 'value'",
            (arg("value")));

        generate_bindings<log_op<T>,boost::mpl::true_>(
            "log",
            "return the natural log of 'value'",
            (arg("value")));

        generate_bindings<log10_op<T>,boost::mpl::true_>(
            "log10",
            "return the base 10 log of 'value'",
            (arg("value")));

        generate_bindings<lerp_op<T>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
            "lerp",
            "return the linear interpolation of 'a' to 'b' using parameter 't'",
            (arg("a"),arg("b"),arg("t")));

        generate_bindings<lerpfactor_op<T>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
            "lerpfactor",
            "return how far m is between a and b, that is return t such that\n"
            "if:\n"
            "    t = lerpfactor(m, a, b);\n"
            "then:\n"
            "    m = lerp(a, b, t);\n"
            "\n"
            "If a==b, return 0.\n",
            (arg("m"),arg("a"),arg("b")));

        generate_bindings<clamp_op<T>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
            "clamp",
            "return the value clamped to the range [low,high]",
            (arg("value"),arg("low"),arg("high")));

        generate_bindings<floor_op<T>,boost::mpl::true_>(
            "floor",
            "return the closest integer less than or equal to 'value'",
            (arg("value")));

        generate_bindings<ceil_op<T>,boost::mpl::true_>(
            "ceil",
            "return the closest integer greater than or equal to 'value'",
            (arg("value")));

        generate_bindings<trunc_op<T>,boost::mpl::true_>(
            "trunc",
            "return the closest integer with magnitude less than or equal to 'value'",
            (arg("value")));

        generate_bindings<rgb2hsv_op<T>,boost::mpl::true_>(
            "rgb2hsv",
            "return the hsv version of an rgb color",
            args("rgb"));

        generate_bindings<hsv2rgb_op<T>,boost::mpl::true_>(
            "hsv2rgb",
            "return the rgb version of an hsv color",
            args("hsv"));

        generate_bindings<sin_op<T>,boost::mpl::true_>(
            "sin",
            "return the sine of the angle theta",
            args("theta"));

        generate_bindings<cos_op<T>,boost::mpl::true_>(
            "cos",
            "return the cosine of the angle theta",
            args("theta"));

        generate_bindings<tan_op<T>,boost::mpl::true_>(
            "tan",
            "return the tangent of the angle theta",
            args("theta"));

        generate_bindings<asin_op<T>,boost::mpl::true_>(
            "asin",
            "return the arcsine of the value x",
            args("x"));

        generate_bindings<acos_op<T>,boost::mpl::true_>(
            "acos",
            "return the arccosine of the value x",
            args("x"));

        generate_bindings<atan_op<T>,boost::mpl::true_>(
            "atan",
            "return the arctangent of the value x",
            args("x"));

        generate_bindings<atan2_op<T>,boost::mpl::true_,boost::mpl::true_>(
            "atan2",
            "return the arctangent of the coordinate x,y - note the y "
            "is the first argument for consistency with libm ordering",
            args("y","x"));

        generate_bindings<sqrt_op<T>,boost::mpl::true_>(
            "sqrt",
            "return the square root of x",
            args("x"));

        generate_bindings<pow_op<T>,boost::mpl::true_,boost::mpl::true_>(
            "pow",
            "return x**y",
            args("x","y"));

        generate_bindings<exp_op<T>,boost::mpl::true_>(
             "exp",
             "return exp(x)",
             args("x"));

        generate_bindings<sinh_op<T>,boost::mpl::true_>(
             "sinh",
             "return sinh(x)",
             args("x"));

        generate_bindings<cosh_op<T>,boost::mpl::true_>(
             "cosh",
             "return cosh(x)",
             args("x"));

        def("cmp", IMATH_NAMESPACE::cmp<T>);
        def("cmpt", IMATH_NAMESPACE::cmpt<T>);
        def("iszero", IMATH_NAMESPACE::iszero<T>);
        def("equal", IMATH_NAMESPACE::equal<T, T, T>);
    }
};

} // namespace

void register_functions()
{
    //
    // Utility Functions
    //

    // nb: MSVC gets confused about which arg we want (it thinks it
    // might be boost::arg), so telling it which one explicitly here.
    typedef boost::python::arg arg;

    using fp_types = boost::mpl::vector<float, double>;
    boost::mpl::for_each<fp_types>(RegisterFloatDoubleOps());

    generate_bindings<abs_op<int>,boost::mpl::true_>(
        "abs",
        "return the absolute value of 'value'",
        (arg("value")));

    generate_bindings<sign_op<int>,boost::mpl::true_>(
        "sign",
        "return 1 or -1 based on the sign of 'value'",
        (arg("value")));

    generate_bindings<clamp_op<int>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
        "clamp",
        "return the value clamped to the range [low,high]",
        (arg("value"),arg("low"),arg("high")));

    generate_bindings<divs_op,boost::mpl::true_,boost::mpl::true_>(
        "divs",
        "return x/y where the remainder has the same sign as x:\n"
        "    divs(x,y) == (abs(x) / abs(y)) * (sign(x) * sign(y))\n",
        (arg("x"),arg("y")));
    generate_bindings<mods_op,boost::mpl::true_,boost::mpl::true_>(
        "mods",
        "return x%y where the remainder has the same sign as x:\n"
        "    mods(x,y) == x - y * divs(x,y)\n",
        (arg("x"),arg("y")));

    generate_bindings<divp_op,boost::mpl::true_,boost::mpl::true_>(
        "divp",
        "return x/y where the remainder is always positive:\n"
        "    divp(x,y) == floor (double(x) / double (y))\n",
        (arg("x"),arg("y")));
    generate_bindings<modp_op,boost::mpl::true_,boost::mpl::true_>(
        "modp",
        "return x%y where the remainder is always positive:\n"
        "    modp(x,y) == x - y * divp(x,y)\n",
        (arg("x"),arg("y")));

    generate_bindings<bias_op,boost::mpl::true_,boost::mpl::true_>(
         "bias",
         "bias(x,b) is a gamma correction that remaps the unit interval such that bias(0.5, b) = b.",
         (arg("x"),arg("b")));

    generate_bindings<gain_op,boost::mpl::true_,boost::mpl::true_>(
         "gain",
         "gain(x,g) is a gamma correction that remaps the unit interval with the property that gain(0.5, g) = 0.5.\n"
         "The gain function can be thought of as two scaled bias curves forming an 'S' shape in the unit interval.",
         (arg("x"),arg("g")));

    //
    // Vectorized utility functions
    //
    generate_bindings<rotationXYZWithUpDir_op<float>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
        "rotationXYZWithUpDir",
        "return the XYZ rotation vector that rotates 'fromDir' to 'toDir'"
        "using the up vector 'upDir'",
        args("fromDir","toDir","upDir"));
}

} // namespace PyImath


/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/src/mtkmath.hpp
 * @brief several math utility functions.
 */

#ifndef MTKMATH_H_
#define MTKMATH_H_

#include <cmath>
#include <boost/math/tools/precision.hpp>

#include "../types/vect.hpp"

#ifndef M_PI
#define M_PI  3.1415926535897932384626433832795
#endif

namespace MTK {

namespace internal {

template<class Manifold>
struct traits {
	typedef typename Manifold::scalar scalar;
	enum {DOF = Manifold::DOF};
	typedef vect<DOF, scalar> vectorized_type;
	typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
};

template<>
struct traits<float> : traits<Scalar<float> > {};
template<>
struct traits<double> : traits<Scalar<double> > {};

}  // namespace internal

/**
 * \defgroup MTKMath Mathematical helper functions
 */
//@{

//! constant @f$ \pi @f$
const double pi = M_PI;

template<class scalar> inline scalar tolerance();

template<> inline float  tolerance<float >() { return 1e-5f; }
template<> inline double tolerance<double>() { return 1e-11; }

/**
 * normalize @a x to @f$[-bound, bound] @f$.
 *
 * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
 */
template<class scalar>
inline scalar normalize(scalar x, scalar bound){
	if(std::fabs(x) <= bound) return x;
	int r = (int)(x *(scalar(1.0)/ bound));
	return x - ((r + (r>>31) + 1) & ~1)*bound;
}

//TODO this implementation might be faster?
inline double normalize2(double x, double interval){
	return x - interval * (rint(x * (1.0/interval)));
}

/**
 * Calculate cosine and sinc of sqrt(x2).
 * @param x2 the squared angle must be non-negative
 * @return a pair containing cos and sinc of sqrt(x2)
 */
template<class scalar>
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
	using std::sqrt;
	using std::cos;
	using std::sin;
	static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
	static scalar const taylor_2_bound = sqrt(taylor_0_bound);
	static scalar const taylor_n_bound = sqrt(taylor_2_bound);

	assert(x2>=0 && "argument must be non-negative");

	// FIXME check if bigger bounds are possible
	if(x2>=taylor_n_bound) {
		// slow fall-back solution
		scalar x = sqrt(x2);
		return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
	}

	// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
	// TODO Find optimal coefficients using Remez algorithm
	static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
	scalar cosi = 1., sinc=1;
	scalar term = -1/2. * x2;
	for(int i=0; i<3; ++i) {
		cosi += term;
		term *= inv[2*i];
		sinc += term;
		term *= -inv[2*i+1] * x2;
	}

	return std::make_pair(cosi, sinc);

}

template<class scalar, int n>
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
	scalar norm2 = vec.squaredNorm();
	std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
	scalar mult = cos_sinc.second * scale; // == std::sin(alpha) / norm;
	result = mult * vec;
	return cos_sinc.first;
}

/**
 * Inverse function to @c exp.
 *
 * @param result @c vectview to the result
 * @param w      scalar part of input
 * @param vec    vector part of input
 * @param scale  scale result by this value
 * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
 */
template<class scalar, int n>
void log(vectview<scalar, n> result,
		const scalar &w, const vectview<const scalar, n> vec,
		const scalar &scale, bool plus_minus_periodicity)
{
	// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
	scalar nv = vec.norm();
	if(nv < tolerance<scalar>()) {
		if(!plus_minus_periodicity && w < 0) {
			// find the maximal entry:
			int i;
			nv = vec.cwiseAbs().maxCoeff(&i);
			result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
			return;
		}
		nv = tolerance<scalar>();
	}
	scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );

	result = s * vec;
}

//@}

} // namespace MTK

#endif /* MTKMATH_H_ */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_REM_PIO2_CEPHES_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_REM_PIO2_CEPHES_HPP_INCLUDED
#include <nt2/trigonometric/functions/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/round.hpp>
#include <nt2/include/functions/simd/fast_toint.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/bitwise_and.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/twoopi.hpp>
#include <nt2/include/constants/pio2_1.hpp>
#include <nt2/include/constants/pio2_2.hpp>
#include <nt2/include/constants/pio2_3.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::rem_pio2_cephes_, tag::cpu_
                            , (A0)(A1)
                            , (generic_ < floating_<A0> > )
                              (generic_ < integer_<A1>  > )
                              (generic_ < floating_<A0> > )
                            )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& x, A1 & n, A0 & xr)
    {
      n = nt2::rem_pio2_cephes(x, xr);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::rem_pio2_cephes_, tag::cpu_
                            , (A0)
                            , (generic_ < floating_<A0> > )
                            )
  {
    typedef typename nt2::meta::as_integer<A0>::type int_t;
    typedef std::pair<int_t, A0>               result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type res;
      res.first = nt2::rem_pio2_cephes(a0,res.second);
      return res;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::rem_pio2_cephes_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A0> >)
                            )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& x, A0 & xr) const
    {
      A0 xi =  nt2::round(x*nt2::Twoopi<A0>());
      xr  = x-xi*nt2::Pio2_1<A0>();
      xr -= xi*nt2::Pio2_2<A0>();
      xr -= xi*nt2::Pio2_3<A0>();
      return nt2::bitwise_and(nt2::fast_toint(xi), Three<result_type>());
    }
  };
} }

#endif


/*
 * @Description: Extended Kalman Filter for IMU-Lidar-GNSS-Odo-Mag fusion
 * @Author: Ge Yao
 * @Date: 2020-11-12 15:14:07
 */
#include <limits>

#include <cmath>
#include <iostream>
#include <fstream>
#include <ostream>

// use sophus to handle so3 hat & SO3 log operations:
#include <sophus/so3.hpp>

// SVD for observability analysis:
#include <Eigen/SVD>

// mag table lookup:
#include "lidar_localization/models/mag_table/geo_mag_declination.hpp"

#include "lidar_localization/models/kalman_filter/extended_kalman_filter.hpp"

#include "lidar_localization/global_defination/global_defination.h"

#include "lidar_localization/tools/CSVWriter.hpp"

#include "glog/logging.h"

namespace lidar_localization {

ExtendedKalmanFilter::ExtendedKalmanFilter(const YAML::Node& node) {
    //
    // parse config:
    //
    // a. earth constants:
    EARTH.GRAVITY_MAGNITUDE = node["earth"]["gravity_magnitude"].as<double>();
    EARTH.ROTATION_SPEED = node["earth"]["rotation_speed"].as<double>();

    EARTH.LATITUDE = node["earth"]["latitude"].as<double>();
    EARTH.LONGITUDE = node["earth"]["longitude"].as<double>();

    /*
    // TODO: fix missing dependency mathlib
    double dec = get_mag_declination(
        static_cast<float>(EARTH.LATITUDE), static_cast<float>(EARTH.LONGITUDE)
    );
    double inc = get_mag_inclination(
        static_cast<float>(EARTH.LATITUDE), static_cast<float>(EARTH.LONGITUDE)
    );
    double mag = get_mag_strength(
        static_cast<float>(EARTH.LATITUDE), static_cast<float>(EARTH.LONGITUDE)
    );

    EARTH.MAG.B_U = -mag*sin(inc/180.0*M_PI);
    EARTH.MAG.B_N = +mag*cos(inc/180.0*M_PI)*cos(dec/180.0*M_PI);
    EARTH.MAG.B_E = +mag*cos(inc/180.0*M_PI)*sin(dec/180.0*M_PI);
    */

    EARTH.MAG.B_E = node["earth"]["magneto"]["B_E"].as<double>();
    EARTH.MAG.B_N = node["earth"]["magneto"]["B_N"].as<double>();
    EARTH.MAG.B_U = node["earth"]["magneto"]["B_U"].as<double>();

    EARTH.LATITUDE *= M_PI / 180.0;

    // b. prior state covariance:
    COV.PRIOR.POSI = node["covariance"]["prior"]["pos"].as<double>();
    COV.PRIOR.VEL = node["covariance"]["prior"]["vel"].as<double>();
    // TODO: find a better way for quaternion orientation prior covariance assignment
    COV.PRIOR.ORI = node["covariance"]["prior"]["ori"].as<double>();
    COV.PRIOR.EPSILON = node["covariance"]["prior"]["epsilon"].as<double>();
    COV.PRIOR.DELTA = node["covariance"]["prior"]["delta"].as<double>();
    // c. process noise:
    COV.PROCESS.GYRO = node["covariance"]["process"]["gyro"].as<double>();
    COV.PROCESS.ACCEL = node["covariance"]["process"]["accel"].as<double>();
    // d. measurement noise:
    COV.MEASUREMENT.POSE.POSI = node["covariance"]["measurement"]["pose"]["pos"].as<double>();
    COV.MEASUREMENT.POSE.ORI = node["covariance"]["measurement"]["pose"]["ori"].as<double>();
    COV.MEASUREMENT.POSI = node["covariance"]["measurement"]["pos"].as<double>();
    COV.MEASUREMENT.VEL = node["covariance"]["measurement"]["vel"].as<double>();
    COV.MEASUREMENT.MAG = node["covariance"]["measurement"]["mag"].as<double>();
    // e. motion constraint:
    MOTION_CONSTRAINT.ACTIVATED = node["motion_constraint"]["activated"].as<bool>();
    MOTION_CONSTRAINT.W_B_THRESH = node["motion_constraint"]["w_b_thresh"].as<double>();

    // prompt:
    LOG(INFO) << std::endl
              << "Iterative Extended Kalman Filter params:" << std::endl
              << "\tgravity magnitude: " << EARTH.GRAVITY_MAGNITUDE << std::endl
              << "\tearth rotation speed: " << EARTH.ROTATION_SPEED << std::endl
              << "\tlatitude: " << EARTH.LATITUDE << std::endl
              << "\tlongitude: " << EARTH.LATITUDE << std::endl
              << "\tmagneto: " << std::endl
              << "\t\tB_E: " << EARTH.MAG.B_E << std::endl
              << "\t\tB_N: " << EARTH.MAG.B_N << std::endl
              << "\t\tB_U: " << EARTH.MAG.B_U << std::endl
              << std::endl
              << "\tprior cov. pos.: " << COV.PRIOR.POSI  << std::endl
              << "\tprior cov. vel.: " << COV.PRIOR.VEL << std::endl
              << "\tprior cov. ori: " << COV.PRIOR.ORI << std::endl
              << "\tprior cov. epsilon.: " << COV.PRIOR.EPSILON  << std::endl
              << "\tprior cov. delta.: " << COV.PRIOR.DELTA << std::endl
              << std::endl
              << "\tprocess noise gyro.: " << COV.PROCESS.GYRO << std::endl
              << "\tprocess noise accel.: " << COV.PROCESS.ACCEL << std::endl
              << std::endl
              << "\tmeasurement noise pose.: " << std::endl
              << "\t\tpos: " << COV.MEASUREMENT.POSE.POSI << ", ori.: " << COV.MEASUREMENT.POSE.ORI << std::endl
              << "\tmeasurement noise pos.: " << COV.MEASUREMENT.POSI << std::endl
              << "\tmeasurement noise vel.: " << COV.MEASUREMENT.VEL << std::endl
              << "\tmeasurement noise mag.: " << COV.MEASUREMENT.MAG << std::endl
              << std::endl
              << "\tmotion constraint: " << std::endl
              << "\t\tactivated: " << (MOTION_CONSTRAINT.ACTIVATED ? "true" : "false") << std::endl
              << "\t\tw_b threshold: " << MOTION_CONSTRAINT.W_B_THRESH << std::endl
              << std::endl;

    //
    // init filter:
    //
    // a. earth constants:
    g_ = Eigen::Vector3d(
        0.0,
        0.0,
        EARTH.GRAVITY_MAGNITUDE
    );
    w_ = Eigen::Vector3d(
        0.0,
        EARTH.ROTATION_SPEED*cos(EARTH.LATITUDE),
        EARTH.ROTATION_SPEED*sin(EARTH.LATITUDE)
    );
    b_ = Eigen::Vector3d(
        EARTH.MAG.B_E,
        EARTH.MAG.B_N,
        EARTH.MAG.B_U
    );

    // b. prior state & covariance:
    ResetState();
    ResetCovariance();

    // c. process noise:
    Q_.block<3, 3>(0, 0) = COV.PROCESS.GYRO*Eigen::Matrix3d::Identity();
    Q_.block<3, 3>(3, 3) = COV.PROCESS.ACCEL*Eigen::Matrix3d::Identity();

    // d. measurement noise:
    RPose_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSE.POSI*Eigen::Matrix3d::Identity();
    RPose_.block<4, 4>(3, 3) = COV.MEASUREMENT.POSE.ORI*Eigen::Matrix4d::Identity();

    RPoseVel_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSE.POSI*Eigen::Matrix3d::Identity();
    RPoseVel_.block<4, 4>(3, 3) = COV.MEASUREMENT.POSE.ORI*Eigen::Matrix4d::Identity();
    RPoseVel_.block<3, 3>(7, 7) = COV.MEASUREMENT.VEL*Eigen::Matrix3d::Identity();

    RPosi_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();

    RPosiVel_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();
    RPosiVel_.block<3, 3>(3, 3) = COV.MEASUREMENT.VEL*Eigen::Matrix3d::Identity();

    RPosiMag_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();
    RPosiMag_.block<3, 3>(3, 3) = COV.MEASUREMENT.MAG*Eigen::Matrix3d::Identity();

    RPosiVelMag_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSI*Eigen::Matrix3d::Identity();
    RPosiVelMag_.block<3, 3>(3, 3) = COV.MEASUREMENT.VEL*Eigen::Matrix3d::Identity();
    RPosiVelMag_.block<3, 3>(6, 6) = COV.MEASUREMENT.MAG*Eigen::Matrix3d::Identity();

    // e. process equation:
    F_.block<3, 3>( INDEX_POS, INDEX_VEL ) = Eigen::Matrix3d::Identity();

    // f. measurement equation:
    GPose_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();
    GPose_.block<4, 4>( 3, INDEX_ORI ) = Eigen::Matrix4d::Identity();

    GPoseVel_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();
    GPoseVel_.block<4, 4>( 3, INDEX_ORI ) = Eigen::Matrix4d::Identity();

    GPosi_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    GPosiVel_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    GPosiMag_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    GPosiVelMag_.block<3, 3>( 0, INDEX_POS ) = Eigen::Matrix3d::Identity();

    // init soms:
    QPose_.block<DIM_MEASUREMENT_POSE, DIM_STATE>(0, 0) = GPose_;
    QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>(0, 0) = GPoseVel_;
    QPosi_.block<DIM_MEASUREMENT_POSI, DIM_STATE>(0, 0) = GPosi_;
    QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>(0, 0) = GPosiVel_;
    QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>(0, 0) = GPosiMag_;
    QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>(0, 0) = GPosiVelMag_;
}

/**
 * @brief  init filter
 * @param  pose, init pose
 * @param  vel, init vel
 * @param  imu_data, init IMU measurements
 * @return true if success false otherwise
 */
void ExtendedKalmanFilter::Init(
    const Eigen::Vector3d &vel,
    const IMUData &imu_data
) {
    // get init C_nb from IMU estimation:
    Eigen::Matrix3d C_nb = imu_data.GetOrientationMatrix().cast<double>();
    Eigen::Vector4d q_nb(
        imu_data.orientation.w,
        imu_data.orientation.x,
        imu_data.orientation.y,
        imu_data.orientation.z
    );
    // get init v_n from v_b:
    Eigen::Vector3d v_n = C_nb*vel;

    // set init pose:
    init_pose_.block<3, 3>(0, 0) = C_nb;

    // set init velocity:
    init_vel_ = v_n;

    // set init state:
    X_.block<3, 1>(INDEX_POS, 0) = init_pose_.block<3, 1>(0, 3);
    X_.block<3, 1>(INDEX_VEL, 0) = v_n;
    X_.block<4, 1>(INDEX_ORI, 0) = q_nb;
    X_.block<3, 1>(INDEX_GYRO_BIAS, 0) = gyro_bias_;
    X_.block<3, 1>(INDEX_ACCEL_BIAS, 0) = accel_bias_;

    // init IMU data buffer:
    imu_data_buff_.clear();
    imu_data_buff_.push_back(imu_data);

    // init filter time:
    time_ = imu_data.time;

    LOG(INFO) << std::endl
              << "IEKF Inited at " << static_cast<int>(time_) << std::endl
              << "Init Position: "
              << init_pose_(0, 3) << ", "
              << init_pose_(1, 3) << ", "
              << init_pose_(2, 3) << std::endl
              << "Init Velocity: "
              << init_vel_.x() << ", "
              << init_vel_.y() << ", "
              << init_vel_.z() << std::endl;
}

/**
 * @brief  Kalman update
 * @param  imu_data, input IMU measurements
 * @return true if success false otherwise
 */
bool ExtendedKalmanFilter::Update(const IMUData &imu_data) {
    // update IMU buff:
    if (time_ < imu_data.time) {
        // Kalman prediction for covariance:
        UpdateCovarianceEstimation(imu_data);

        // Kalman prediction for state:
        imu_data_buff_.push_back(imu_data);
        UpdateStateEstimation();
        imu_data_buff_.pop_front();

        // move forward:
        time_ = imu_data.time;

        return true;
    }

    return false;
}

/**
 * @brief  Kalman correction, pose measurement and other measurement in body frame
 * @param  measurement_type, input measurement type
 * @param  measurement, input measurement
 * @return void
 */
bool ExtendedKalmanFilter::Correct(
    const IMUData &imu_data,
    const MeasurementType &measurement_type, const Measurement &measurement
) {
    static Measurement measurement_;

    // get time delta:
    double time_delta = measurement.time - time_;

    if ( time_delta > -0.05 ) {
        // perform Kalman prediction:
        if ( time_ < measurement.time ) {
            Update(imu_data);
        }

        // get observation in navigation frame:
        measurement_ = measurement;
        measurement_.T_nb = init_pose_ * measurement_.T_nb;

        // correct error estimation:
        CorrectStateEstimation(measurement_type, measurement_);

        return true;
    }

    LOG(INFO) << "IEKF Correct: Observation is not synced with filter. Skip, "
              << (int)measurement.time << " <-- " << (int)time_ << " @ " << time_delta
              << std::endl;

    return false;
}

/**
 * @brief  get odometry estimation
 * @param  pose, init pose
 * @param  vel, init vel
 * @return void
 */
void ExtendedKalmanFilter::GetOdometry(
    Eigen::Matrix4f &pose, Eigen::Vector3f &vel
) {
    // init:
    Eigen::Matrix4d pose_double = Eigen::Matrix4d::Identity();
    Eigen::Vector3d vel_double = Eigen::Vector3d::Zero();

    // eliminate error:
    // a. position:
    pose_double.block<3, 1>(0, 3) = X_.block<3, 1>(INDEX_POS, 0);
    // b. velocity:
    vel_double = X_.block<3, 1>(INDEX_VEL, 0);
    // c. orientation:
    Eigen::Quaterniond q_nb(
        X_(INDEX_ORI + 0, 0),
        X_(INDEX_ORI + 1, 0),
        X_(INDEX_ORI + 2, 0),
        X_(INDEX_ORI + 3, 0)
    );
    pose_double.block<3, 3>(0, 0) = q_nb.toRotationMatrix();

    // finally:
    pose_double = init_pose_.inverse() * pose_double;
    vel_double = init_pose_.block<3, 3>(0, 0).transpose() * vel_double;

    pose = pose_double.cast<float>();
    vel = vel_double.cast<float>();
}

/**
 * @brief  get covariance estimation
 * @param  cov, covariance output
 * @return void
 */
void ExtendedKalmanFilter::GetCovariance(Cov &cov) {
    static int OFFSET_X = 0;
    static int OFFSET_Y = 1;
    static int OFFSET_Z = 2;

    // a. delta position:
    cov.pos.x = P_(INDEX_POS + OFFSET_X, INDEX_POS + OFFSET_X);
    cov.pos.y = P_(INDEX_POS + OFFSET_Y, INDEX_POS + OFFSET_Y);
    cov.pos.z = P_(INDEX_POS + OFFSET_Z, INDEX_POS + OFFSET_Z);

    // b. delta velocity:
    cov.vel.x = P_(INDEX_VEL + OFFSET_X, INDEX_VEL + OFFSET_X);
    cov.vel.y = P_(INDEX_VEL + OFFSET_Y, INDEX_VEL + OFFSET_Y);
    cov.vel.z = P_(INDEX_VEL + OFFSET_Z, INDEX_VEL + OFFSET_Z);

    // c. delta orientation:
    cov.ori.w = P_(INDEX_ORI + 0, INDEX_ORI + 0);
    cov.ori.x = P_(INDEX_ORI + 1, INDEX_ORI + 1);
    cov.ori.y = P_(INDEX_ORI + 2, INDEX_ORI + 2);
    cov.ori.z = P_(INDEX_ORI + 3, INDEX_ORI + 3);

    // d. gyro. bias:
    cov.gyro_bias.x = P_(INDEX_GYRO_BIAS + OFFSET_X, INDEX_GYRO_BIAS + OFFSET_X);
    cov.gyro_bias.y = P_(INDEX_GYRO_BIAS + OFFSET_Y, INDEX_GYRO_BIAS + OFFSET_Y);
    cov.gyro_bias.z = P_(INDEX_GYRO_BIAS + OFFSET_Z, INDEX_GYRO_BIAS + OFFSET_Z);

    // e. accel bias:
    cov.accel_bias.x = P_(INDEX_ACCEL_BIAS + OFFSET_X, INDEX_ACCEL_BIAS + OFFSET_X);
    cov.accel_bias.y = P_(INDEX_ACCEL_BIAS + OFFSET_Y, INDEX_ACCEL_BIAS + OFFSET_Y);
    cov.accel_bias.z = P_(INDEX_ACCEL_BIAS + OFFSET_Z, INDEX_ACCEL_BIAS + OFFSET_Z);
}

/**
 * @brief  get unbiased angular velocity in body frame
 * @param  angular_vel, angular velocity measurement
 * @param  C_nb, corresponding orientation of measurement
 * @return unbiased angular velocity in body frame
 */
inline Eigen::Vector3d ExtendedKalmanFilter::GetUnbiasedAngularVel(
    const Eigen::Vector3d &angular_vel,
    const Eigen::Matrix3d &C_nb
) {
    return angular_vel - gyro_bias_;
}

/**
 * @brief  get unbiased linear acceleration in navigation frame
 * @param  linear_acc, linear acceleration measurement
 * @param  C_nb, corresponding orientation of measurement
 * @return unbiased linear acceleration in navigation frame
 */
inline Eigen::Vector3d ExtendedKalmanFilter::GetUnbiasedLinearAcc(
    const Eigen::Vector3d &linear_acc,
    const Eigen::Matrix3d &C_nb
) {
    return C_nb*(linear_acc - accel_bias_) - g_;
}

/**
 * @brief  remove gravity component from accel measurement
 * @param  f_b, accel measurement measurement
 * @param  C_nb, orientation matrix
 * @return f_b
 */
inline Eigen::Vector3d ExtendedKalmanFilter::RemoveGravity (
    const Eigen::Vector3d &f_b,
    const Eigen::Matrix3d &C_nb
) {
    return f_b - C_nb.transpose()*g_;
}

/**
 * @brief  apply motion constraint on velocity estimation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::ApplyMotionConstraint(void) {
    const Eigen::Quaterniond q_nb(
        X_( INDEX_ORI + 0, 0),
        X_( INDEX_ORI + 1, 0),
        X_( INDEX_ORI + 2, 0),
        X_( INDEX_ORI + 3, 0)
    );
    const Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();

    Eigen::Vector3d v_b = C_nb.transpose() * X_.block<3, 1>( INDEX_VEL, 0 );
    v_b.y() = 0.0;
    X_.block<3, 1>( INDEX_VEL, 0 ) = C_nb * v_b;
}

/**
 * @brief  get angular delta
 * @param  index_curr, current imu measurement buffer index
 * @param  index_prev, previous imu measurement buffer index
 * @param  angular_delta, angular delta output
 * @return true if success false otherwise
 */
bool ExtendedKalmanFilter::GetAngularDelta(
    const size_t index_curr, const size_t index_prev,
    Eigen::Vector3d &angular_delta
) {
    if (
        index_curr <= index_prev ||
        imu_data_buff_.size() <= index_curr
    ) {
        return false;
    }

    const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
    const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

    double delta_t = imu_data_curr.time - imu_data_prev.time;

    Eigen::Vector3d angular_vel_curr = Eigen::Vector3d(
        imu_data_curr.angular_velocity.x,
        imu_data_curr.angular_velocity.y,
        imu_data_curr.angular_velocity.z
    );
    Eigen::Matrix3d R_curr = imu_data_curr.GetOrientationMatrix().cast<double>();
    angular_vel_curr = GetUnbiasedAngularVel(angular_vel_curr, R_curr);

    Eigen::Vector3d angular_vel_prev = Eigen::Vector3d(
        imu_data_prev.angular_velocity.x,
        imu_data_prev.angular_velocity.y,
        imu_data_prev.angular_velocity.z
    );
    Eigen::Matrix3d R_prev = imu_data_prev.GetOrientationMatrix().cast<double>();
    angular_vel_prev = GetUnbiasedAngularVel(angular_vel_prev, R_prev);

    angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev);

    return true;
}

/**
 * @brief  update orientation with effective rotation angular_delta
 * @param  angular_delta, effective rotation
 * @param  R_curr, current orientation
 * @param  R_prev, previous orientation
 * @return void
 */
void ExtendedKalmanFilter::UpdateOrientation(
    const Eigen::Vector3d &angular_delta,
    Eigen::Matrix3d &R_curr, Eigen::Matrix3d &R_prev
) {
    // magnitude:
    double angular_delta_mag = angular_delta.norm();
    // direction:
    Eigen::Vector3d angular_delta_dir = angular_delta.normalized();

    // build delta q:
    double angular_delta_cos = cos(angular_delta_mag/2.0);
    double angular_delta_sin = sin(angular_delta_mag/2.0);

    Eigen::Quaterniond q(
        X_(INDEX_ORI + 0, 0),
        X_(INDEX_ORI + 1, 0),
        X_(INDEX_ORI + 2, 0),
        X_(INDEX_ORI + 3, 0)
    );
    Eigen::Quaterniond dq(
        angular_delta_cos,
        angular_delta_sin*angular_delta_dir.x(),
        angular_delta_sin*angular_delta_dir.y(),
        angular_delta_sin*angular_delta_dir.z()
    );

    // update:
    R_prev = q.toRotationMatrix();

    q = q*dq;
    q.normalize();

    R_curr = q.toRotationMatrix();

    // write back:
    X_(INDEX_ORI + 0, 0) = q.w();
    X_(INDEX_ORI + 1, 0) = q.x();
    X_(INDEX_ORI + 2, 0) = q.y();
    X_(INDEX_ORI + 3, 0) = q.z();
}

/**
 * @brief  get velocity delta
 * @param  index_curr, current imu measurement buffer index
 * @param  index_prev, previous imu measurement buffer index
 * @param  R_curr, corresponding orientation of current imu measurement
 * @param  R_prev, corresponding orientation of previous imu measurement
 * @param  velocity_delta, velocity delta output
 * @param  linear_acc_mid, mid-value unbiased linear acc
 * @return true if success false otherwise
 */
bool ExtendedKalmanFilter::GetVelocityDelta(
    const size_t index_curr, const size_t index_prev,
    const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev,
    double &T,
    Eigen::Vector3d &velocity_delta
) {
    if (
        index_curr <= index_prev ||
        imu_data_buff_.size() <= index_curr
    ) {
        return false;
    }

    const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
    const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

    T = imu_data_curr.time - imu_data_prev.time;

    Eigen::Vector3d linear_acc_curr = Eigen::Vector3d(
        imu_data_curr.linear_acceleration.x,
        imu_data_curr.linear_acceleration.y,
        imu_data_curr.linear_acceleration.z
    );
    linear_acc_curr = GetUnbiasedLinearAcc(linear_acc_curr, R_curr);
    Eigen::Vector3d linear_acc_prev = Eigen::Vector3d(
        imu_data_prev.linear_acceleration.x,
        imu_data_prev.linear_acceleration.y,
        imu_data_prev.linear_acceleration.z
    );
    linear_acc_prev = GetUnbiasedLinearAcc(linear_acc_prev, R_prev);

    // mid-value acc can improve error state prediction accuracy:
    velocity_delta = 0.5*T*(linear_acc_curr + linear_acc_prev);

    return true;
}

/**
 * @brief  update orientation with effective velocity change velocity_delta
 * @param  T, timestamp delta
 * @param  velocity_delta, effective velocity change
 * @return void
 */
void ExtendedKalmanFilter::UpdatePosition(const double &T, const Eigen::Vector3d &velocity_delta) {
    X_.block<3, 1>(INDEX_POS, 0) += T*X_.block<3, 1>(INDEX_VEL, 0) + 0.5*T*velocity_delta;
    X_.block<3, 1>(INDEX_VEL, 0) += velocity_delta;
}

/**
 * @brief  update state estimation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::UpdateStateEstimation(void) {
    // get deltas:
    Eigen::Vector3d angular_delta;
    GetAngularDelta(1, 0, angular_delta);

    // update orientation:
    Eigen::Matrix3d R_curr, R_prev;
    UpdateOrientation(angular_delta, R_curr, R_prev);

    // get velocity delta:
    double T;
    Eigen::Vector3d velocity_delta;
    // save mid-value unbiased linear acc for error-state update:
    GetVelocityDelta(1, 0, R_curr, R_prev, T, velocity_delta);

    // update position:
    UpdatePosition(T, velocity_delta);

    // apply motion constraint:
    ApplyMotionConstraint();
}

/**
 * @brief  get block matrix for velocity update by orientation quaternion
 * @param  f_b, accel measurement
 * @param  q_nb, orientation quaternion
 * @return block matrix Fvq
 */
Eigen::Matrix<double, 3, 4> ExtendedKalmanFilter::GetFVelOri(
    const Eigen::Vector3d &f_b,
    const Eigen::Quaterniond &q_nb
) {
    // get F:
    Eigen::Matrix<double, 4, 3> T_Ff;
    T_Ff << +q_nb.w(), -q_nb.z(), +q_nb.y(),
            +q_nb.x(), +q_nb.y(), +q_nb.z(),
            -q_nb.y(), +q_nb.x(), +q_nb.w(),
            -q_nb.z(), -q_nb.w(), +q_nb.x();
    Eigen::Vector4d F = 2 * T_Ff * f_b;

    // get Fvq:
    Eigen::Matrix<double, 3, 4> Fvq;
    Fvq << +F(0), +F(1), +F(2), +F(3),
           -F(3), -F(2), +F(1), +F(0),
           +F(2), -F(3), -F(0), +F(1);

    return Fvq;
}

/**
 * @brief  get block matrix for orientation quaternion update by orientation quaternion
 * @param  w_b, gyro measurement
 * @return block matrix Fqq
 */
Eigen::Matrix<double, 4, 4> ExtendedKalmanFilter::GetFOriOri(
    const Eigen::Vector3d &w_b
) {
    // get Fqq:
    Eigen::Matrix<double, 4, 4> Fqq;
    Fqq <<      0.0, -w_b.x(), -w_b.y(), -w_b.z(),
           +w_b.x(),      0.0, +w_b.z(), -w_b.y(),
           +w_b.y(), -w_b.z(),      0.0, +w_b.x(),
           +w_b.z(), +w_b.y(), -w_b.x(),      0.0;

    return 0.5 * Fqq;
}

/**
 * @brief  get block matrix for orientation quaternion update by epsilon, angular velocity bias
 * @param  q_nb, orientation quaternion
 * @return block matrix Fqe
 */
Eigen::Matrix<double, 4, 3> ExtendedKalmanFilter::GetFOriEps(
    const Eigen::Quaterniond &q_nb
) {
    // get Fqe:
    Eigen::Matrix<double, 4, 3> Fqe;
    Fqe << -q_nb.x(), -q_nb.y(), -q_nb.z(),
           +q_nb.w(), -q_nb.z(), +q_nb.y(),
           +q_nb.z(), +q_nb.w(), -q_nb.x(),
           -q_nb.y(), +q_nb.x(), +q_nb.w();

    return 0.5 * Fqe;
}

/**
 * @brief  set process equation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::SetProcessEquation(const IMUData &imu_data) {
    // parse IMU measurement:
    const Eigen::Vector3d f_b(
        imu_data.linear_acceleration.x,
        imu_data.linear_acceleration.y,
        imu_data.linear_acceleration.z
    );
    const Eigen::Vector3d w_b(
        imu_data.angular_velocity.x,
        imu_data.angular_velocity.y,
        imu_data.angular_velocity.z
    );

    // parse orientation:
    const Eigen::Quaterniond q_nb(
        X_(INDEX_ORI + 0, 0),
        X_(INDEX_ORI + 1, 0),
        X_(INDEX_ORI + 2, 0),
        X_(INDEX_ORI + 3, 0)
    );
    const Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();

    //
    // EKF is linearized around VectorX::Zero
    //
    // a. set equation for velocity:
    F_.block<3, 4>(INDEX_VEL,         INDEX_ORI) = GetFVelOri(f_b, q_nb);
    F_.block<3, 3>(INDEX_VEL,  INDEX_ACCEL_BIAS) = B_.block<3, 3>(INDEX_VEL, 3) = C_nb;

    // b. set equation for orientation quaternion:
    F_.block<4, 4>(INDEX_ORI,         INDEX_ORI) = GetFOriOri(w_b);
    F_.block<4, 3>(INDEX_ORI,   INDEX_GYRO_BIAS) = B_.block<4, 3>(INDEX_ORI, 0) = GetFOriEps(q_nb);
}

/**
 * @brief  update covariance estimation
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::UpdateCovarianceEstimation(
    const IMUData &imu_data
) {
    // update process equation:
    SetProcessEquation(imu_data);

    // get discretized process equations:
    double T = imu_data.time - time_;

    // approximate to 1st order:
    MatrixF F = MatrixF::Identity() + T*F_;

    MatrixB B = T*B_;

    // perform Kalman prediction for covariance:
    P_ = F*P_*F.transpose() + B*Q_*B.transpose();
}

/**
 * @brief  get block matrix for observation by orientation quaternion
 * @param  m_n, measurement in navigation frame
 * @param  q_nb, orientation quaternion
 * @return block matrix Gq
 */
Eigen::Matrix<double, 3, 4> ExtendedKalmanFilter::GetGMOri(
    const Eigen::Vector3d &m_n,
    const Eigen::Quaterniond &q_nb
) {
    // get F:
    Eigen::Matrix<double, 4, 3> T_Gm;
    T_Gm << +q_nb.w(), +q_nb.z(), -q_nb.y(),
            +q_nb.x(), +q_nb.y(), +q_nb.z(),
            -q_nb.y(), +q_nb.x(), -q_nb.w(),
            -q_nb.z(), +q_nb.w(), +q_nb.x();
    Eigen::Vector4d G = 2 * T_Gm * m_n;

    // get Fvq:
    Eigen::Matrix<double, 3, 4> Gq;
    Gq << +G(0), +G(1), +G(2), +G(3),
          +G(3), -G(2), +G(1), -G(0),
          -G(2), -G(3), +G(0), +G(1);

    return Gq;
}

/**
 * @brief  correct state estimation using frontend pose
 * @param  T_nb, input frontend pose estimation
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPose(
    const Eigen::Matrix4d &T_nb
) {
    // parse measurement:
    YPose_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    Eigen::Quaterniond q_nb(T_nb.block<3, 3>(0,0));
    YPose_(3, 0) = q_nb.w();
    YPose_(4, 0) = q_nb.x();
    YPose_(5, 0) = q_nb.y();
    YPose_(6, 0) = q_nb.z();

    // build Kalman gain:
    MatrixRPose R = GPose_*P_*GPose_.transpose() + RPose_;
    MatrixKPose K = P_*GPose_.transpose()*R.inverse();

    // perform Kalman correct:
    P_ = (MatrixP::Identity() - K*GPose_)*P_;
    X_ = X_ + K*(YPose_ - GPose_*X_);

    // normalize quaternion:
    X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
}

/**
 * @brief  correct state estimation using GNSS position
 * @param  T_nb, input GNSS position
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosi(
    const Eigen::Matrix4d &T_nb
) {
    // parse measurement:
    YPosi_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);

    // build Kalman gain:
    MatrixRPosi R = GPosi_*P_*GPosi_.transpose() + RPosi_;
    MatrixKPosi K = P_*GPosi_.transpose()*R.inverse();

    // perform Kalman correct:
    P_ = (MatrixP::Identity() - K*GPosi_)*P_;
    X_ = X_ + K*(YPosi_ - GPosi_*X_);

    // normalize quaternion:
    X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
}

/**
 * @brief  correct state estimation using frontend pose & body velocity measurement
 * @param  T_nb, input frontend pose estimation
 * @param  v_b, input odo
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPoseVel(
    const Eigen::Matrix4d &T_nb,
    const Eigen::Vector3d &v_b
) {
    // parse measurement:
    YPoseVel_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);

    Eigen::Quaterniond q_nb_obs(T_nb.block<3, 3>(0,0));
    YPoseVel_(3, 0) = q_nb_obs.w();
    YPoseVel_(4, 0) = q_nb_obs.x();
    YPoseVel_(5, 0) = q_nb_obs.y();
    YPoseVel_(6, 0) = q_nb_obs.z();

    YPoseVel_.block<3, 1>(7, 0) = v_b;

    // iterative observation:
    for (size_t i = 0; i < 3; ++i) {
        // set observation equation:
        Eigen::Quaterniond q_nb_pred(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb_pred = q_nb_pred.toRotationMatrix();
        GPoseVel_.block<3, 3>( 7, INDEX_VEL ) = C_nb_pred.transpose();
        GPoseVel_.block<3, 4>( 7, INDEX_ORI ) = GetGMOri(X_.block<3, 1>(INDEX_VEL, 0), q_nb_pred);

        // build Kalman gain:
        MatrixRPoseVel R = GPoseVel_*P_*GPoseVel_.transpose() + RPoseVel_;
        MatrixKPoseVel K = P_*GPoseVel_.transpose()*R.inverse();
        VectorYPoseVel Y = VectorYPoseVel::Zero();
        Y.block<3, 1>(0, 0) = X_.block<3, 1>(INDEX_POS, 0);
        Y.block<4, 1>(3, 0) = X_.block<4, 1>(INDEX_ORI, 0);
        Y.block<3, 1>(7, 0) = C_nb_pred.transpose() * X_.block<3, 1>(INDEX_VEL, 0);

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPoseVel_)*P_;
        X_ = X_ + K*(YPoseVel_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation using GNSS position and odometer measurement
 * @param  T_nb, input GNSS position
 * @param  v_b, input odo
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosiVel(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b
) {
    // parse measurement:
    YPosiVel_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    YPosiVel_.block<3, 1>(3, 0) = v_b;

    // iterative observation:
    for (size_t i = 0; i < 1; ++i) {
        // set observation equation:
        Eigen::Quaterniond q_nb(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();
        GPosiVel_.block<3, 3>( 3, INDEX_VEL ) = C_nb.transpose();
        GPosiVel_.block<3, 4>( 3, INDEX_ORI ) = GetGMOri(X_.block<3, 1>(INDEX_VEL, 0), q_nb);

        // build Kalman gain:
        MatrixRPosiVel R = GPosiVel_*P_*GPosiVel_.transpose() + RPosiVel_;
        MatrixKPosiVel K = P_*GPosiVel_.transpose()*R.inverse();
        VectorYPosiVel Y = X_.block<6, 1>(0, 0);
        Y.block<3, 1>(3, 0) = C_nb.transpose() * Y.block<3, 1>(3, 0);

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPosiVel_)*P_;
        X_ = X_ + K*(YPosiVel_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation using GNSS position and magneto measurement
 * @param  T_nb, input GNSS position
 * @param  B_b, input magneto
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosiMag(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &B_b
) {
    // parse measurement:
    YPosiMag_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    YPosiMag_.block<3, 1>(3, 0) = B_b;

    // iterative observation:
    for (size_t i = 0; i < 1; ++i) {
        // set observation equation:
        const Eigen::Quaterniond q_nb(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();
        GPosiMag_.block<3, 4>( 3, INDEX_ORI ) = GetGMOri(b_, q_nb);

        // build Kalman gain:
        MatrixRPosiMag R = GPosiMag_*P_*GPosiMag_.transpose() + RPosiMag_;
        MatrixKPosiMag K = P_*GPosiMag_.transpose()*R.inverse();
        VectorYPosiMag Y = VectorYPosiMag::Zero();
        Y.block<3, 1>(0, 0) = X_.block<3, 1>( INDEX_POS, 0);
        Y.block<3, 1>(3, 0) = C_nb.transpose() * b_;

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPosiMag_)*P_;
        X_ = X_ + K*(YPosiMag_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation using GNSS position, odometer and magneto measurement
 * @param  T_nb, input GNSS position
 * @param  v_b, input odo
 * @param  B_b, input magneto
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimationPosiVelMag(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &B_b
) {
    // parse measurement:
    YPosiVelMag_.block<3, 1>(0, 0) = T_nb.block<3, 1>(0,3);
    YPosiVelMag_.block<3, 1>(3, 0) = v_b;
    YPosiVelMag_.block<3, 1>(6, 0) = B_b;

    // iterative observation:
    for (size_t i = 0; i < 1; ++i) {
        // set observation equation:
        const Eigen::Quaterniond q_nb(
            X_(INDEX_ORI + 0, 0),
            X_(INDEX_ORI + 1, 0),
            X_(INDEX_ORI + 2, 0),
            X_(INDEX_ORI + 3, 0)
        );
        Eigen::Matrix3d C_nb = q_nb.toRotationMatrix();
        GPosiVelMag_.block<3, 3>( 3, INDEX_VEL ) = C_nb.transpose();
        GPosiVelMag_.block<3, 4>( 3, INDEX_ORI ) = GetGMOri(X_.block<3, 1>(INDEX_VEL, 0), q_nb);
        GPosiVelMag_.block<3, 4>( 6, INDEX_ORI ) = GetGMOri(b_, q_nb);

        // build Kalman gain:
        MatrixRPosiVelMag R = GPosiVelMag_*P_*GPosiVelMag_.transpose() + RPosiVelMag_;
        MatrixKPosiVelMag K = P_*GPosiVelMag_.transpose()*R.inverse();
        VectorYPosiVelMag Y = VectorYPosiVelMag::Zero();
        Y.block<3, 1>(0, 0) = X_.block<3, 1>( INDEX_POS, 0);
        Y.block<3, 1>(3, 0) = C_nb.transpose() * X_.block<3, 1>(INDEX_VEL, 0);
        Y.block<3, 1>(6, 0) = C_nb.transpose() * b_;

        // perform Kalman correct:
        P_ = (MatrixP::Identity() - K*GPosiVelMag_)*P_;
        X_ = X_ + K*(YPosiVelMag_ - Y);

        // normalize quaternion:
        X_.block<4, 1>( INDEX_ORI, 0 ).normalize();
    }
}

/**
 * @brief  correct state estimation
 * @param  measurement_type, measurement type
 * @param  measurement, input measurement
 * @return void
 */
void ExtendedKalmanFilter::CorrectStateEstimation(
    const MeasurementType &measurement_type,
    const Measurement &measurement
) {
    switch ( measurement_type ) {
        case MeasurementType::POSE:
            CorrectStateEstimationPose(measurement.T_nb);
            break;
        case MeasurementType::POSE_VEL:
            CorrectStateEstimationPoseVel(measurement.T_nb, measurement.v_b);
            break;
        case MeasurementType::POSI:
            CorrectStateEstimationPosi(measurement.T_nb);
            break;
        case MeasurementType::POSI_VEL:
            CorrectStateEstimationPosiVel(measurement.T_nb, measurement.v_b);
            break;
        case MeasurementType::POSI_MAG:
            CorrectStateEstimationPosiMag(measurement.T_nb, measurement.B_b);
            break;
        case MeasurementType::POSI_VEL_MAG:
            CorrectStateEstimationPosiVelMag(measurement.T_nb, measurement.v_b, measurement.B_b);
            break;
        default:
            break;
    }

    // update bias:
    if ( IsCovStable(INDEX_GYRO_BIAS, 1.0e-6) )  gyro_bias_ = X_.block<3, 1>(INDEX_GYRO_BIAS, 0);
    if ( IsCovStable(INDEX_ACCEL_BIAS, 1.0e-6) ) accel_bias_ = X_.block<3, 1>(INDEX_ACCEL_BIAS, 0);
}

/**
 * @brief  is covariance stable
 * @param  INDEX_OFFSET, state index offset
 * @param  THRESH, covariance threshold, defaults to 1.0e-5
 * @return void
 */
bool ExtendedKalmanFilter::IsCovStable(
    const int INDEX_OFFSET,
    const double THRESH
) {
    if ( INDEX_ORI == INDEX_OFFSET ) {
        for (int i = 0; i < 4; ++i) {
            if ( P_(INDEX_OFFSET + i, INDEX_OFFSET + i) > THRESH ) {
                return false;
            }
        }
    } else {
        for (int i = 0; i < 3; ++i) {
            if ( P_(INDEX_OFFSET + i, INDEX_OFFSET + i) > THRESH ) {
                return false;
            }
        }
    }

    return true;
}

/**
 * @brief  reset filter state
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::ResetState(void) {
    // reset current state:
    X_ = VectorX::Zero();
}

/**
 * @brief  reset filter covariance
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::ResetCovariance(void) {
    P_ = MatrixP::Zero();

    P_.block<3, 3>(       INDEX_POS,        INDEX_POS) = COV.PRIOR.POSI*Eigen::Matrix3d::Identity();
    P_.block<3, 3>(       INDEX_VEL,        INDEX_VEL) = COV.PRIOR.VEL*Eigen::Matrix3d::Identity();
    // TODO: find a better way for quaternion orientation prior covariance assignment
    P_.block<4, 4>(       INDEX_ORI,        INDEX_ORI) = COV.PRIOR.ORI*Eigen::Matrix4d::Identity();
    P_.block<3, 3>( INDEX_GYRO_BIAS,  INDEX_GYRO_BIAS) = COV.PRIOR.EPSILON*Eigen::Matrix3d::Identity();
    P_.block<3, 3>(INDEX_ACCEL_BIAS, INDEX_ACCEL_BIAS) = COV.PRIOR.DELTA*Eigen::Matrix3d::Identity();
}

/**
 * @brief  get Q for pose measurement
 * @param  void
 * @return void
 */
void ExtendedKalmanFilter::GetQPose(Eigen::MatrixXd &Q) {
    // build observability matrix for position measurement:
    for (int i = 1; i < DIM_STATE; ++i) {
        QPose_.block<DIM_MEASUREMENT_POSE, DIM_STATE>(i*DIM_MEASUREMENT_POSE, 0) = (
            QPose_.block<DIM_MEASUREMENT_POSE, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSE, 0) * F_
        );
    }

    Q = QPose_;
}

/**
 * @brief  get Q for pose & body velocity measurement
 * @param  void
 * @return void
 */
void  ExtendedKalmanFilter::GetQPoseVel(Eigen::MatrixXd &Q) {
    // build observability matrix for position & velocity measurement:
    QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>(0, 0) = GPoseVel_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>(i*DIM_MEASUREMENT_POSE_VEL, 0) = (
            QPoseVel_.block<DIM_MEASUREMENT_POSE_VEL, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSE_VEL, 0) * F_
        );
    }

    Q = QPoseVel_;
}

/**
 * @brief  get Q for GNSS position measurement
 * @param  void
 * @return QPosi
 */
void ExtendedKalmanFilter::GetQPosi(Eigen::MatrixXd &Q) {
    // build observability matrix for position measurement:
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosi_.block<DIM_MEASUREMENT_POSI, DIM_STATE>(i*DIM_MEASUREMENT_POSI, 0) = (
            QPosi_.block<DIM_MEASUREMENT_POSI, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI, 0) * F_
        );
    }

    Q = QPosi_;
}

/**
 * @brief  get Q for GNSS position & body velocity measurement
 * @param  void
 * @return QPosiVel
 */
 void ExtendedKalmanFilter::GetQPosiVel(Eigen::MatrixXd &Q) {
    // build observability matrix for position & velocity measurement:
    QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>(0, 0) = GPosiVel_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>(i*DIM_MEASUREMENT_POSI_VEL, 0) = (
            QPosiVel_.block<DIM_MEASUREMENT_POSI_VEL, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI_VEL, 0) * F_
        );
    }

    Q = QPosiVel_;
}

/**
 * @brief  get Q for GNSS position & magneto measurement
 * @param  void
 * @return QPosiMag
 */
 void ExtendedKalmanFilter::GetQPosiMag(Eigen::MatrixXd &Q) {
    // build observability matrix for position measurement:
    QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>(0, 0) = GPosiMag_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>(i*DIM_MEASUREMENT_POSI_MAG, 0) = (
            QPosiMag_.block<DIM_MEASUREMENT_POSI_MAG, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI_MAG, 0) * F_
        );
    }

    Q = QPosiMag_;
}

/**
 * @brief  get Q for GNSS position, body velocity & magneto measurement
 * @param  void
 * @return QPosiVelMag
 */
 void ExtendedKalmanFilter::GetQPosiVelMag(Eigen::MatrixXd &Q) {
    // build observability matrix for position & velocity measurement:
    QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>(0, 0) = GPosiVelMag_;
    for (int i = 1; i < DIM_STATE; ++i) {
        QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>(i*DIM_MEASUREMENT_POSI_VEL_MAG, 0) = (
            QPosVelMag_.block<DIM_MEASUREMENT_POSI_VEL_MAG, DIM_STATE>((i - 1)*DIM_MEASUREMENT_POSI_VEL_MAG, 0) * F_
        );
    }
    Q = QPosVelMag_;
}

/**
 * @brief  update observability analysis
 * @param  measurement_type, measurement type
 * @return void
 */
void ExtendedKalmanFilter::UpdateObservabilityAnalysis(
    const double &time,
    const MeasurementType &measurement_type
) {
    // get Q:
    Eigen::MatrixXd Q;
    switch ( measurement_type ) {
        case MeasurementType::POSE:
            GetQPose(Q);
            break;
        case MeasurementType::POSE_VEL:
            GetQPoseVel(Q);
            break;
        case MeasurementType::POSI:
            GetQPosi(Q);
            break;
        case MeasurementType::POSI_VEL:
            GetQPosiVel(Q);
            break;
        case MeasurementType::POSI_MAG:
            GetQPosiMag(Q);
            break;
        case MeasurementType::POSI_VEL_MAG:
            GetQPosiVelMag(Q);
            break;
        default:
            break;
    }

    observability.time_.push_back(time);
    observability.Q_.push_back(Q);
}

/**
 * @brief  save observability analysis to persistent storage
 * @param  measurement_type, measurement type
 * @return void
 */
bool ExtendedKalmanFilter::SaveObservabilityAnalysis(
    const MeasurementType &measurement_type
) {
    // get fusion strategy:
    std::string type;
    switch ( measurement_type ) {
        case MeasurementType::POSE:
            type = std::string("pose");
            break;
        case MeasurementType::POSE_VEL:
            type = std::string("pose_velocity");
            break;
        case MeasurementType::POSI:
            type = std::string("position");
            break;
        case MeasurementType::POSI_VEL:
            type = std::string("position_velocity");
            break;
        case MeasurementType::POSI_MAG:
            type = std::string("position_magneto");
            break;
        case MeasurementType::POSI_VEL_MAG:
            type = std::string("position_velocity_magneto");
            break;
        default:
            return false;
            break;
    }

    // build Q_so:
    const int N = observability.Q_.at(0).rows();

    std::vector<std::vector<double>> q_data, q_so_data;

    Eigen::MatrixXd Qso(
        observability.Q_.size() * N,
        DIM_STATE
    );
    for (size_t i = 0; i < observability.Q_.size(); ++i) {
        const double &time = observability.time_.at(i);
        const Eigen::MatrixXd &Q = observability.Q_.at(i);

        Qso.block(i * N, 0, N, DIM_STATE) = Q;

        KalmanFilter::AnalyzeQ(DIM_STATE, time, Q, q_data);

        KalmanFilter::AnalyzeQ(DIM_STATE, time, Qso.block(0, 0, (i + 1)*N, DIM_STATE), q_so_data);
    }

    std::string q_data_csv = WORK_SPACE_PATH + "/slam_data/observability/" + type + ".csv";
    std::string q_so_data_csv = WORK_SPACE_PATH + "/slam_data/observability/" + type + "_som.csv";

    KalmanFilter::WriteAsCSV(DIM_STATE, q_data, q_data_csv);
    KalmanFilter::WriteAsCSV(DIM_STATE, q_so_data, q_so_data_csv);

    return true;
}

} // namespace lidar_localization


/**
 * @file /yocs_math_toolkit/include/yocs_math_toolkit/sophus_helpers.hpp
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef yocs_math_toolkit_SOPHUS_HELPERS_HPP_
#define yocs_math_toolkit_SOPHUS_HELPERS_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ecl/config/macros.hpp>
#include <ecl/converters.hpp>
#include <ecl/exceptions/standard_exception.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#if defined(ECL_CXX11_FOUND)
    #include <memory>
#endif
#include <sophus/se3.hpp>
#include <sophus/se2.hpp>
#include <sophus/so2.hpp>
#include <sophus/so3.hpp>
#include <string>

#include "formatters.hpp"

/*****************************************************************************
** Namespace
*****************************************************************************/

namespace Sophus {

/*****************************************************************************
 ** C++11 Api Only
 *****************************************************************************/

#if defined(ECL_CXX11_FOUND)
    typedef std::shared_ptr<SE3f> SE3fPtr;

    /// Converts a line drawn between two points on the z-plane into the transform of a sophus frame relative to the origin.
    Sophus::SE3fPtr points2DToSophusTransform(float from_x, float from_y, float to_x, float to_y);
#endif

/*****************************************************************************
** Interfaces
*****************************************************************************/

template<typename T>
std::ostream & operator << ( std::ostream & out, const SE3<T> & se3 )
{
//  typename SE3<T>::Tangent tanget_vector = SE3<T>::log( se3 );
//  out << tanget_vector.transpose();
  const Eigen::Matrix<T,3,1> & t = se3.translation();
  const Eigen::Quaternion<T> & q = se3.unit_quaternion();
  out << t.transpose() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();
  return out;
}

template<typename T>
std::ostream & operator << ( std::ostream & out, const SE2<T> & se2 )
{
  typename SE2<T>::Tangent tanget_vector = SE2<T>::log( se2 );
  out << tanget_vector.transpose();
  return out;
}

/**
 * @brief Convert a full Sophus pose into a 2 dimensional pose.
 *
 * The 2d pose is a typical mobile robot 2d pose with (x, y, heading) with
 * heading measured in radians.
 **/
Eigen::Vector3f toPose2D(const Sophus::SE3f& pose);
/**
 * @brief Convert a 2 dimensional pose to a full Sophus pose in 3d.
 *
 * The 2d pose is a typical mobile robot 2d pose with (x, y, heading) with
 * heading measured in radians.
 **/
Sophus::SE3f toPose3D(const Eigen::Vector3f& pose);

class PlanarRotation2Quaternion
{
public:
  PlanarRotation2Quaternion(){}
  Eigen::Quaternionf operator() ( float theta )
  {
    Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
    Eigen::Quaternionf q(R);
    q.normalize();
    return q;
//    Eigen::Quaternionf q;
//    // in this case x and y part is zero since we assumbed that rotationa round z axis on the xy-plane
//    q.vec() << 0, 0, sin(theta*0.5f);
//    q.w() = cos(theta*0.5f);
//    q.normalize();
//    return q;
  }

  void convert( float theta, Eigen::Quaternionf & q )
  {
    q.vec() << 0, 0, sin(theta*0.5f);
    q.w() = cos(theta*0.5f);
  }
};

} // namespace Sophus

/*****************************************************************************
** Converters
*****************************************************************************/

namespace ecl {

/**
 * @brief Converter class representing the Sophus::toPose3D method.
 */
template<>
class Converter<Sophus::SE3f, Eigen::Vector3f> {
public:
  Sophus::SE3f operator()(const Eigen::Vector3f& pose);
};

/**
 * @brief Converter class representing the Sophus::toPose2D method.
 */
template<>
class Converter<Eigen::Vector3f, Sophus::SE3f> {
public:
  Eigen::Vector3f operator()(const Sophus::SE3f& pose);
};

} // namespace ecl

#endif /* yocs_math_toolkit_SOPHUS_HELPERS_HPP_ */


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_GEODESIC_DISTANCE_HPP
#define BOOST_GRAPH_GEODESIC_DISTANCE_HPP

#include <boost/graph/detail/geodesic.hpp>
#include <boost/graph/exterior_property.hpp>

namespace boost
{
template <typename Graph,
          typename DistanceType,
          typename ResultType,
          typename Divides = std::divides<ResultType> >
struct mean_geodesic_measure
    : public geodesic_measure<Graph, DistanceType, ResultType>
{
    typedef geodesic_measure<Graph, DistanceType, ResultType> base_type;
    typedef typename base_type::distance_type distance_type;
    typedef typename base_type::result_type result_type;

    result_type operator ()(distance_type d, const Graph& g)
    {
        function_requires< VertexListGraphConcept<Graph> >();
        function_requires< NumericValueConcept<DistanceType> >();
        function_requires< NumericValueConcept<ResultType> >();
        function_requires< AdaptableBinaryFunctionConcept<Divides,ResultType,ResultType,ResultType> >();

        return (d == base_type::infinite_distance())
            ? base_type::infinite_result()
            : div(result_type(d), result_type(num_vertices(g) - 1));
    }
    Divides div;
};

template <typename Graph, typename DistanceMap>
inline mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, double>
measure_mean_geodesic(const Graph&, DistanceMap)
{
    return mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, double>();
}

template <typename T, typename Graph, typename DistanceMap>
inline mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, T>
measure_mean_geodesic(const Graph&, DistanceMap)
{
    return mean_geodesic_measure<Graph, typename property_traits<DistanceMap>::value_type, T>();
}

// This is a little different because it's expected that the result type
// should (must?) be the same as the distance type. There's a type of
// transitivity in this thinking... If the average of distances has type
// X then the average of x's should also be type X. Is there a case where this
// is not true?
//
// This type is a little under-genericized... It needs generic parameters
// for addition and division.
template <typename Graph, typename DistanceType>
struct mean_graph_distance_measure
    : public geodesic_measure<Graph, DistanceType, DistanceType>
{
    typedef geodesic_measure<Graph, DistanceType, DistanceType> base_type;
    typedef typename base_type::distance_type distance_type;
    typedef typename base_type::result_type result_type;

    inline result_type operator ()(distance_type d, const Graph& g)
    {
        function_requires< VertexListGraphConcept<Graph> >();
        function_requires< NumericValueConcept<DistanceType> >();

        if(d == base_type::infinite_distance()) {
            return base_type::infinite_result();
        }
        else {
            return d / result_type(num_vertices(g));
        }
    }
};

template <typename Graph, typename DistanceMap>
inline mean_graph_distance_measure<Graph, typename property_traits<DistanceMap>::value_type>
measure_graph_mean_geodesic(const Graph&, DistanceMap)
{
    typedef typename property_traits<DistanceMap>::value_type T;
    return mean_graph_distance_measure<Graph, T>();
}

template <typename Graph,
          typename DistanceMap,
          typename Measure,
          typename Combinator>
inline typename Measure::result_type
mean_geodesic(const Graph& g,
                DistanceMap dist,
                Measure measure,
                Combinator combine)
{
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::distance_type Distance;

    Distance n = detail::combine_distances(g, dist, combine, Distance(0));
    return measure(n, g);
}

template <typename Graph,
            typename DistanceMap,
            typename Measure>
inline typename Measure::result_type
mean_geodesic(const Graph& g, DistanceMap dist, Measure measure)
{
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::distance_type Distance;

    return mean_geodesic(g, dist, measure, std::plus<Distance>());
}

template <typename Graph, typename DistanceMap>
inline double
mean_geodesic(const Graph& g, DistanceMap dist)
{ return mean_geodesic(g, dist, measure_mean_geodesic(g, dist)); }

template <typename T, typename Graph, typename DistanceMap>
inline T
mean_geodesic(const Graph& g, DistanceMap dist)
{ return mean_geodesic(g, dist, measure_mean_geodesic<T>(g, dist)); }

template <typename Graph,
            typename DistanceMatrixMap,
            typename GeodesicMap,
            typename Measure>
inline typename property_traits<GeodesicMap>::value_type
all_mean_geodesics(const Graph& g,
                    DistanceMatrixMap dist,
                    GeodesicMap geo,
                    Measure measure)
{
    function_requires< VertexListGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    function_requires< ReadablePropertyMapConcept<DistanceMatrixMap,Vertex> >();
    typedef typename property_traits<DistanceMatrixMap>::value_type DistanceMap;
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::result_type Result;
    function_requires< WritablePropertyMapConcept<GeodesicMap,Vertex> >();
    function_requires< NumericValueConcept<Result> >();

    // NOTE: We could compute the mean geodesic here by performing additional
    // computations (i.e., adding and dividing). However, I don't really feel
    // like fully genericizing the entire operation yet so I'm not going to.

    Result inf = numeric_values<Result>::infinity();
    Result sum = numeric_values<Result>::zero();
    VertexIterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        DistanceMap dm = get(dist, *i);
        Result r = mean_geodesic(g, dm, measure);
        put(geo, *i, r);

        // compute the sum along with geodesics
        if(r == inf) {
            sum = inf;
        }
        else if(sum != inf) {
            sum += r;
        }
    }

    // return the average of averages.
    return sum / Result(num_vertices(g));
}

template <typename Graph, typename DistanceMatrixMap, typename GeodesicMap>
inline typename property_traits<GeodesicMap>::value_type
all_mean_geodesics(const Graph& g, DistanceMatrixMap dist, GeodesicMap geo)
{
    function_requires< GraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    function_requires< ReadablePropertyMapConcept<DistanceMatrixMap,Vertex> >();
    typedef typename property_traits<DistanceMatrixMap>::value_type DistanceMap;
    function_requires< WritablePropertyMapConcept<GeodesicMap,Vertex> >();
    typedef typename property_traits<GeodesicMap>::value_type Result;

    return all_mean_geodesics(g, dist, geo, measure_mean_geodesic<Result>(g, DistanceMap()));
}

template <typename Graph, typename GeodesicMap, typename Measure>
inline typename Measure::result_type
small_world_distance(const Graph& g, GeodesicMap geo, Measure measure)
{
    function_requires< DistanceMeasureConcept<Measure,Graph> >();
    typedef typename Measure::result_type Result;

    Result sum = detail::combine_distances(g, geo, std::plus<Result>(), Result(0));
    return measure(sum, g);
}

template <typename Graph, typename GeodesicMap>
inline typename property_traits<GeodesicMap>::value_type
small_world_distance(const Graph& g, GeodesicMap geo)
{ return small_world_distance(g, geo, measure_graph_mean_geodesic(g, geo)); }

}

#endif