卡尔曼滤波器的简单理解

参考内容：
1、https://blog.csdn.net/u010720661/article/details/63253509#commentBox
2、https://www.bilibili.com/video/av4356232?from=search&seid=8060035898652294506
3、https://blog.csdn.net/michaelhan3/article/details/85453368

对卡尔曼滤波器的简单理解。

卡尔曼滤波分为两个部分：预测部分和观测部分

一、预测

-----------------------------------------手动分割线--------------------------------------------
假设一个情景，一辆小车在路上行驶，可以用他的位置和速度来表示他的状态


该状态满足高斯分布，p表示位置；v表示速度；u为控制量，比如说油门，表示加速度；
那么小车在当前时刻的状态就可以表示为：

写成矩阵形式为：

令

可以将矩阵表达式简写为：

每个时刻的状态的不确定性由协方差P表示，协方差之间的不确定性传递关系为：（协方差矩阵的性质：cov(Ax,By) = Acov(x,y)B‘）

考虑到预测模型本身的不确定性，所以需要加上误差Q，所以当前时刻的协方差P为：
经过预测之后得到当前时刻的状态 Xt 以及不确定性 Pt 记为：

（Xpre 任为高斯分布）

二、观测

-----------------------------------------手动分割线--------------------------------------------

现在假设路面上有一测量仪可以测量小车的位置，用Zt表示

Zt与状态X之间的关系为：

Zt为高斯分布，其中H为观测矩阵；V为观测器本身带来的误差
记观测噪声 V 的协方差为 R

经过观测之后得到当前时刻的观测量 Zt 以及噪声协方差 R

三、数据融合

-----------------------------------------手动分割线--------------------------------------------
现在已经得到两个高斯分布了，分别为为预测部分的高斯分布（ Xpre ）和观测部分的高斯分布（ Zt ），大致分布如图所示：

显然两个高斯分布的相交处的数据接近于真实数据的可能性更大一点，所以可以通过高斯分布相乘求得新的高斯分布，具体过程如下：
///

两个高斯分布相乘结果

令

则

将其扩展为矩阵形式

///

通过传感器计算出的预测部分的高斯分布：

观测器的数据及不确定性的观测部分的高斯分布：

相乘得到的新的高斯分布：

将表达式(4),(5),(6)代入表达式(1),(2),(3)即可得到：
( 注意：K里边包含了H ，所以H-1K = K，具体是什么原因不清楚，如果有大神知道的话还请赐教 )

其中K称为卡尔曼增益，其作用为：
1、权衡Ppre 、R的权重( 就是相信哪个数据来源更多一点 )
2、将残差 (Zt - HXpre) 的表示形式从观测域转换到状态域

上式给出了完整的更新步骤方程。Xt’ 是新的最优估计，Xt’ 和Pt’ 将放到下一个预测和更新方程中不断迭代。