KalmanFilter.h

Go to the documentation of this file.

#ifndef LAR_KALMAN_FILTER_H
#define LAR_KALMAN_FILTER_H 1

#include "Eigen/Dense"

namespace lar_content
{

class KalmanFilter3D
{
public:
    KalmanFilter3D(double dt, double processVariance, double measurementVariance, const Eigen::VectorXd &x);

    ~KalmanFilter3D();

    void Predict();

    void Update(const Eigen::VectorXd &z);

    const Eigen::VectorXd &GetState() const;

    const Eigen::VectorXd GetPosition() const;

    const Eigen::VectorXd GetDirection() const;

    const Eigen::VectorXd &GetTemporaryState() const;

private:
    double m_dt;             
    Eigen::VectorXd m_x;     
    Eigen::MatrixXd m_P;     
    Eigen::MatrixXd m_F;     
    Eigen::MatrixXd m_H;     
    Eigen::MatrixXd m_R;     
    Eigen::MatrixXd m_Q;     
    Eigen::VectorXd m_xTemp; 
    Eigen::MatrixXd m_PTemp; 
};

class KalmanFilter2D
{
public:
    KalmanFilter2D(double dt, double processVariance, double measurementVariance, const Eigen::VectorXd &x);

    ~KalmanFilter2D();

    void Predict();

    void Update(const Eigen::VectorXd &z);

    const Eigen::VectorXd &GetState() const;

    const Eigen::VectorXd GetPosition() const;

    const Eigen::VectorXd GetDirection() const;

    const Eigen::VectorXd &GetTemporaryState() const;

private:
    double m_dt;             
    Eigen::VectorXd m_x;     
    Eigen::MatrixXd m_P;     
    Eigen::MatrixXd m_F;     
    Eigen::MatrixXd m_H;     
    Eigen::MatrixXd m_R;     
    Eigen::MatrixXd m_Q;     
    Eigen::VectorXd m_xTemp; 
    Eigen::MatrixXd m_PTemp; 
};

} // namespace lar_content

#endif // #ifndef LAR_KALMAN_FILTER_H