卡尔曼滤波C++代码

卡尔曼滤波预测和更新两部分，卡尔曼滤波主要实现预测过程个更新过程；因此在代码实现的时候需要完成四部分

1. 变量定义
   2）变量形状设置(由构造函数实现)
   3） 初始化，送入初始的状态量和协方差
   4） 预测过程
   使用状态转化方程完成状态量的先验估计，然后根据初始的协方差更新先验估计的协方差；
   5）更新过程
   首先需要从系统中得到当前时刻的观测量(该值是直接从系统中得到的，不需要再代码计算，因为是使用卡尔曼增益来平衡真实观察量与先验估计值的比例）；这里需要观测矩阵，和观测噪声的协方差；

#define _MYKALMAN_H
#define _MYKALMAN_H
#include
#include 

class KalmanFilter
{
private:
    int stateSize; //state variable's dimenssion
    int measSize; //measurement variable's dimession
    int uSize; //control variables's dimenssion
    Eigen::VectorXd x;  //状态量
    Eigen::VectorXd z;  //观测量
    Eigen::MatrixXd A;  //状态转移矩阵
    Eigen::MatrixXd B;  //控制矩阵
    Eigen::VectorXd u;  //输入矩阵
    Eigen::MatrixXd P;  //先验估计协方差
    Eigen::MatrixXd H;  //观测矩阵
    Eigen::MatrixXd R;  //测量噪声协方差
    Eigen::MatrixXd Q;  //过程噪声协方差
public:
    KalmanFilter(int stateSize_, int measSize_, int uSize_);
    ~KalmanFilter() {}
    void init(Eigen::VectorXd& x_, Eigen::MatrixXd& P_, Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_);
    Eigen::VectorXd predict(Eigen::MatrixXd& A_);
    Eigen::VectorXd predict(Eigen::MatrixXd& A_, Eigen::MatrixXd& B_, Eigen::VectorXd& u_);
    void update(Eigen::MatrixXd& H_, Eigen::VectorXd z_meas);
};

KalmanFilter::KalmanFilter(int stateSize_ = 0, int measSize_ = 0, int uSize_ = 0) :stateSize(stateSize_), measSize(measSize_), uSize(uSize_)
// sateSize状态量个数
// uSize输入的维度
// 
{
    if (stateSize == 0 || measSize == 0)
    {
        std::cerr << "Error, State size and measurement size must bigger than 0\n";
    }

    x.resize(stateSize);
    x.setZero();

    A.resize(stateSize, stateSize);
    A.setIdentity();

    u.resize(uSize);
    u.transpose();
    u.setZero();

    B.resize(stateSize, uSize);
    B.setZero();

    P.resize(stateSize, stateSize);
    P.setIdentity();

    H.resize(measSize, stateSize);
    H.setZero();

    z.resize(measSize);
    z.setZero();

    Q.resize(stateSize, stateSize);
    Q.setZero();

    R.resize(measSize, measSize);
    R.setZero();
}

//初始化，卡尔曼滤波初始化只需要初始化初始状态，初始协方差矩阵，初始过程噪声协方差，初始化观测噪声协方差
void KalmanFilter::init(Eigen::VectorXd& x_, Eigen::MatrixXd& P_, Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_)
{
    x = x_;
    P = P_;
    R = R_;
    Q = Q_;
}

//有输入矩阵和控制矩阵的卡尔曼滤波预测过程
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_, Eigen::MatrixXd& B_, Eigen::VectorXd& u_)
{
    A = A_;
    B = B_;
    u = u_;
    x = A * x + B * u;//根据时刻t-1的状态由状态转换举着预测时刻t的先验估计值，
                      //因为当前是使用系统的状态转移矩阵来进行预测时刻t的预测值，并不清楚过程噪声如何，所以不适用过程噪声
    Eigen::MatrixXd A_T = A.transpose();
    P = A * P * A_T + Q;//时刻t先验估计值的的协方差,Q为过程噪声的协方差
    return x;
}

//没有输入矩阵和控制矩阵的卡尔曼滤波预测过程
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_)
{
    A = A_;
    x = A * x;
    Eigen::MatrixXd A_T = A.transpose();
    P = A * P * A_T + Q;
    //  cout << "P-=" << P<< endl;
    return x;
}

void KalmanFilter::update(Eigen::MatrixXd& H_, Eigen::VectorXd z_meas)
// H_   观测矩阵
//z_means 观测量，由系统真实观测输入
{
    H = H_;
    Eigen::MatrixXd temp1, temp2, Ht;
    Ht = H.transpose();
    temp1 = H * P * Ht + R;
    temp2 = temp1.inverse();//(H*P*H'+R)^(-1)
    Eigen::MatrixXd K = P * Ht * temp2;
    z = H * x;
    x = x + K * (z_meas - z);
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize, stateSize);//Identity()单位阵
    P = (I - K * H) * P;
    //  cout << "P=" << P << endl;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118