离散卡尔曼滤波（详解卡尔曼滤波）

今日疯言疯语：

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

长期可内推大疆，知乎私聊发送内推码

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

直观理解

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

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

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

那么问题来了，导弹离目标的距离现在既有个观测值7m，又有个估计值6m。到底相信哪个？单纯相信观测值万一雷达被敌方干扰了呢？单纯相信估计值那么万一上个时刻的距离估计值或者速度不准呢？所以，我们要根据观测值和估计值的准确度来得到最终导弹离目标的距离估计值。准确度高的就最终结果比重高，准确度低就占比低。如果雷达测量的那个7m准确度是0.2809，根据速度估计出的那个6m准确度是0.01，那么最终的距离估计结果就是 result=(1−0.28090.01+0.2809)∗6+0.28090.01+0.2809∗7result=(1-\frac{0.2809}{0.01+0.2809})*6 + \frac{0.2809}{0.01+0.2809}*7 米. （这些数据大家有直观理解就好，文末会给出计算方法）

事实上 0.28090.01+0.2809\frac{0.2809}{0.01+0.2809} 这个就是卡尔曼增益，它就是表示这个雷达数据相对于根据速度计算出的估计值的靠谱程度。

直观理解讲完了，以上。

卡尔曼滤波怎么做的？

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

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

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

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

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

我们用 xt−1x_{t-1} 表示导弹在t-1时刻的位置的概率分布（它画出来就和上面那个图长的一样）。 vt−1v_{t-1} 表示导弹在t-1时刻的速度的概率分布。用 ztz_{t} 表示雷达测量到导弹离目标的距离概率分布。并且它们都是正态分布。

我们知道正态分布可以这么表示 N(均值，方差)。

现在我们是已知

导弹在t-1时刻的位置的概率分布，为 xt−1=N(10,0.22) x_{t-1}=N(10, 0.2^2) 。其实0.2就是上个时刻导弹位置的误差导弹的速度的概率分布，为 vt−1=N(4,0.72)v_{t-1}=N(4,0.7^2) 。其实0.7就是获取速度的那个传感器噪声误差。你会问咋就有噪声误差了？不知道你有没有用过手机里面的指南针那个软件，明明你没动那个指南针还是会在某个范围内波动。还有称体重，明明你没动那个称数据就是不断的动。这就是噪声误差。雷达在t时刻测量到导弹离目标的距离的概率分布，为 zt=N(7,0.12)z_t=N(7,0.1^2) ，这0.1就是雷达的噪声误差

在前面我们知道了卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差，需要实施“两步走”战略。

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

我们来看看具体怎么做的。

计算粗略估计值（这个是做了一个硬假设：导弹是保持匀速运动），大家可以对比着看下直观理解是怎么做的： 在时刻的粗略估计值x在t时刻的粗略估计值=xt−1−vt−1=N(10,0.22)−N(4,0.72)=N(6,0.22+0.72)x在t时刻的粗略估计值=x_{t-1}-v_{t-1}=N(10,0.2^2)-N(4,0.7^2)=N(6,0.2^2+0.7^2) 根据 粗略估计值的概率分布与雷达的测量值概率分布得到精确估计值的概率分布。现在我们是用概率分布来计算。所以和直观理解中的计算方式有不同。因为直观理解中的计算方式是相当于粗略估计值这个概率分布的均值与雷达测量值的概率分布的均值进行加权和得到精确估计值的概率分布的均值。由于正态分布是均值那个地方的概率最大，所以当前时刻导弹位置的精确估计值就是它概率分布的均值。但是现在我们还是没有回答怎么根据粗略估计值的概率分布与雷达的测量值概率分布得到精确估计值的概率分布。其实这个也很简单。直接把这两个概率分布相乘即可。关于为何是直接相乘请参考这个回答《如何通俗并尽可能详细解释卡尔曼滤波？》。这是由贝叶斯滤波所推导得到的。所以我们得到当前时刻导弹位置的精确估计是 在时刻的粗略估计值xt=x在t时刻的粗略估计值×zt=N(6,0.22+0.72)×N(7,0.12)x_t=x在t时刻的粗略估计值 \times z_t=N(6,0.2^2+0.7^2) \times N(7,0.1^2) 。 上式乘法的结果是： N(6,0.22+0.72)×N(7,0.12)=N(6,0.532)×N(7,0.12)=N(6∗0.12+7∗0.5320.532+0.12,0.12∗0.5320.532+0.12)N(6,0.2^2+0.7^2) \times N(7,0.1^2)=N(6,0.53^2) \times N(7,0.1^2)=N( \frac{6*0.1^2+7*0.53^2}{0.53^2+0.1^2},\frac{0.1^2*0.53^2}{0.53^2+0.1^2})。注意这里用到了高斯分布的乘法公式（这里我贴出来省的大家搜了） N(μ1,σ12)∗N(μ2,σ22)=N(σ12μ2+σ22μ1σ12+σ22,σ12σ22σ12+σ22) N(\mu_1,\sigma_1^2)*N(\mu_2,\sigma_2^2)=N(\frac{\sigma_1^2 \mu_2 + \sigma_2^2 \mu_1} {\sigma_1^2 + \sigma_2^2} , \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2}) 。大家可以对比着直观理解那看看有什么相似的地方。事实上可以看到我们就是把方差作为可信度，方差越大越不可信。然后也就是说用方差做权重进行加权和得到均值。xt=N(6∗0.12+7∗0.5320.532+0.12,0.12∗0.532(0.532+0.12)2)=N((1−0.5320.532+0.12)∗6+0.5320.532+0.12∗7,0.532+0.12(0.532+0.12)2)x_t=N(\frac{6*0.1^2+7*0.53^2}{0.53^2+0.1^2},\frac{0.1^2*0.53^2}{(0.53^2+0.1^2)^2})=N((1-\frac{0.53^2}{0.53^2+0.1^2})*6+\frac{0.53^2}{0.53^2+0.1^2}*7,\frac{0.53^2+0.1^2}{(0.53^2+0.1^2)^2}) 。而正态分布是在均值那个地方的概率最大。所以当前时刻导弹位置的最优估计就是上面这个概率分布的均值 。而 0.5320.532+0.12\frac{0.53^2}{0.53^2+0.1^2} 这个东西就叫做卡尔曼增益！

以上！你的赞和关注是我愿意分享的最大动力！

卡尔曼滤波Python代码实践

这个实践的已知量是导弹做水平运动，已知导弹每个时刻的速度dv,和速度测量仪器的标准差是v_std，还已知每个时刻用GPS测量出导弹位置position_noise以及GPS的方差是predict_var。（注意标准差的平方是方差，不用觉得奇怪）

我们需要用卡尔曼滤波根据这些信息获得融合两种传感器后的位置信息position_predict# -*- coding: utf-8 -*- """ @司南牧 """ import numpy as np t = np.linspace(1,100,100) # 在1~100s内采样100次 a = 0.5 # 加速度值 position = (a * t**2)/2 position_noise = position+np.random.normal(0,120,size=(t.shape[0])) # 模拟生成GPS位置测量数据（带噪声） import matplotlib.pyplot as plt plt.plot(t,position,label=truth position) plt.plot(t,position_noise,label=only use measured position) #---------------卡尔曼滤波---------------- # 初始的估计导弹的位置就直接用GPS测量的位置 predicts = [position_noise[0]] position_predict = predicts[0] predict_var = 0 odo_var = 120**2 #这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低 v_std = 50 # 测量仪器的方差（这个方差在现实生活中是需要我们进行传感器标定才能算出来的，可搜Allan方差标定） for i in range(1,t.shape[0]): dv = (position[i]-position[i-1]) + np.random.normal(0,50) # 模拟从IMU读取出的速度 position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置 predict_var += v_std**2 # 更新预测数据的方差 # 下面是Kalman滤波 position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var) predict_var = (predict_var * odo_var)/(predict_var + odo_var) predicts.append(position_predict) plt.plot(t,predicts,label=kalman filtered position) plt.legend() plt.show()

推荐阅读：

怎样从实际场景上理解粒子滤波（Particle Filter）？

什么是梯度下降法？

Ai酱：{高中生能看懂的}梯度下降是个啥？

Ai酱：易懂的神经网络理论到实践(1)：单个神经元+随机梯度下降学习逻辑与规则

Ai酱：[漫谈]高中的消元法解方程与线性代数及LU分解之间的联系

适合初学者的机器学习神经网络理论到实践

快速提高逼格的电脑快捷键有哪些？

司南牧：【易懂教程】我是如何十分钟理解与推导贝叶斯滤波(Bayes Filter)算法？

推荐开源项目：[简单的SLAM与机器人教程与编程实践-github](varyshare/easy_slam_tutorial)