kalman.cpp

This collection of C++ templates wraps the FORTRAN or C interfaces for LAPACK so that they integrate

#include <boost/numeric/ublas/matrix.hpp>
#include "cholesky.hpp"
#include "lufactor.hpp"
#include "matrix_types.hpp"
#include "kalman.hpp"

using namespace ulapack;

// Cholesky factorised form of the Kalman update step
void kalman_update(Vector &x, Matrix &P, 
	const Vector &innov, const Matrix &R, const Matrix &H)
{
    Matrix PHt = prod(P, trans(H));
    Matrix S = prod(H, PHt) + R; // innovation covariance
    Matrix Sci = inv(chol(S));  
    Matrix Wc = prod(PHt, Sci);
    Matrix W = prod(Wc, trans(Sci)); // Kalman gain

    noalias(x) += prod(W, innov);
	noalias(P) -= prod(Wc, trans(Wc)); 
}

// TODO: want inverse triangular: Sci = inv_tri(chol(S));
// TODO: compare with in-place operations