【目标跟踪】卡尔曼滤波（公式推导与代码）

前言

​ 1、卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波)过程。

​ 2、在跟踪中卡尔曼滤波可以基于目标前一时刻的位置，来预测当前时刻的位置，并且可以比传感器更准确的估计目标的位置。

​ 3、卡尔曼滤波不需要前面的历史数据，只需要前一时刻的状态数据就可以进行预测。

参考链接：https://www.kalmanfilter.net/background.html （kalman滤波保姆级教程）

一、卡尔曼滤波推导

1.1、设想场景

试想场景：小明正前方有一个人甲，小明用肉眼测量与甲距离为10m。假设小明和甲都静止不动，小明有个激光测距仪告诉小明与甲的距离分别为9.9m。

由于肉眼测量与激光测距仪的数据都不能确定为真值，那我们有没有办法结合他们的数据找个更准确的值呢？

假设小明肉眼估计的距离误差为 0.5m，激光测距仪误差0.2m

先来看看卡尔曼滤波如何做的 （后面会解释为什么这么做）

1.2、一维公式推导

​ 卡尔曼滤波基于高斯分布来建立状态方程。

​在已有的测量数据情况下，我们要尽可能找到一个更加准确的值。

我们要获取更加准确值，我们需要同时符合两者假设条件。假定两者都属于正态分布，我们把两个高斯分布相乘，结果正好可以获得另一个放缩的高斯分布，得到了这个我们认为是最大后验概率分布。

可以手动推导下：

现在获得一个新的高斯分布。此时

令相同部分为K

则

一定要手动推导一下，然后回过头看我们假设的场景。

1.3、类比多维

我们通过第一个场景得到

这个是通过一维高斯分布获得的，现在我们类比下多维高斯分布

二、代码示例

2.1、一维空间

例1：假设飞行器沿某一个方向做匀速运动

v = 40m/s

测量值（m） 30110 30265 30740 30750 31135 31015 31180 31610 31960 31865

测量间隔 5s

2.2、二维空间

1、我们会预测车体状态 2、我们会测量车体状态 测量值去修正预测值，得到新的状态继续预测，如此反复进行