KalmanLinearAlgebra.h

Go to the documentation of this file.

#ifndef KALMANLINEARALGEBRA_H
#define KALMANLINEARALGEBRA_H

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

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