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/LU.h>

namespace tag {

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

    typedef State state_type;
    typedef Model model_type;

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

    void predict(double dt){
        model.updateState( state, dt );
        state.covariance = TooN::transformCovariance(model.getJacobian( state, dt ), state.covariance) + model.getNoiseCovariance( dt );
        TooN::Symmetrize(state.covariance);
    }

    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 );
        TooN::Matrix<Measurement::M_DIMENSION> I = TooN::transformCovariance(H, state.covariance) + R;
        TooN::LU<Measurement::M_DIMENSION> lu(I);
        TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> K = state.covariance * H.T() * lu.get_inverse();
        const TooN::Vector<Measurement::M_DIMENSION> & innovation = m.getInnovation( state );
        TooN::Vector<State::STATE_DIMENSION> stateInnovation = K * innovation;
        model.updateFromMeasurement( state, stateInnovation );
        state.covariance = (identity - K * H) * state.covariance;
        TooN::Symmetrize( state.covariance );
    }

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

} // namespace tag

#endif

---