navmath.h

#ifndef INCLUDE_NAVLIB_NAVMATH
#define INCLUDE_NAVLIB_NAVMATH

#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/function.hpp>

namespace ublas = boost::numeric::ublas;

namespace navlib
{

template <typename T>
bool invert (const ublas::matrix<T>& M, ublas::matrix<T>& InvM) 
{
        // make sure the matrices are square and the same size
        assert(M.size1() == M.size2());
        assert(M.size1() == InvM.size1());
        assert(M.size2() == InvM.size2());
        
        // create a working copy of the input
        ublas::matrix<T> A(M);
        
        // create a permutation matrix for the LU-factorization
        ublas::permutation_matrix<std::size_t> pivots(A.size1());

        // perform LU-factorization
        int res = lu_factorize(A, pivots);
        if( res != 0 ) return false;
        
        // if the determinant is greater than some value lu_substitute will crash
        double det = 1.0;
        for (std::size_t k=0; k < pivots.size(); k++)
        {
                if (pivots(k) != k)
                        det *= -1.0;
                det *= A(k, k);
        }
        if (det > 1.0e20) return false;

        // create identity matrix of "inverse"
        InvM.assign(ublas::identity_matrix<T>(A.size1()));

        // backsubstitute to get the inverse
        try
        {
                lu_substitute(A, pivots, InvM);
        }
        catch (ublas::internal_logic)
        {
                std::cerr << "internal logic error" << std::endl;
        }
        return true;
}

template <typename T>
ublas::matrix<T> jacobian(boost::function< ublas::vector<T> (ublas::vector<T>) > f, ublas::vector<T> x, T tol)
{
        ublas::matrix<T> J = ublas::zero_matrix<T>(f(x).size(), x.size());
        for (unsigned int k=0; k<x.size(); k++)
        {
                ublas::vector<T> dx = ublas::zero_vector<T>(x.size());
                dx[k] = static_cast<T>(0.5)*tol;
                ublas::column(J,k) = (f(x+dx) - f(x-dx))/tol;
        }
        return J;
}

template <typename T = double>
class ExtendedKalmanFilter
{
public:
        typedef boost::function< ublas::vector<T> (ublas::vector<T>) > PredictFun;
        typedef boost::function< ublas::vector<T> (ublas::vector<T>) > ObserveFun;
        
        ExtendedKalmanFilter(ublas::vector<T> x0, T kP=1.0e-9)
        {
                _x = x0;
                _P = kP*ublas::identity_matrix<T>(x0.size());
        }

        ExtendedKalmanFilter(ublas::vector<T> x0, ublas::matrix<T> P0)
        {
                assert (x0.size() == P0.size1() && x0.size() == P0.size2());
                _x = x0;
                _P = P0;
        }

        void predict(PredictFun f, ublas::matrix<T> Q, T tol=1.0e-9)
        {
                ublas::matrix<T> F = jacobian<T>(f, _x, tol);
                _x = f(_x);
                _P = ublas::prod(ublas::matrix<T>(ublas::prod(F, _P)), 
                         ublas::trans(F)) + Q;
        }

        void correct(ObserveFun h, ublas::vector<T> z, ublas::matrix<T> R, 
                T tol=1.0e-9)
        {
                ublas::matrix<T> H, S, K;
                ublas::vector<T> y = z - h(_x);
                H = jacobian<T>(h, _x, tol);
                S = ublas::prod(ublas::matrix<T>(ublas::prod(H, _P)), ublas::trans(H))
                    + R;
                ublas::matrix<T> SInv(S.size1(), S.size2());
                invert<T>(S, SInv);
                K = ublas::prod(ublas::matrix<T>(ublas::prod(_P, ublas::trans(H))), 
                    SInv);
                _x = _x + ublas::prod(K, y);
                _P = ublas::prod((ublas::identity_matrix<double>(K.size1()) - 
                        ublas::matrix<T>(ublas::prod(K, H))), _P);
        }
        
        ublas::vector<T> getState() const
        {
                return _x;
        }

private:
        ublas::vector<T> _x; 
        ublas::matrix<T> _P; 
};

// TODO: Rework the Levenberg-Marquardt implementation from tpbvp.cpp into a nice library function

// Runga-Kutta 4th order integration step
template<class T>
ublas::vector<T> rk4step(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn, const T& dt, const T& t, ublas::vector<T> x)
{
        const static T one_half = static_cast<T>(0.5);
        const static T one_sixth = static_cast<T>(1.0/6.0);
        ublas::vector<T> k1 = dt*dxdtFxn(t, x);
    ublas::vector<T> k2 = dt*dxdtFxn(t+one_half*dt, x + one_half*k1);
    ublas::vector<T> k3 = dt*dxdtFxn(t+one_half*dt, x + one_half*k2);
    ublas::vector<T> k4 = dt*dxdtFxn(t+dt, x + k3);
    return x + one_sixth*(k1+2.0*k2+2.0*k3+k4);
}

// Runga-Kutta 4th order integrator
template<class T>
ublas::vector<T> rk4(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn,
        const T& t0, const T& tf, ublas::vector<T> x0, unsigned int numIter)
{
        ublas::vector<T> x = x0;
        double dt = (tf-t0)/numIter;
        for (unsigned int k=0; k<=numIter; k++)
        {
                T t = static_cast<T>(k)*dt;
                x = rk4step(dxdtFxn, dt, t, x);
        }
        return x;
}

// Runga-Kutta 4th order integration step
template<class T>
ublas::vector<T> rkf45step(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn, T& dt, T& t, ublas::vector<T> x, double err)
{
        const static T a21 = static_cast<T>(1.0/4.0);
        const static T a31 = static_cast<T>(3.0/32.0);
        const static T a32 = static_cast<T>(9.0/32.0);
        const static T a41 = static_cast<T>(1932.0/2197.0);
        const static T a42 = static_cast<T>(-7200.0/2197.0);
        const static T a43 = static_cast<T>(7296.0/2197.0);
        const static T a51 = static_cast<T>(493.0/216.0);
        const static T a52 = static_cast<T>(-8.0);
        const static T a53 = static_cast<T>(3680.0/513.0);
        const static T a54 = static_cast<T>(-845.0/4104.0);
        const static T a61 = static_cast<T>(-8.0/27.0);
        const static T a62 = static_cast<T>(2.0);
        const static T a63 = static_cast<T>(-3544.0/2565.0);
        const static T a64 = static_cast<T>(1859.0/4104.0);
        const static T a65 = static_cast<T>(-11.0/40.0);

        const static T b41 = static_cast<T>(16.0/135.0);
        const static T b42 = static_cast<T>(0.0);
        const static T b43 = static_cast<T>(6656.0/12825.0);
        const static T b44 = static_cast<T>(28561.0/56430.0);
        const static T b45 = static_cast<T>(-9.0/50.0);
        const static T b46 = static_cast<T>(2.0/55.0);

        const static T b51 = static_cast<T>(25.0/216.0);
        const static T b52 = static_cast<T>(0.0);
        const static T b53 = static_cast<T>(1408.0/2565.0);
        const static T b54 = static_cast<T>(2197.0/4104.0);
        const static T b55 = static_cast<T>(-1.0/5.0);
        const static T b56 = static_cast<T>(0.0);

        const static T c2 = static_cast<T>(1.0/4.0);
        const static T c3 = static_cast<T>(3.0/8.0);
        const static T c4 = static_cast<T>(12.0/13.0);
        const static T c5 = static_cast<T>(1.0);
        const static T c6 = static_cast<T>(1.0/2.0);

        // k1=tstep*f(t,y)
        ublas::vector<T> k1 = dt*dxdtFxn(t, x);

        // k2=tstep*f(t+.25.tstep,y+.25*k1)
        ublas::vector<T> k2 = dt*dxdtFxn(t+c2*dt, x + a21*k1);

        // k3=tstep*f(t+3tstep/8, y+(3.k1+9.k2)/32) => (tt,y+3/8*(k1/4+3k2/4))
        ublas::vector<T> k3 = dt*dxdtFxn(t+c3*dt, x + a31*k1 + a32*k2);

        // k4=tstep*f(t+12tstep/13, y+(1932.k1-7200.k2+7296*k3)/2197)
        ublas::vector<T> k4 = dt*dxdtFxn(t+c4*dt, x + a41*k1 + a42*k2 + a43*k3);

        // k5=tstep*f(t+tstep, y+(439.k1/216-8.k2+3680.k3/513-845*k4/4104))
        ublas::vector<T> k5 = dt*dxdtFxn(t+c5*dt, x + a51*k1 + a52*k2 + a53*k3 + a54*k4);

        // k6=tstep*f(t+tstep/2, y+(-8.k1/27+2.k2-3544.k3/2565+1859.k4/4104-11.k5/40))
        ublas::vector<T> k6 = dt*dxdtFxn(t+c6*dt, x + a61*k1 + a62*k2 + a63*k3 + a64*k4 + a65*k5);

        // 4th order sol is y4th=y+25/216*k1+1408/2565.k3+2197/4104*k4-k5/5
        ublas::vector<T> x4 = x + b41*k1 + b42*k2 + b43*k3 + b44*k4 + b45*k5 + b46*k6;

        // 5th order sol is y5th=y+16/135*k1+6656/12825.k3+28561/56430*k4-9k5/50+2/55k6
        ublas::vector<T> x5 = x + b51*k1 + b52*k2 + b53*k3 + b54*k4 + b55*k5 + b56*k6;
        
        // and opt step size is s.h where err is the desired accuracy (dimension cancels with y5,y4)
        // s = {err.h/(2*(y5th-y4th)))}^0.25
        double diff = ublas::inner_prod(x5-x4, x5-x4);
        if (diff < err)
        {
                t += dt;
                x = x5;
        }
        dt = pow(err/(2.0*diff), 0.25)*dt;
        return x;
}

// Runga-Kutta-Fhelberg integrator
template<class T>
ublas::vector<T> rkf45(boost::function< ublas::vector<T> (T, ublas::vector<T>) > dxdtFxn, T t0, T tf, ublas::vector<T> x0, double err)
{
        ublas::vector<T> x = x0;
        double dt = 0.1*tf;
        double t = t0;
        while(t < tf)
        {
                dt = minimum(dt, tf-t);
                x = rkf45step(dxdtFxn, dt, t, x, err);
        }
        return x;
}


template <typename T>
bool lmStep(boost::function< ublas::vector<T> 
        (ublas::vector<T>) > f, ublas::vector<T>& p, ublas::vector<T> z, 
        T& gain, T dgain=2.0, T tol=1.0e-9)
{
        // Find the derivative of the function /w respect to a change in parameters
        ublas::matrix<T> J = jacobian<T>(f, p, tol);
        
        // Find the error due to the current parameters
    ublas::vector<T> e1v = z - f(p);
        T e1 = ublas::inner_prod(e1v, e1v);

        // If the matrix inversion can't be performed return false
        ublas::matrix<T> pInvJ(p.size(), p.size());
        if (!(invert<T>(ublas::prod(ublas::trans(J), J) + 
                gain*ublas::identity_matrix<T>(p.size()), pInvJ)))
                return false;

        // Get the next (hopefully better) parameter set
        ublas::vector<T> dp = ublas::prod(ublas::prod(pInvJ, trans(J)), e1v);
        ublas::vector<T> e2v = z - f(p+dp);
        T e2 = ublas::inner_prod(e2v, e2v);
        if (e2 < e1)
        {
        p += dp;
        gain /= dgain;
        }
    else
        {
        gain *= dgain;
        }
        return true;
}

/*// k-dimensional tree data structure, not tested yet...
typedef boost::function<bool (const &T, const &T, const unsigned int& axis)> KdPredFun;
typedef boost::function<double (const &T, const &T)> KdDistFun;
template<class T, unsigned int K>
class KdTree
{
public:
        KdTree(std::vector<T> points, KdPredFun cmp, unsigned int depth=0) :
          _leftChild(NULL), _rightChild(NULL)
        {
                // Select axis based on depth so that axis cycles through all values
                unsigned int axis = depth % K;

                // Sort point list to select median
                std::sort<T>(points.begin(), points.end(), boost::bind(cmp, _1, _2, axis));
                std::vector<T>::iterator median = points.begin()+points.size()/2;
                
                // Create node and construct subtrees
                _point = *median;
                if (median != points.begin())
                {
                        std::vector<T> lhs(points.begin(), median);
                        _leftChild = new KdTree(lhs, cmp, depth+1);
                }
                if (median != points.end())
                {
                        std::vector<T> rhs(median, points.end());
                        _leftChild = new KdTree(rhs, cmp, depth+1);
                }
        }
        
        ~KdTree()
        {
                if (_leftChild != NULL)
                        delete _leftChild;
                if (_rightChild != NULL)
                        delete _rightChild;
        }
        
        T getPoint()
        {
                return _point;
        }

        T getNearestNeighbor(const T& point, KdDistFun dist, unsigned int depth=0, 
                minDist = )
        {
                T result;
                minDist = std::numeric_limits<double>::infinity();
                if (dist(point, _point) < minDist)
                {
                        result = _point;
                }
                if (dist(point, _rightChild->getPoint() < minDist)
                {
                        
                }
        }

private:
        T _point;
        KdTree* _leftChild;
        KdTree* _rightChild;
};*/

} // namespace navlib

#endif

---