卡尔曼滤波（Kalman Filter）原理与公式推导
一、背景---卡尔曼滤波的意义
随着传感技术、机器人、自动驾驶以及航空航天等技术的不断发展，对控制系统的精度及稳定性的要求也越来越高。卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。
对于Kalman Filter的理解，用过的都知道“黄金五条”公式，且通过“预测”与“更新”两个过程来对系统的状态进行最优估计，但完整的推导过程却不一定能写出来，希望通过此文能对卡尔曼滤波的原理及状态估计算法有更一步的理解。
二、卡尔曼滤波的基本模型
假设一离散线性动态系统的模型如下所示：
-------(1)
--------------------(2)
其中，各变量表征的意义为：
———————————————————————————
系统状态矩阵，-------， 状态阵的观测量（实测）
状态转移矩阵 ，-------， 控制输入矩阵
状态观测矩阵
过程噪声，-------， 测量噪声
———————————————————————————
如果大家学过《现代控制理论》的话，对上述模型的描述形式一定不会陌生，只是多了变量 与 。其中，随机变量 代表过程噪声（process noise）， 代表测量噪声（measurement noise），且为高斯白噪声，协方差分别为 和 ，即 ， 。
为什么要引入这两个变量呢？
对于大多数实际的控制系统（如倒立摆系统）而言，它并不是一个严格的线性时变系统(Linear Time System)，亦或系统结构参数的不确定性，导致估计的状态值 存在偏差，而这个偏差值由过程噪声 来表征。对于状态观测方程——(2)而言，我们知道采集的信号往往是包含噪声及干扰信号的，亦或观测矩阵 自身的观测偏差，从而导致了测得的实际信号（观测值）与真实值之间存在一定的偏差，这个量由 来表征。
三、Kalman Filter的公式推导
对于状态估计算法而言，我们可以获取状态量的三个值：状态预测值（ ）、最优估计值（ ）以及真实值（ ），卡尔曼滤波的原理就是利用卡尔曼增益来修正状态预测值，使其逼近真实值。
为使其便于理解，对卡尔曼滤波的推导过程，第一个过程分为状态估计协方差 的推导，即代价函数的求取；第二步则是卡尔曼增益矩阵及其他准则的推导。
3.1 状态估计协方差
在对状态估计协方差 求取之前，先介绍状态估计算法中关于系统状态矩阵的几种表示：
——————————————————————————————————
状态的真实值；
状态的预测值，也称先验状态估计值（a prior state estimate）；
状态的最优估计值，也称后验状态估计值（a posteriori state estimate）；
——————————————————————————————————
- 状态预测值 由状态预测方程可得：黄金1条
--------(3)
- 状态最优估计值 可由状态更新方程可得：黄金2条
-----(4)
通过该方程可知，卡尔曼增益 实际上表征了状态最优估计过程中模型预测误差（Predicted error）与量测误差（Measurement error）的比重（如下所示），即 。当 时，即预测误差为0，系统的状态值完全取决与预测值（ ）；而当 时，即量测误差为0，系统的状态值完全取决于量侧值。
Predicted error/ (Predicted error + Measurement error)
因此，可令：
- ---------------(5)
- ---------------(6)
- -----------(7)
- --------------(8)
其中：
—————————————————————————
先验状态误差
后验状态误差
真实值与预测值之间的协方差
真实值与最优估计值之间的协方差
—————————————————————————
由方程(2)(4)可知，
--------(9)
------(10)
变换后的方程，
---(11)
联立方程(5)(6)可知，
-----------------(12)
因此，由方程(8)可知估计误差方差阵 为：
---(13)
展开可知
--------(14)
3.2 状态变量
卡尔曼滤波的估计原则就是使最优状态估计的协方差 最小，使其越来越逼近于真实值。因此，其目标函数为：
-----------------------(15)
对卡尔曼增益矩阵 求偏导，不难可知：
----(16)
由此可知最优估计条件下的卡尔曼增益矩阵 为-----黄金三条
----(17)
联立式(11)(14)即可知道估计误差方差阵为-----------黄金四条
----------(18)
黄金5条的最后一条的状态估计协方差 ，由式（5）可知：
化简可知：
由式（7）可知：
-----(19)
--------------------(20)
由式（20）不难得知，预测协方差矩阵 为：---黄金5条
-----------(21)
至此，卡尔曼滤波的黄金5条全部推导完成，其“状态预测”与“状态更新”过程如下图所示。
四、小结
卡尔曼滤波作为一种状态最优估计算法，与《现代控制系统》中的观测器设计思维类似，都是利用观测量并结合系统的模型来对系统的状态进行估计。但二者又有不同，由前面的介绍可知卡尔曼滤波的目的是利用卡尔曼增益来修正状态预测值，而观测器设计的目的是当系统的有些状态信息无法有效获取或不易测得时，通过状态重构，以便实现反馈控制。
转自：https://zhuanlan.zhihu.com/p/48876718