卡尔曼滤波理解

卡尔曼计算步骤分为预测和修正两个阶段

一，预测阶段
 

X_pred   = A * X_evlt + B * u
X_pred_p = A * X_evlt_p * A.transpose() + Q

矩阵中的数据为各维度的分量，如二维噪声矩阵中的值是每个维度的噪声分量。

1. u为系统控制量，A和B为系统参数
2. X_evlt表示上一时刻的最优估计结果
3. X_pred为当前时刻预测结果
4.  A.transpose()表示矩阵转置
5. X_pred_p为预测结果的协方差，表示预测的误差(不确定性)
6. X_evlt_p为上一时刻最优估计的协方差
7. Q表示预测过程的噪声协方差，过程噪声服从正态分布

其意义是以上一时刻的最优估计作为输入根据线性状态方程预测当前时刻的状态，通俗的讲，以上一时刻的最优结果预测当前结果，
以上一时刻最优结果的不确定性来预测当前结果的不确定性

预测方程可参考网上小车位置与速度的例子来理解，链接https://blog.csdn.net/m0_38089090/article/details/79523784

二，修正阶段

Kg = X_pred_p* H.transpose() / (H * X_pred_p H.transpose() + R)
X_evlt   = X_pred + Kg * (Z - H * X_pred)
X_evlt_p = (I - Kg * H) * X_pred_p

y = Z - H * X_pred
S = H * X_pred_p H.transpose() + R

1. Kg为卡尔曼增益
2. R为观测噪声协方差
3. X_evlt为当前时刻的最优估计结果，即所要求的最终结果，它与“预测阶段”中的X_evlt是不同时刻的
4. X_evlt_p为当前最优估计的协方差
5.  Z是当前观测结果
6. y是测量余量，X_evlt = X_pred + Kg * y
7. S是测量余量协方差矩阵，是Kg公式中的分子

对于最优估计以及卡尔曼增益的理解，可参考网上计算房间温度的说明，拷贝内容如下

假设研究一个房间的温度。根据经验判断，房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。
假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。
另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，
假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。
然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，
我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。
算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

三，协方差

根据上一时刻的协方差求当前时刻的协方差公式是由协方差的性质求得的
如变量x的协方差为：
Cov(x)= E
变量y=x*A，则变量y的协方差为：
Cov(Ax)=A * E * A.transpose()

 

四，伪代码

Eigen::MatrixXd pred_curr = A * last_evlt + B * u;
Eigen::MatrixXd pred_curr_p = A * last_evlt_p * A.transpose() + Q;

/*
这一步是计算卡尔曼增益，其意义是根据当前观测结果的不确定性与当前预测结果的不确定性得到的新的不确定性，然后在两个结果中求得最优
*/
MatrixXd tmp = H * pred_curr_p * H.transpose() + R;
Eigen::MatrixXd Kg = pred_curr_p * H.transpose() * tmp.inverse();

Eigen::MatrixXd curr_evlt = pred_curr + Kg * (observe_curr - H * pred_curr);
Eigen::MatrixXd curr_evlt_p = (I - Kg * H) * pred_curr_p;