扩展卡尔曼滤波实战：导弹如何融合雷达与速度数据

今日疯言疯语：

学完卡尔曼滤波可以说你就是地球上战斗力超强的人了！因为几乎任何带电的军事装备、航空航天装备都需要Kalman滤波算法！—— @Ai酱

先回顾下在这篇回答中《如何通俗并尽可能详细解释卡尔曼滤波？》提高的对卡尔曼滤波的直观理解。

直观理解

首先卡尔曼滤波要解决的问题是什么？我以我军发射一枚导弹攻击敌方某固定位置目标为例（搞科技的总要点情怀，老是讲啥小车运动，温度计这些就太low）。导弹需要每隔一秒开雷达测量下离目标的距离。然后由于雷达有误差，所以需要融合自己上个时刻的位置、速度等信息来更准确的确定当前时刻离目标的距离。这就是卡尔曼滤波需要解决的事

从直观理解卡尔曼滤波是怎么解决这个问题的呢？

首先导弹已知“当前这秒雷达测量的导弹离目标的距离（我们称它为观测值，比如雷达直接测量导弹离目标距离7m）”，“上个时刻导弹离目标距离”和“导弹自己当前时刻的速度”这三个数据。而根据“上个时刻导弹离目标距离”和“导弹自己当前时刻的速度”可以估算出当前导弹离目标的距离（我们称它为估计值）。比如：上一秒离目标10m，速度是4m/s，那么现在这秒估计就离目标距离是6m。这个速度数据可以从传感器里面读取也可以从发动机那获得。（根据速度估计导弹距离这个计算叫做移动模型建模，想了解移动模型与观测值和滤波算法之间的联系可以看这篇文章《机器人移动模型：根据控制命令预测机器人位置》）

那么问题来了，导弹离目标的距离现在既有个观测值7m，又有个估计值6m。到底相信哪个？单纯相信观测值万一雷达被敌方干扰了呢？单纯相信估计值那么万一上个时刻的距离估计值或者速度不准呢？所以，我们要根据观测值和估计值的准确度来得到最终导弹离目标的距离估计值。准确度高的就最终结果比重高，准确度低就占比低。如果雷达测量的那个7m准确度是49%，根据速度估计出的那个6m准确度是1%. 这种加权平均的权重值 这个就是所谓的卡尔曼增益，它就是表示这个雷达数据相对于根据速度计算出的估计值的靠谱程度。

直观理解讲完了，以上。

卡尔曼滤波怎么做的？

我们先回顾总结下直观理解中是怎么做的。

根据上一秒导弹的位置 和 导弹的的速度估计出当前时刻导弹的位置粗略估计值。将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。

在前面我们也提到了导弹的位置和雷达测量值都是有误差的。所以卡尔曼想用概率来衡量数据的可信度。

比如：雷达测量的数据它就不只是一个数字了。而是说测量发现导弹有0.8的概率在7m那个位置，有0.1的概率在7.2m那个位置。有0.1的概率在6.9m那个位置。这些数据就叫做概率分布。概率分布的意思就是很多个值还有它们各自出现的概率多大所组成的数据就叫做概率分布。

卡尔曼认为导弹速度、导弹位置、雷达测距的测量值这些都服从正态分布（对就是高中学的那个正态分布）。这是啥意思呢？比如下面这个图是讲上个时刻导弹的位置的概率分布，从图中看出它在10m（横轴）那个位置的纵坐标最高所以概率最大。它在其他地方的概率相对小一点。

以上！你的赞和关注是我愿意分享的最大动力！更多详情请关注知乎@司南牧