卡尔曼滤波的推导过程详解

在学习卡尔曼滤波的时候看了很多博客讲这方面的知识，感觉都讲得表面的东西，无法了解它五个公式真正代表的过程，这篇博客我想以我的理解讲讲卡尔曼滤波。

首先我先写出卡尔曼滤波的具体过程，首先针对如下状态空间模型（因为卡尔曼滤波都是针对状态空间模型进行滤波的）：

$x\left ( k+1 \right )=\phi *x\left ( k \right )+\Gamma *w\left ( k \right )$

$y\left ( k \right )=H*x\left ( k \right )+v\left ( k \right )$

其中 $w\left ( k \right ),v\left ( k \right )$ 分别为系统噪声和测量噪声，对于不了解这个模型怎么回事的建议去补补现代控制理论的知识，这里就不加解释这个模型是什么。

针对上述模型存在如下卡尔曼滤波迭代公式：

这里我想解释一下这五个公式中变量代表的含义：

1. $x_{k/k-1}$ 代表状态一步预测值，简单理解就是我们在已知模型的基础上（这时候不知道下一拍的测量值）预测下一拍系统的状态值得到的结果。举个例子，假如一个恒温系统中，此时估计的温度为38度，在不知道下一时刻温度计测量值的基础上估计系统下一时刻的预测值为38度。

2. $P_{k/k-1}$ 代表状态预测的误差，由公式1可以知道状态预测值 $x_{k/k-1}$ ，但是这个值肯定和我们下一拍实际的状态值有所偏差， $P_{k/k-1}$ 就是体现这个偏差大小的一个指标，统计学上将误差均方差。

3. $K_{k}$ 表示滤波增益，怎么理解这个增益？？我们由系统方程可以得到下一拍状态预测值，由传感器可以得到状态的测量值，当前这两个数据对于状态的真实值来说都是存在误差的，怎么将误差达到更小呢？？将两种数据融合在一起可以达到误差更小，融合的方式就是这和滤波增益 $K_{k}$ ，你可以看成两种数据之间的一个权重。

4. $x_{k}$ 代表状态估计，就是将上面将的两种数据融合之后的值。

5. $P_{k/k}$ 表示状态估计均方差，我们融合后的数据肯定相对于真实值之间还存在一定的误差， $P_{k/k}$ 表示就是这个误差的大小，也是均方差。

通过上述的解释后，我将卡尔曼滤波推导过程写下来，推导的方式我现在看到的有两种：一种是严恭敏老师在捷联惯导算法与组合导航算法中的推导；一种是邓自立老师在信息融合滤波理论及其应用中的推导。严老师的推导过程便于理解，但是推导过程不是特别严谨，邓老师的推导过程严谨，但是过程很难理解，但是理解之后对于后续的学习很有好处，不然你连新息是啥都不知道。

严老师推导过程

由状态方程可知，一步预测状态表达式如下：

（1）

这里是因为系统方差W为零均值的误差，所以有：

$E\left ( w \right )=0$ （2）

则状态一步预测误差如下：

（3）

将状态方程和一步预测表达式带入上式可知：

（4）

由此可得一步预测均方差误差阵：

（5）

我们定义一个名词：新息，新息表达式如下：

$\epsilon \left ( t \right )=y\left ( t \right )-H*x\left ( k/k-1\right )$ （6）

这里可以通过新息来对状态估计值来进行修正，即得到如下式子：

$x\left ( k \right )=x\left ( k/k-1 \right )+K*\epsilon \left ( t \right )$ （7）

由此我们可以得到k时刻的状态估计误差如下：

（8）

状态估计误差均方差阵如下：

（9）

下面就是怎么来求滤波增益K的问题？？我们估计状态的目的肯定是为了使得估计值与真实值之间误差最小，这里的误差最小指的各个状态的均方误差之和最小，即：

（10）

（11）

等价于：

（12）

由此可得：

（13）

求最小可以对上式进行求导获得：

下面给出矩阵求导的两个式子如下：

（14）

利用矩阵求导式子对关于 $K_{k}$ 求偏导可得：

（15）

根据函数极值原理，令其右端等于零可得：

（16）

（17）

将式16带入式9，即可得到：

（18）

由此卡尔曼滤波的五个迭代式子就都导出了，感觉这样推导比较好理解一点，因为严老师在讲义上讲这部分很细，所以以上部分好多都是照搬的，自己理解的部分很少，有不懂的可以多看看严老师讲义。

邓老师书上推导

邓老师推导过程就相对而言更加严谨一点，需要的数学知识也多一点，以下全都是我个人对书上知识的理解。

首先我们要估计一个系统的状态值，我们能够获得的是系统输出的测量值y(t)，即我们要做的就是怎么利用y(t)来估计状态x(t)，我们采用的是线性最小估计，即：

$\hat{x}=A*y+b$ （2.1）

其中A,b都为矩阵，我们需要做的就是怎么获得参数A和b。

首先我们得到期望方差均方差表达式如下：

$J=E\left [ \left ( x-A*y-b\right )^{T}\left ( x-A*y-b \right ) \right ]$ （2.2）

选择A和b极小化J,则有 $\frac{\partial J}{\partial b}=0$ ,则有：

$\frac{\partial J}{\partial x}=-2E\left ( x-b-A*y \right )=0$ （2.3）

则有：

b=E(x)-A*E(y) （2.4）

将式2.4带入式2.2，则有：

这里不妨令：

（2.5）

则有：

（2.6）

置 $\frac{\partial J}{\partial A}=0$ ,则有：

（2.7）

因为：

（2.8）

则有：

（2.9）

由此可得线性最小估计下，状态估计值表达式如下：

（2.10）

射影定理：

我一直想着怎么将这段知识能把射影定理给绕过去，因为射影定理理解起来比较难，但是没办法，还是绕不过去，还是需要了解一些定义。

推论1：（正交性）

（2.11）

证明：

（2.12）

上述式子成立是因为：

（2.13）

由此证明上述推论。

定义：我们称 $x-\hat{x}$ 与y不相关为 $x-\hat{x}$ 与y正交，记 $\left ( x-\hat{x} \right ) \perp y$ ，称为 $\hat{x}$ 为x在y上的投影，记为 $\hat{x}=proj\left ( x|y \right )$ 。示意图如下：