kalmanfilter.h

#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

class KalmanFilter
{
public:
    KalmanFilter();
    KalmanFilter(int state_vector_size, int measurement_vector_size);

    cv::Mat F; //Prediction matrix
    cv::Mat P; //Prediction covariance
    cv::Mat H; //Measurement matrix
    cv::Mat R; //Error Matrix (measurements - related to covariance)
    cv::Mat K; //Kalman gain
    cv::Mat a; //State
    cv::Mat a_p; //Predicted state
    cv::Mat y;   //Difference: y = H a

    int states; //Number of states
    int measurements; //Number of measurements
    bool initial;
    bool built;

    //Build every matrix and vector
    void build(int state_vector_size, int measurement_vector_size);

    void updateP(cv::Mat newP);
    void updateH(cv::Mat newH);


    //Makes all Kalman process at once
    void apply(cv::Mat measurement, cv::Mat measurement_error,
               cv::Mat Perror);

    //Initialize everything for the first time
    void init(cv::Mat measurement, cv::Mat measurement_error);

    //Applies F to current state a;
    void predictState(cv::Mat Perror); //Step 1

    //Input is a measurement vector
    void getResidual(cv::Mat mesurement); //Step2

    //Input is a covariance matrix of the measurement
    void getKalmanGain(cv::Mat measurement_error);

    void updateState();

    cv::Mat identity(int N);

};

#endif // KALMANFILTER_H