入门必读 | 卡尔曼滤波的理解、推导和应用

编辑 | 古月居

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心技术交流群

后台回复【数据集下载】获取计算机视觉近30种数据集！

1. 卡尔曼滤波简介

目的 ：对线性系统状态的估计

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。

2. 卡尔曼滤波原理

方法 ：利用状态过程中的噪声ω 和 观测噪声υ ，给予合理的权重，对状态x 进行估计

详解的原文链接：

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

对此，进行一些简单的描述，只求通俗易懂：

假如想开发了一个可以在树林里到处跑的小机器人，那么需要知道它实时所在的位置，才能导航。

可以在机器人上装GPS定位装置，精度大约为10米，但树林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖，所以这远远不够。

此时还可以获取一些运动的信息:

添加里程计和惯性惯性测量单元记录运动的过程，判断出机器人的姿势和行走距离。

获取发送给电机的指令，知道机器人是否在朝一个方向移动并且没有人干预，预测在下一个状态，机器人很可能朝着相同的方向移动。

但由于传感器的精度、树林里的坑坑洼洼和自然界的风等影响，里程计和惯性测量单元实际并不能非常精确表示机器人实际姿势和行走距离，预测也不是很完美。

GPS 、里程计和惯性测量单元等传感器告诉了我们一些状态信息，我们的预测告诉了我们机器人会怎样运动，但都只是间接的，并且伴随着一些不确定和不准确性。

但是，如果使用所有对我们可用的信息，根据其本身的噪声，分配一定的权重，就能得到一个比任何依据自身估计更好的结果，这就是卡尔曼滤波的用处。

3. 卡尔曼滤波的公式

3.1. 五个公式

先了解一下卡尔曼滤波的五个公式：

3.2. 公式作用

公式（一）：计算预测状态值
公式（二）：计算预测值和真实值之间的预测误差协方差矩阵
公式（三）：在公式一和公式二的条件下，求得卡尔曼增益
公式（四）：计算估计值
公式（五）：计算估计值和真实值之间的误差协方差矩阵

4. 卡尔曼滤波的公式推导

4.1. 符号说明

在推导前，为了在推导过程中混淆，先做一个符号说明：

4.2. 概念说明

4.2.1 高斯分布

通过概率密度函数来定义高斯分布：

高斯分布的概率密度函数是：

均值为μ，标准差为σ

而上述的过程噪声ω 和 观测噪声υ皆服从高斯分布。

4.2.2 协方差矩阵

方差：各个样本与样本均值的差的平方和的均值，度量一组数据的分散程度。

协方差：两个变量总体误差的期望，度量两个变量线性相关性程度。

当两个变量是相同的情况，协方差就是方差。

如果两个变量的变化趋势相反，其中一个变量大于自身的期望值时，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。

两者之间的联系或者关系，关系越大，协方差越大。

协方差矩阵：数据集中两两变量的协方差组成，每个元素是各个向量元素之间的协方差

噪声协方差矩阵越小说明噪声的误差越小，可信度越高，其对角线上的值就是方差。

误差协方差矩阵越小说明过程噪声和量测噪声的关系越小，用比例分开过程噪声ω 和 观测噪声υ，如果关系越小，分开的越精确。

比如一堆白砂糖和盐，如果两种混合的很均匀，说它关系很大，也就越难用比例的方法将其分开。

4.3. 更新优化的状态估计值

假定k时刻的系统状态与k-1时刻有关，并且内部有噪声，则

4.4. 求优化预测状态

在无噪声情况下的预测

观测新息反应了预测噪声ω和观测噪声υ对状态的综合影响

将过程噪声ωk 看作一个 新息 * 比例，则

其中公式(6)，仅卡尔曼增益Kk为未知

4.5. 求卡尔曼增益

来看误差协方差矩阵：

而卡尔曼增益Kk应该使误差协方差矩阵Pk最小

状态新息（状态 - 预测状态）：

假设在无噪声情况下，状态新息:

则误差协方差矩阵：

此时，来看看预测误差协方差矩阵：

此时，再把误差协方差矩阵展开：

若使用T表示误差协方差矩阵的对角线，则

对T[Pk ]求导，就可找到最小的均方差，使T[Pk ]最小，就能得到卡尔曼增益Kk

对T[Pk ]求导：

令其为0，则卡尔曼增益：

其中观测矩阵H和观测噪声协方差矩阵R为常数，所以卡尔曼增益Kk与预测误差协方差矩阵Pk’ 有关

4.6. 求预测误差协方差矩阵

假设观测矩阵 H = 1，那么卡尔曼增益：

则Pk’ 越大，Kk越大，权重重视反馈测量

则Pk’ 越小，Kk越小，权重重视预测值

此时，来看看下一状态的预测误差协方差矩阵：

因为状态x 和过程噪声ωk是独立的，所以再简化

其中状态转移矩阵A和预测噪声协方差矩阵Q为常数，所以预测误差协方差矩阵Pk’与上一状态的误差协方差矩阵Pk-1有关

4.7 求误差协方差矩阵

4.8. 加入外界对系统的作用

在加入外界对系统的作用时，可整理出卡尔曼滤波的五个公式：

5. 卡尔曼滤波应用

用python来简单说明

import numpy as np
import matplotlib.pyplot as plt


delta_t = 0.1                               # 每秒钟采一次样
end_t = 7                                   # 时间长度
time_t = end_t * 10                         # 采样次数
t = np.arange(0, end_t, delta_t)            # 设置时间数组


u = 1                                       # 定义外界对系统的作用 加速度
x = 1 / 2 * u * t ** 2                      # 实际真实位置


v_var = 1                                   # 测量噪声的方差
# 创建高斯噪声，精确到小数点后两位
v_noise = np.round(np.random.normal(0, v_var, time_t), 2)


X = np.mat([[0], [0]])                      # 定义预测优化值的初始状态
v = np.mat(v_noise)                         # 定义测量噪声
z = x + v                                   # 定义测量值（假设测量值=实际状态值+噪声）
A = np.mat([[1, delta_t], [0, 1]])          # 定义状态转移矩阵
B = [[1 / 2 * (delta_t ** 2)], [delta_t]]   # 定义输入控制矩阵
P = np.mat([[1, 0], [0, 1]])                # 定义初始状态协方差矩阵
Q = np.mat([[0.001, 0], [0, 0.001]])        # 定义状态转移(预测噪声)协方差矩阵
H = np.mat([1, 0])                          # 定义观测矩阵
R = np.mat([1])                             # 定义观测噪声协方差
X_mat = np.zeros(time_t)                    # 初始化记录系统预测优化值的列表


for i in range(time_t):
    # 预测
    X_predict = A * X + np.dot(B, u)        # 估算状态变量
    P_predict = A * P * A.T + Q             # 估算状态误差协方差
    # 校正
    K = P_predict * H.T / (H * P_predict * H.T + R)     # 更新卡尔曼增益
    X = X_predict + K * (z[0, i] - H * X_predict)       # 更新预测优化值
    P = (np.eye(2) - K * H) * P_predict                 # 更新状态误差协方差
    # 记录系统的预测优化值
    X_mat[i] = X[0, 0]


plt.rcParams['font.sans-serif'] = ['SimHei']    # 设置正常显示中文
plt.plot(x, "b", label='实际状态值')             # 设置曲线数值
plt.plot(X_mat, "g", label='预测优化值')
plt.plot(z.T, "r--", label='测量值')
plt.xlabel("时间")                               # 设置X轴的名字
plt.ylabel("位移")                               # 设置Y轴的名字
plt.title("卡尔曼滤波示意图")                     # 设置标题
plt.legend()                                    # 设置图例
plt.show()                                      # 显示图表

运行上面的卡尔曼滤波，得到下图：

可以看出，尽管测量值波动很大，但最终的预测优化值与实际状态值相差不大。

[1] python的代码地址:

https://github.com/JoveH-H/A-simple-explanation/blob/master/Kalman_filtering.py

[2] jupyter notebook的代码地址:

https://github.com/JoveH-H/A-simple-explanation/blob/master/ipynb/Kalman_filtering.ipynb

版权声明：本文为CSDN博主「氢键H-H」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：

https://blog.csdn.net/qq_32618327/article/details/89669062

往期回顾

史上最全综述 | 3D目标检测算法汇总！（单目/双目/LiDAR/多模态/时序/半弱自监督）

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

加入我们：自动驾驶之心技术交流群汇总！

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！