kalmanfilter.h

Go to the documentation of this file.

#ifndef TAG_FILTERKALMANFILTER_H
#define TAG_FILTERKALMANFILTER_H

#include <TooN/TooN.h>
#include <TooN/helpers.h>
#include <TooN/Cholesky.h>

namespace tag {

template<class State, class Model>
class KalmanFilter{
public:

    typedef State state_type;
    typedef Model model_type;

    KalmanFilter(){
        identity = TooN::Identity;
    }

    void predict(double dt){
        //state.covariance = TooN::transformCovariance(model.getJacobian( state, dt ), state.covariance) + model.getNoiseCovariance( dt );
        const TooN::Matrix<State::STATE_DIMENSION> & A = model.getJacobian( state, dt );
        state.covariance = A * state.covariance * A.T() + model.getNoiseCovariance( dt );
        TooN::Symmetrize(state.covariance);
        model.updateState( state, dt );
    }

    template<class Measurement> void filter(Measurement & m){
        const TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> & H = m.getMeasurementJacobian( state );
        const TooN::Matrix<Measurement::M_DIMENSION> & R = m.getMeasurementCovariance( state );
        const TooN::Vector<Measurement::M_DIMENSION> & innovation = m.getInnovation( state );
        const TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> P12 = state.covariance * H.T();
        TooN::Cholesky<Measurement::M_DIMENSION> denom(H * P12 + R);
        state.covariance = state.covariance - P12 * denom.backsub(P12.T());
        // TooN::Symmetrize(state.covariance);  // not necessary, above seems to be good enough
        const TooN::Vector<State::STATE_DIMENSION> stateInnovation = P12 * denom.backsub(innovation);
        model.updateFromMeasurement( state, stateInnovation );
    }

    TooN::Matrix<State::STATE_DIMENSION> identity;
    State state;
    Model model;
};

} // namespace tag

#endif