#ifndef __KALMAN__FILTER__H
#define __KALMAN__FILTER__H

#include <tf/transform_datatypes.h>
#include <ctime>
#include <vector>
#include <Eigen/Dense>

class KalmanFilter
{
private:

    typedef struct POSEINFO{
        tf::Pose pose;
        double theta;
        double error;
    } poseInfo;
    tf::Pose                    m_last_pose;
    std::vector<poseInfo>       poses_vec;
    tf::Vector3                 velocity;
    clock_t                     m_last_time;
    // Discrete time step
    double dt_;
    // Matrices for computation
    Eigen::MatrixXd A_[3], C_[3], Q_[3], R_[3], P_[3], K_[3], I_[3], x_hat[3], x_hat_new[3];
    // System dimensions
    void posesCombine();

public:
    int m_, n_;
    /**
     * Create a Kalman filter with the specified matrices.
     *   A - System dynamics matrix 转移
     *   C - Output matrix 筛选
     *   Q - Process noise covariance 预测conv
     *   R - Measurement noise covariance 测量conv
     *   P - Estimate error covariance 预测
     */
    void init(
        const Eigen::MatrixXd &A,
        const Eigen::MatrixXd &C,
        const Eigen::MatrixXd &Q,
        const Eigen::MatrixXd &R,
        const Eigen::MatrixXd &P);
    KalmanFilter();
    ~KalmanFilter();
    void addData(tf::Pose pose1,double error1);
    tf::Pose filter();
};

#endif