卡尔曼滤波（Kalman Filter）原理与公式推导【转载】

转载知乎文章，并增加补充更直白的点描述。

一、背景---卡尔曼滤波的意义

随着传感技术、机器人、自动驾驶以及航空航天等技术的不断发展，对控制系统的精度及稳定性的要求也越来越高。卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。

对于Kalman Filter的理解，用过的都知道“黄金五条”公式，且通过“预测”与“更新”两个过程来对系统的状态进行最优估计，但完整的推导过程却不一定能写出来，希望通过此文能对卡尔曼滤波的原理及状态估计算法有更一步的理解。

 

二、卡尔曼滤波的基本模型

 自我补充：由两个方程：

1、状态转移方程（状态变换方程）：运动过程如何变化的。（定位）

2、状态观测方程（路标点观测结果）：运动过程中，观察到的路标点。（建图）

我补充的：Uk代表传感器采集数据（输入数据）。

 为什么要引入这两个变量呢？

过程噪声

测量噪声

三、Kalman Filter的公式推导

卡尔曼滤波的原理就是利用卡尔曼增益来修正状态预测值，使其逼近真实值。

过程推导：

• 第一个过程：代价函数的求取。
• 第二个过程：卡尔曼增益矩阵。即增益修正。

3.1 状态估计协方差 

假设某事件：a+b=res。

1、 预测值：先验状态估计。演变值res：根据已知a,b得到结果res。a+b得到res。这个求得res的过程就叫先验估计。正向推理。由因得果。种瓜得瓜，种豆得豆。

2、 最优估计值：后验状态估计值：已经得到了res，和传感器数据a，推测为了得到这个res，最佳的状态b。

这秧子上结的果是瓜，春天你种下的肯定是瓜种子。执果索引。这个瓜的大小就推测今年雨水的状态（b）（或说种子生长条件，或者变化干扰因素）。

这个种子就是前一刻（k-1）结果。这个瓜就是此时k的结果。变换过程就是这个生长状态。为何我们关心（求解）这个状态，因为基于这个状态就能准确预测此时的真实结果。

目的就是为了做预测分析。

就这两句话：收费都不为过。关注点赞。

从概率学讲：贝叶斯公式。

1、预测：黄金1条：基于前一刻状态（k-1）和当前传感器数据，以及噪声，对当前状态预测。

2、更新：黄金2条：对于当前状态预测结合卡尔曼增益K获取当前状态的最优估计。更新

通过该方程可知，卡尔曼增益K实际上表征了状态最优估计过程中模型预测误差（Predicted error）与量测误差（Measurement error）的比重（如下所示）

3.2 状态变量

 至此，卡尔曼滤波的黄金5条全部推导完成，其“状态预测”与“状态更新”过程如下图所示。

四、小结

卡尔曼滤波作为一种状态最优估计算法，与《现代控制系统》中的观测器设计思维类似，都是利用观测量并结合系统的模型来对系统的状态进行估计。但二者又有不同，由前面的介绍可知卡尔曼滤波的目的是利用卡尔曼增益来修正状态预测值，而观测器设计的目的是当系统的有些状态信息无法有效获取或不易测得时，通过状态重构，以便实现反馈控制。