Kalman filtering--卡尔曼滤波

参考资料：

https://mp.weixin.qq.com/s/SuDysn3Y4li8fdxz3pqIKw

图说卡尔曼滤波，一份通俗易懂的教程 - 论智的文章 - 知乎 https://zhuanlan.zhihu.com/p/39912633

卡尔曼滤波(Kalman filter) 含详细数学推导 - Mockingjay的文章 - 知乎 https://zhuanlan.zhihu.com/p/134595781

http://t.csdn.cn/ZpWW1

开启炼丹之旅

以导航系统为例，物体在某一时刻的位置p以及速度v状态可以用向量表示为：

由于传感器的读数是对当前状态的最佳估计，故存在不确定性，该不确定性可用协方差矩阵P表示，另一种理解为位置和当前速度间是存在相关关系，不难想到当期速度增加，下一期位置也就更远：

1、基础预测

基于该信息可以预测其下一时期的状态：

位置p=上期位置+移动距离

速度v=上期状态

矩阵形式为

𝒙_𝒕|𝒕−𝟏表示基于t-1 期状态对t 期进行预测，其中𝐹𝑡为状态转移矩阵，描述的是从上一状态转化到下一状态的发生的变换。

协方差矩阵同样会发生变化，𝑷𝒕|𝒕−𝟏表示表示基于t-1 期精度/不确定性/方差对t 期的进行

预测，根据矩阵协方差性质：

2、加入系统控制

当向物体施加外力是会对其状态产生影响，F=ma这里可以用加速度a表示外力影响：

矩阵形式：

其中Bt为状态控制矩阵，该部分表示外力如何影响物体状态/影响程度；ut为状态控制向量，表示施加的外力大小。因为施加的外力是人为控制的，不存在不确定性，所以协方差矩阵不作更新。

3、加入不确定因素

例如路面状况导致轮胎打滑、逆风阻力抵消施加的力等情况均为无法控制的外部的不确定因素。对于这一部分，假设该干扰𝑤𝑡服从均值为0，标准差为𝑄𝑡的正态分布。

此时状态预测方程进一步更新为：

到这里总结一下：

新的状态估计=上期卡尔曼最佳估计𝒙𝒕−𝟏+系统控制+不确定因素（此处假设均值为零）

新的不确定性=上期方差最优估计 𝑷𝒕|𝒕−𝟏+外部不确定因素

4、状态空间方程

以上物体运动可以用如下状态空间方程描述：

𝑥𝑡|𝑡−1是基于上一状态对当前状态的预测；状态读数𝒛𝒕是传感器对于真实状态的感知加上传感器自身精度误差之和的结果。传感器读数永远不会等于真实状态，但会是对物体真实状态的映射，两者存在对应关系但绝非对等。这里引入关系矩阵𝑯𝒕表示真实状态与传感器读数的映射关系，同时用𝑣𝑡表示噪声，𝑧𝑡后面进一步解释。

有了 𝒙𝒕|𝒕−𝟏和𝒛𝒕，怎么估计真实状态真实状态 𝒙𝒕？？？

传感器视角

预期观测到什么？

以上“基础预测+系统控制+不确定因素”共同预测物体下一状态。此前对物体所做的预测状态𝑥𝑡|𝑡−1以及𝑃𝑡|𝑡−1下，观测到传感器的观测值应该为：

此处是将预测状态直接转化为传感器读数，不存在额外的不确定性𝑣𝑡，因此这时对应的协方差矩阵：

此时完成了对观测值的预测，预测其结果服从如下高斯分布：

实际观测到什么？

再申：传感器对当前状态读数𝑧𝑡是一种（最优）估计，因此存在不确定性/噪声，可认为噪声𝑣𝑡服从均值为0，标准差为𝑅𝑡的正态分布：

那么观察结果服从以下分布：

Kalman炼丹大法

这里借用一张图，出处为https://zhuanlan.zhihu.com/p/444977764

这幅图展现是预测观测结果预测观结果 𝜇0（图中用𝜇𝑒𝑥𝑝𝑒𝑐𝑡𝑒𝑑表示 ）和实际观测结果 𝑧𝑡的分布可能，卡尔曼滤波的核心就是要从这两个分布中对真实结果进行估计。黄色部分是两个分布重叠部分，也就是概率更高的结果，如何获取这部分？概率密度函数乘积

预期观测结果与真实观测结果的概率密度函数乘积

将预期观测结果𝑁(𝜇0,𝛴0)与真实观测结果𝑁(𝜇1,𝛴1)相乘可以得到以下新的正态分布（此处不涉及分布具体推导内容，只对结果进行概述）：

令

那么

矩阵形式

代入 𝜇0,𝛴0,𝜇1,𝛴1

有

通过乘逆矩阵可以将上式化简为

其中𝑲′就是卡尔曼增益

另一个角度

卡尔曼滤波的核心思想可以理解为加权，即通过对预测值𝑥𝑡|𝑡−1（先验估计）和观测值𝑧𝑡赋予不同权重求和得到最优估计（后验估计）。

那么该加权表达式可以写成

如何求得卡尔曼增益𝑲′？最小化估计误差

从最小化t期估计误差𝒆𝒕入手，估计误差等于真实状态𝑥𝑡与最优估计𝑥𝑡′的差值

此时最优估计协方差𝑃𝑡

𝑃𝑡的展开式中包含卡尔曼增益𝑲′，最小化估计误差的目标下，对𝑲′求导并令其等于零，可以求解得到相同得结果

具体过程参考http://t.csdn.cn/ZpWW1

写在最后的一些思考：

1、为什么要将“预测状态𝑥𝑡|𝑡−1”再转化为“预测的观测结果𝐻𝑡𝑥𝑡|𝑡−1”？

我认为是要兼顾传感器的实际读数这一分布，类似统一量纲/统一单位，这样得到的两个分布都是基于传感器视角的才能直接将其概率密度函数进行相乘。否则𝑥𝑡|𝑡−1是一个状态分布，而𝑧𝑡为传感器读数分布，两者不能直接相乘。

2、为什么两个分布密度函数相乘后左边是𝐻𝑡𝑥𝑡′ 而不直接是𝑥𝑡′ ？

统一量纲，因为两个进行相乘的两个分布都是基于传感器读数视角，从真实状态到传感器读数状态间存在关系矩阵𝐻𝑡，所以乘积结果就是传感器读数估计。