KalmanLinearAlgebra.h

Go to the documentation of this file.

#ifndef KALMANLINEARALGEBRA_H
#define KALMANLINEARALGEBRA_H

#include <cmath>
#include "boost/serialization/array_wrapper.hpp"  // workaround for deficiency in boost 1.64
#include "boost/numeric/ublas/vector.hpp"
#include "boost/numeric/ublas/matrix.hpp"
#include "boost/numeric/ublas/symmetric.hpp"
#include "boost/numeric/ublas/lu.hpp"

namespace trkf {

  namespace ublas = boost::numeric::ublas;

  template<int N>
  struct KVector
  {
    typedef ublas::vector<double, ublas::bounded_array<double, N> > type;
  };

  template<int N>
  struct KSymMatrix
  {
    typedef ublas::symmetric_matrix<double, ublas::lower, ublas::row_major, ublas::bounded_array<double, N*(N+1)/2> > type;
  };

  template<int N, int M>
  struct KMatrix
  {
    typedef ublas::matrix<double, ublas::row_major, ublas::bounded_array<double, N*M> > type;
  };

  template<int N>
  struct KHMatrix
  {
    typedef typename KMatrix<N,5>::type type;
  };

  template<int N>
  struct KGMatrix
  {
    typedef typename KMatrix<5,N>::type type;
  };

  typedef typename KVector<5>::type TrackVector;

  typedef typename KSymMatrix<5>::type TrackError;

  typedef typename KMatrix<5,5>::type TrackMatrix;

  template <class T, class TRI, class L, class A>
  bool syminvert(ublas::symmetric_matrix<T, TRI, L, A>& m)
  {
    typedef typename ublas::symmetric_matrix<T, TRI, L, A>::size_type size_type;
    typedef typename ublas::symmetric_matrix<T, TRI, L, A>::value_type value_type;

    // In situ Cholesky decomposition m = LDL^T.
    // D is diagonal matrix.
    // L is lower triangular with ones on the diagonal (ones not stored).

    for(size_type i = 0; i < m.size1(); ++i) {
      for(size_type j = 0; j <= i; ++j) {

        value_type ele = m(i,j);

        for(size_type k = 0; k < j; ++k)
          ele -= m(k,k) * m(i,k) * m(j,k);

        // Diagonal elements (can't have zeroes).

        if(i == j) {
          if(ele == 0.)
            return false;
        }

        // Off-diagonal elements.

        else
          ele = ele / m(j,j);

        // Replace element.

        m(i,j) = ele;
      }
    }

    // In situ inversion of D by simple division.
    // In situ inversion of L by back-substitution.

    for(size_type i = 0; i < m.size1(); ++i) {
      for(size_type j = 0; j <= i; ++j) {

        value_type ele = m(i,j);

        // Diagonal elements.
        
        if(i == j)
          m(i,i) = 1./ele;

        // Off diagonal elements.

        else {
          value_type sum = -ele;
          for(size_type k = j+1; k < i; ++k)
            sum -= m(i,k) * m(k,j);
          m(i,j) = sum;
        }
      }
    }

    // Recompose the inverse matrix in situ by matrix multiplication m = L^T DL.

    for(size_type i = 0; i < m.size1(); ++i) {
      for(size_type j = 0; j <= i; ++j) {

        value_type sum = m(i,i);
        if(i != j)
          sum *= m(i,j);

        for(size_type k = i+1; k < m.size1(); ++k)
          sum += m(k,k) * m(k,i) * m(k,j);

        m(i,j) = sum;
      }
    }

    // Done (success).

    return true;
  }

  template <class T, class L, class A>
  bool invert(ublas::matrix<T, L, A>& m)
  {
    // Make sure matrix is square.

    if(m.size1() != m.size2())
      return false;

    // Create permutation matrix for pivoting.

    ublas::permutation_matrix<std::size_t> pm(m.size1());

    // Make temp copy of input matrix.

    ublas::matrix<T, L, A> mcopy(m);

    // Do LU factorization with partial pivoting.
    // This step will fail if matrix is singular.

    int res = lu_factorize(mcopy, pm);
    if( res != 0 )
      return false;

    // Set original matrix to the identity matrix.

    m.assign(ublas::identity_matrix<T>(m.size1()));

    // Do backsubstitution.

    lu_substitute(mcopy, pm, m);

    // Done (success).

    return true;
  }

  template <class M>
  typename M::value_type trace(const M& m)
  {
    typename M::size_type n = std::min(m.size1(), m.size2());
    typename M::value_type result = 0.;

    for(typename M::size_type i = 0; i < n; ++i)
      result += m(i,i);

    return result;
  }
}

#endif