追根溯源卡尔曼滤波原理，源头详解公式由来

0. 前言

之前关于卡尔曼滤波看了很多博客和论文，开始总有一种晕圈的感觉，因为很多文章都是上来给你摆出来几个数学公式，告诉你哪几个公式是预测的，哪几个公式是更新的，然后就稀里糊涂地仿个真让你看下估计量准不准。
一开始做项目的时候并没有关心其中的因为所以，直接就拿来用了，现在又面临着毕设和考研复试都和卡尔曼滤波相关，所以最近就用了几天时间通过各种渠道反复思考，终于对其中的原理、因果和来源有一个深入的了解。

今天就以通俗易懂的方式从“最小均方误差准则”的角度写一篇自己对卡尔曼滤波的较为完整的见解，也希望能够给初学者提供一些帮助，如有错误，欢迎批评指正。

1. 卡尔曼滤波简介

Kalman滤波是一种线性、无偏、最小均方误差估计算法，是最优估计方法的一种。

“滤波”二字就是在测量目标信号的基础上，采取最优算法，对目标的状态进行精确估计。

所谓最小均方误差准则，是要求目标的状态估计值与真实值误差的方差达到最小值。

之所以称为“估计”，是因为实际系统中必然有各种随机噪声的存在，这就造成无论是状态的线性转移推导还是利用传感器观测实际数据都会存在误差，如果没有这些随机噪声，那就是确定性问题的求解，不存在“估计”问题。

2. 为什么要用卡尔曼滤波

下面举个最常见的例子：

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的， 也就是现在这一分钟的温度等于过去一分钟的温度（假设我们用一分钟来做时间单位）（先验估计） 。假设你对你的经验不是 100% 的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分布。另外，我们在房间里放一个温度计，但是这个温度计也不准确的， 测量值和实际值有偏差，我们也把这些偏差看成是高斯白噪声。好了， 现在对于某一分钟我们有两个有关于该房间的温度值： 你根据经验的预测值 （系统的 预测值）和温度计的值（测量值）。
Kalman要解决的问题是如何使用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的实际温度值.首先你要根据k-1时刻的温度值,来预测k时刻的温度.因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟k-1时刻一样的,假定是23度,同时该值的高斯噪声的偏差是5度[5是这样得到的：如果k-1时刻最优估计误差为3,你对自己预测噪声标准差是4度,他们平方和再开方,就是5）.至于为何是平方和，可以看做两个高斯过程相加[上次最优估计结果是个高斯过程，这次预测也是高斯过程]，所得的也是高斯过程，方差为原先两者的方差之和）]
然后，你从温度计那里得到了 k 时刻的温度值，假设是 25 度，同时该值的噪声标准差是 4 度。 由于我们用于估算 k 时刻的实际温度有两个温度值， 分别是 23 度和 25 度。 究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点， 我们可以上次的估计值的噪声方差及上次的最优估计方差总和之比判断。算出比例因子Kg: Kg^2 = 5^2 + 4^2) ，所以 Kg=0.78 ，我们可以估算出 k 时刻的实际温度值（最优估计）是： 23+0.78*(25-23)=24.56 度[估计值+Kg*(测量值-估计值)].可以看出,因为温度计的协方差比较小（比较相信温度计）,所以估算出的最优温度值偏向温度计的值。
现在我们已经得到 k 时刻的最优温度值了，下一步就是要进入 k+1 时刻，进行新的最优估算。到现在为止，好像还没看到什么递归的东西出现。对了，在进入 k+1 时刻之前，我们还要算出 k 时刻那个最优值（ 24.56 度）的噪声标准差。算法如下： ((1-Kg)*5^2) ^0.5 = 2.35 。这里 的 5 就是上面的k 时刻你预测的那个 23 度温度值的标准差，得出的 2.35 就是进入 k+1 时刻以 后 k 时刻估算出的最优温度值的标准（对应于上面的 3 ） 。 就是这样，卡尔曼滤波器就不断递归，从而估算出最优的温度值。他运行的 很快， 而且它只保留了上一时刻的 最优估计误差标准差。 上面的Kg ， 就是卡尔曼增益 （ Kalman Gain ） 。 他可以随不同的时刻而改变他自己的值！

3. 卡尔曼滤波的核心思想

以k-1时刻的最优估计 $\stackrel{\wedge }{x_{k-1}}$为准，预测k时刻的状态向量，得到预测量$\stackrel{\wedge }{x_k^{-}}$；同时又对k时刻的状态进行观测，得到观测向量$z_k$，再以观测量对预测量进行修正，从而得到k时刻的最优状态估计$\stackrel{\wedge }{x_k}$。

4. 预先的数学知识准备

（1）先验估计：先验状态估计是根据系统过程原理或者经验得到进行的预测。
（2）后验估计：后验状态估计是结合之前的先验状态估计值，再加权观测值得到的估计结果。
（3）先验估计误差：先验估计量（即预测量）$\stackrel{\wedge }{{x_k}^{-}}$和真实状态量$x_k$之间的误差：$e_k^{-}=x_k-\stackrel{\wedge }{{x_k}^{-}}$。
（4）后验估计误差：后验估计量（即最优估计量）$\stackrel{\wedge }{x_k}$和真实状态量$x_k$之间的误差：$e_k=x_k-\stackrel{\wedge }{x_k}$。
（5）均方误差：它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方误差等于每个样本的误差平方再乘以该样本出现的概率的和。
（6）协方差（矩阵）：
协方差$Cov(X,Y)$用于衡量两个变量X与Y之间的总体误差，协方差的绝对值越大，则两个变量相互影响越大；
协方差矩阵$$C\text{=}\left[\begin{array}{ccccc}{\sum }_{11}& \cdots & {\sum }_{1j}& \cdots & {\sum }_{1n}\\ ⋮& \ddots & ⋮& \ddots & ⋮\\ {\sum }_{i1}& \cdots & {\sum }_{ij}& \cdots & {\sum }_{in}\\ ⋮& \ddots & ⋮& \ddots & ⋮\\ {\sum }_{n1}& \cdots & {\sum }_{nj}& \cdots & {\sum }_{nn}\end{array}\right]$$中的每个元素${\sum }_{ij}$是向量中第$i$个变量元素和第$j$个变量元素两个随机变量之间的协方差，衡量它们之间的线性相关性，那么协方差矩阵$C$则表示一组随机变量之间的两两线性相关性，用于衡量多个变量的总体误差。

Notes：
<1>对于线性系统中的状态信息，一般不只包含一个变量（如小车运动时的状态信息一般为位置p和速度v），那么则以状态向量的形式表示，故该状态向量的协方差矩阵是衡量向量内各变量元素的两两线性相关性。

<2>协方差矩阵的主对角线元素分别为每个变量的方差。方差是协方差的一种特殊情况，即当两个变量是相同的情况，描述的是样本集合的各个样本点到均值的距离之平均；

<3>卡尔曼滤波中共涉及三个协方差矩阵：
状态协方差矩阵（误差协方差矩阵）$P_k$，过程噪声协方差矩阵$Q_k$，测量噪声协方差矩阵$R_k$；

<4>由协方差的定义及期望的性质，有：
①[X]和[Y]均为一维随机变量时：
协方差： C o v ( X , Y ) = E { [ X − E ( X ) ] [ Y − E ( Y ) ] } = E { X Y − X E ( Y ) − Y E ( X ) − E ( X ) E ( Y ) } = E ( X Y ) − E ( X ) E ( Y ) − E ( Y ) E ( X ) + E ( X ) E ( Y ) = E ( X Y ) − E ( X ) E ( Y ) \begin{aligned} & Cov(X,Y)=E\left\{ \left[ X-E(X) \right]\left[ Y-E(Y) \right] \right\} \\ & =E\left\{ XY-XE(Y)-YE(X)-E(X)E(Y) \right\} \\ & =E(XY)-E(X)E(Y)-E(Y)E(X)+E(X)E(Y) \\ & =E(XY)-E(X)E(Y) \\ \end{aligned} ​Cov(X,Y)=E{ [X−E(X)][Y−E(Y)]}=E{ XY−XE(Y)−YE(X)−E(X)E(Y)}=E(XY)−E(X)E(Y)−E(Y)E(X)+E(X)E(Y)=E(XY)−E(X)E(Y)​

方差： D ( X ) = C o v ( X , X ) = E { [ X − E ( X ) ] [ X − E ( X ) ] } = E { [ X − E ( X ) ] 2 } D(X)=Cov(X,X)=E\left\{ \left[ X-E(X) \right]\left[ X-E(X) \right] \right\}=E\left\{ { {\left[ X-E(X) \right]}^{2}} \right\} D(X)=Cov(X,X)=E{ [X−E(X)][X−E(X)]}=E{ [X−E(X)]2}

②[X]为n维向量时：
协方差矩阵：$$\begin{array}{rl}C& \text{=E}\left\{\left[X-E(X)\right]{\left[X-E(X)\right]}^T\right\}\\ & \text{=}\left[\begin{array}{ccccc}{\sum }_{11}& \cdots & {\sum }_{1j}& \cdots & {\sum }_{1n}\\ ⋮& \ddots & ⋮& \ddots & ⋮\\ {\sum }_{i1}& \cdots & {\sum }_{ij}& \cdots & {\sum }_{in}\\ ⋮& \ddots & ⋮& \ddots & ⋮\\ {\sum }_{n1}& \cdots & {\sum }_{nj}& \cdots & {\sum }_{nn}\end{array}\right]\end{array}$$

协方差矩阵的性质：
若$X$的协方差矩阵为$$C\text{=E}\left\{\left[X-E(X)\right]{\left[X-E(X)\right]}^T\right\}$$

则$AX$的协方差矩阵为
$$\begin{array}{rl}& C^{\prime }\text{=E}\left\{\left[AX-E(AX)\right]{\left[AX-E(AX)\right]}^T\right\}\\ & \text{=E}\left\{A\left[X-E(X)\right]{\left[X-E(X)\right]}^T{A}^T\right\}\\ & \text{=}A\text{E}\left\{\left[X-E(X)\right]{\left[X-E(X)\right]}^T\right\}{A}^T\\ & \text{=}A{C}^{T}A\end{array}$$

5. 线性系统的方程描述

在线性离散时间系统中，目标运动的状态（转移）方程和观测方程分别可以表示为：

$$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k\text{\hspace{1em}(1)}$$

$$z_k=H{x}_k+v_k\text{\hspace{1em}(2)}$$

注意：一定要清楚这两个方程只是用来描述动态线性系统的，不要和后面卡尔曼预测方程混淆，并且关于这两个方程的由来在下面第6节 我也会介绍。

第一遍看的时候你只需要清楚每个变量叫什么就可以，看完下面第6节的介绍再返回来仔细看这两个公式及各字母的含义。

上述两式中，
$x_k$是k时刻的系统状态向量(M维)；

$A_k$为状态转移矩阵（M阶方阵），描述动态系统在时刻k-1的状态到时刻k的状态之间的转移，应为已知；

$B_k$为k时刻影响控制量$u_k$的控制矩阵（M阶方阵）；

$u_k$是k时刻对系统的控制量(M维)，$B_k{u}_k$和$A_k{x}_{k-1}$之和用来描述由k-1时刻到k时刻状态的线性关系；
（例如一元二次方程$y=ax+b$，$a$相当于状态转移矩阵$A_k$，$b$相当于$B_k{u}_k$， y y