深入理解卡尔曼滤波器（3）：多维卡尔曼滤波器

声明： 本文图文均来源于https://www.kalmanfilter.net/，如有侵权请联系删除。

本文由微信公众号【DeepDriving】整理，由于全文内容较多所以分成3部分发出来。关注公众号【DeepDriving】，后台回复关键字【卡尔曼滤波器】可获取全文PDF。

多维卡尔曼滤波器

前面介绍了一维卡尔曼滤波器，相信大家已经对卡尔曼滤波器有了一定的认识，但是在实际应用中，我们通常需要处理有多维状态数据的系统。比如，对于一个三维空间中的飞机，我们需要一个9维向量来描述其位置、速度和加速度：

$${\left[\begin{array}{ccccccccc}x& y& z& \dot{x}& \dot{y}& \dot{z}& \ddot{x}& \ddot{y}& \ddot{z}\end{array}\right]}^T$$

假设我们采用恒加速度（constant acceleration）动态模型，那么在$n$时刻飞机的状态可以写为

$$\{\begin{array}{l}{x}_n=x_{n-1}+{\dot{x}}_{n-1}\Delta t+\frac{1}{2}{\ddot{x}}_{n-1}\Delta t^2\\ y_n=y_{n-1}+{\dot{y}}_{n-1}\Delta t+\frac{1}{2}{\ddot{y}}_{n-1}\Delta t^2\\ z_n=z_{n-1}+{\dot{z}}_{n-1}\Delta t+\frac{1}{2}{\ddot{z}}_{n-1}\Delta t^2\\ {\dot{x}}_n={\dot{x}}_{n-1}+{\ddot{x}}_{n-1}\Delta t\\ {\dot{y}}_n={\dot{y}}_{n-1}+{\ddot{y}}_{n-1}\Delta t\\ {\dot{z}}_n={\dot{z}}_{n-1}+{\ddot{z}}_{n-1}\Delta t\\ {\ddot{x}}_n={\ddot{x}}_{n-1}\\ {\ddot{y}}_n={\ddot{y}}_{n-1}\\ {\ddot{z}}_n={\ddot{z}}_{n-1}\end{array}$$

在实际应用中，我们通常会使用矩阵的方式来描述多维数据处理的过程。接下来，我们将用矩阵的方式来介绍多维卡尔曼滤波器的几个方程。

状态外推方程

状态外推方程的作用是在当前时刻$n$基于现有的知识去预测$n+1$时刻系统的状态，所以也叫状态预测方程或者状态转移方程，其矩阵形式的公式如下：

$${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}+\mathbf{G}{\mathbf{u}}_{\mathbf{n}}+{\mathbf{w}}_{\mathbf{n}}$$

其中，${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}$是预测的$n+1$时刻的系统状态向量；${\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}$是估计的$n$时刻的系统状态向量；${\mathbf{u}}_{\mathbf{n}}$是控制变量（输入变量），对系统来说是一个可测量的（确定性的）输入；${\mathbf{w}}_{\mathbf{n}}$是过程噪声，是会影响系统状态的不可测量的输入量；$\mathbf{F}$是状态转移矩阵；$\mathbf{G}$是控制矩阵，也叫输入转移矩阵，用于将控制变量映射为状态变量。

以前面的飞机为例，用状态向量$\widehat{x_n}$描述其在三维空间中的位置、速度和加速度

$${\hat{\mathbf{x}}}_{\mathbf{n}}={\left[\begin{array}{ccccccccc}{\hat{x}}_n& {\hat{y}}_n& {\hat{z}}_n& {\hat{\dot{x}}}_n& {\hat{\dot{y}}}_n& {\hat{\dot{z}}}_n& {\hat{\ddot{x}}}_n& {\hat{\ddot{y}}}_n& {\hat{\ddot{z}}}_n\end{array}\right]}^T$$

假如采用恒加速模型，如果不考虑有控制变量输入，那么状态外推方程为

$${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}$$

状态转移方程为

$$\mathbf{F}=\left[\begin{array}{ccccccccc}1& 0& 0& \Delta t& 0& 0& 0.5\Delta t^2& 0& 0\\ 0& 1& 0& 0& \Delta t& 0& 0& 0.5\Delta t^2& 0\\ 0& 0& 1& 0& 0& \Delta t& 0& 0& 0.5\Delta t^2\\ 0& 0& 0& 1& 0& 0& \Delta t& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& \Delta t& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& \Delta t\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

那么

$$\left[\begin{array}{c}{\hat{x}}_{n+1,n}\\ {\hat{y}}_{n+1,n}\\ {\hat{z}}_{n+1,n}\\ {\hat{\dot{x}}}_{n+1,n}\\ {\hat{\dot{y}}}_{n+1,n}\\ {\hat{\dot{z}}}_{n+1,n}\\ {\hat{\ddot{x}}}_{n+1,n}\\ {\hat{\ddot{y}}}_{n+1,n}\\ {\hat{\ddot{z}}}_{n+1,n}\end{array}\right]=\left[\begin{array}{ccccccccc}1& 0& 0& \Delta t& 0& 0& 0.5\Delta t^2& 0& 0\\ 0& 1& 0& 0& \Delta t& 0& 0& 0.5\Delta t^2& 0\\ 0& 0& 1& 0& 0& \Delta t& 0& 0& 0.5\Delta t^2\\ 0& 0& 0& 1& 0& 0& \Delta t& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& \Delta t& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& \Delta t\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{n,n}\\ {\hat{y}}_{n,n}\\ {\hat{z}}_{n,n}\\ {\hat{\dot{x}}}_{n,n}\\ {\hat{\dot{y}}}_{n,n}\\ {\hat{\dot{z}}}_{n,n}\\ {\hat{\ddot{x}}}_{n,n}\\ {\hat{\ddot{y}}}_{n,n}\\ {\hat{\ddot{z}}}_{n,n}\end{array}\right]$$

算得结果

$$\{\begin{array}{l}{\hat{x}}_{n+1,n}={\hat{x}}_{n,n}+{\hat{\dot{x}}}_{n,n}\Delta t+\frac{1}{2}{\hat{\ddot{x}}}_{n,n}\Delta t^2\\ {\hat{y}}_{n+1,n}={\hat{y}}_{n,n}+{\hat{\dot{y}}}_{n,n}\Delta t+\frac{1}{2}{\hat{\ddot{y}}}_{n,n}\Delta t^2\\ {\hat{z}}_{n+1,n}={\hat{z}}_{n,n}+{\hat{\dot{z}}}_{n,n}\Delta t+\frac{1}{2}{\hat{\ddot{z}}}_{n,n}\Delta t^2\\ {\hat{\dot{x}}}_{n+1,n}={\hat{\dot{x}}}_{n,n}+{\hat{\ddot{x}}}_{n,n}\Delta t\\ {\hat{\dot{y}}}_{n+1,n}={\hat{\dot{y}}}_{n,n}+{\hat{\ddot{y}}}_{n,n}\Delta t\\ {\hat{\dot{z}}}_{n+1,n}={\hat{\dot{z}}}_{n,n}+{\hat{\ddot{z}}}_{n,n}\Delta t\\ {\hat{\ddot{x}}}_{n+1,n}={\hat{\ddot{x}}}_{n,n}\\ {\hat{\ddot{y}}}_{n+1,n}={\hat{\ddot{y}}}_{n,n}\\ {\hat{\ddot{z}}}_{n+1,n}={\hat{\ddot{z}}}_{n,n}\end{array}$$

假如我们有加速度传感器可以提供飞机的加速度信息作为系统的输入，所提供的加速度测量值${\mathbf{u}}_{\mathbf{n}}$为

$${\mathbf{u}}_{\mathbf{n}}=\left[\begin{array}{c}{\ddot{x}}_n\\ {\ddot{y}}_n\\ {\ddot{z}}_n\end{array}\right]$$

那么状态外推方程为

$${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}+\mathbf{G}{\mathbf{u}}_{\mathbf{n},\mathbf{n}}$$

转移矩阵$\mathbf{F}$和控制矩阵$\mathbf{G}$分别如下：

$$\mathbf{F}=\left[\begin{array}{cccccc}1& 0& 0& \Delta t& 0& 0\\ 0& 1& 0& 0& \Delta t& 0\\ 0& 0& 1& 0& 0& \Delta t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$

$$\mathbf{G}=\left[\begin{array}{ccc}0.5\Delta t^2& 0& 0\\ 0& 0.5\Delta t^2& 0\\ 0& 0& 0.5\Delta t^2\\ \Delta t& 0& 0\\ 0& \Delta t& 0\\ 0& 0& \Delta t\end{array}\right]$$

那么

$$\left[\begin{array}{c}{\hat{x}}_{n+1,n}\\ {\hat{y}}_{n+1,n}\\ {\hat{z}}_{n+1,n}\\ {\hat{\dot{x}}}_{n+1,n}\\ {\hat{\dot{y}}}_{n+1,n}\\ {\hat{\dot{z}}}_{n+1,n}\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& \Delta t& 0& 0\\ 0& 1& 0& 0& \Delta t& 0\\ 0& 0& 1& 0& 0& \Delta t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{n,n}\\ {\hat{y}}_{n,n}\\ {\hat{z}}_{n,n}\\ {\hat{\dot{x}}}_{n,n}\\ {\hat{\dot{y}}}_{n,n}\\ {\hat{\dot{z}}}_{n,n}\end{array}\right]+\left[\begin{array}{ccc}0.5\Delta t^2& 0& 0\\ 0& 0.5\Delta t^2& 0\\ 0& 0& 0.5\Delta t^2\\ \Delta t& 0& 0\\ 0& \Delta t& 0\\ 0& 0& \Delta t\end{array}\right]\left[\begin{array}{c}{\ddot{x}}_n\\ {\ddot{y}}_n\\ {\ddot{z}}_n\end{array}\right]$$

协方差外推方程

协方差外推方程如下：

$${\mathbf{P}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\mathbf{P}}_{\mathbf{n},\mathbf{n}}{\mathbf{F}}^{\mathbf{T}}+\mathbf{Q}$$

其中，${\mathbf{P}}_{\mathbf{n},\mathbf{n}}$描述了当前估计值的不确定度，是当前系统状态的协方差矩阵；${\mathbf{P}}_{\mathbf{n}+1,\mathbf{n}}$描述了当前预测值的不确定度，是预测的系统状态的协方差矩阵；$\mathbf{F}$是状态转移矩阵；$\mathbf{Q}$是过程噪声协方差矩阵。

现在我们从头来推导一下这个方程。

假设过程噪声为零（$\mathbf{Q}=0$），那么

$${\mathbf{P}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\mathbf{P}}_{\mathbf{n},\mathbf{n}}{\mathbf{F}}^{\mathbf{T}}$$

由前面的背景知识我们知道系统状态向量$\mathbf{x}$的协方差矩阵为

$$COV(\mathbf{x})=E\left(\left(\mathbf{x}-{\mathbf{\mu }}_{\mathbf{x}}\right){\left(\mathbf{x}-{\mathbf{\mu }}_{\mathbf{x}}\right)}^T\right)$$

因此

$${\mathbf{P}}_{\mathbf{n},\mathbf{n}}=E\left(\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right){\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right)}^T\right)$$

根据状态外推方程

$${\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}=\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}+\mathbf{G}{\hat{\mathbf{u}}}_{\mathbf{n},\mathbf{n}}$$

可得

$$\begin{gathered} {\mathbf{P}}_{\mathbf{n}+1,\mathbf{n}}=E\left(\left({\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n}+1,\mathbf{n}}}\right){\left({\hat{\mathbf{x}}}_{\mathbf{n}+1,\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n}+1,\mathbf{n}}}\right)}^T\right) \\ =E\left(\left(\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}+\mathbf{G}{\hat{\mathbf{u}}}_{\mathbf{n},\mathbf{n}}-\mathbf{F}{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}-\mathbf{G}{\hat{\mathbf{u}}}_{\mathbf{n},\mathbf{n}}\right){\left(\mathbf{F}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}+\mathbf{G}{\hat{\mathbf{u}}}_{\mathbf{n},\mathbf{n}}-\mathbf{F}{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}-\mathbf{G}{\hat{\mathbf{u}}}_{\mathbf{n},\mathbf{n}}\right)}^T\right) \\ =E\left(\mathbf{F}\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right){\left(\mathbf{F}\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right)\right)}^T\right) \\ =E\left(\mathbf{F}\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right){\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right)}^T{\mathbf{F}}^{\mathbf{T}}\right) \\ =\mathbf{F}E\left(\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right){\left({\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}-{\mathbf{\mu }}_{{\mathbf{x}}_{\mathbf{n},\mathbf{n}}}\right)}^T\right){\mathbf{F}}^{\mathbf{T}} \\ =\mathbf{F}{\mathbf{P}}_{\mathbf{n},\mathbf{n}}{\mathbf{F}}^{\mathbf{T}} \end{gathered}$$

该如何构建过程噪声协方差矩阵$\mathbf{Q}$呢？

假设系统的状态为位置、速度和加速度，分别用$x$，$v$和$a$来表示。对于恒速模型来说，过程噪声的协方差矩阵为

$$\mathbf{Q}=\left[\begin{array}{cc}V(x)& COV(x,v)\\ COV(v,x)& V(v)\end{array}\right]$$

我们将用随机加速度方差${\sigma }_a^2$来表示位置、速度的方差和协方差。由前面的背景知识可以知道

$$V(v)={\sigma }_v^2=E\left(v^2\right)-{\mu }_v^2=E\left({\left(a\Delta t\right)}^2\right)-{\left({\mu }_a\Delta t\right)}^2=\Delta t^2\left(E\left(a^2\right)-{\mu }_a^2\right)=\Delta t^2{\sigma }_a^2$$

$$V(x)={\sigma }_x^2=E\left(x^2\right)-{\mu }_x^2=E\left({\left(\frac{1}{2}a\Delta t^2\right)}^2\right)-{\left(\frac{1}{2}{\mu }_a\Delta t^2\right)}^2=\frac{\Delta t^4}{4}\left(E\left(a^2\right)-{\mu }_a^2\right)=\frac{\Delta t^4}{4}{\sigma }_a^2$$

$$COV(x,v)=COV(v,x)=E\left(xv\right)-{\mu }_x{\mu }_v=E\left(\frac{1}{2}a\Delta t^{2}a\Delta t\right)-\left(\frac{1}{2}{\mu }_a\Delta t^2{\mu }_a\Delta t\right)=\frac{\Delta t^3}{2}\left(E\left(a^2\right)-{\mu }_a^2\right)=\frac{\Delta t^3}{2}{\sigma }_a^2$$

那么

$$\mathbf{Q}={\sigma }_a^2\left[\begin{array}{cc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2\end{array}\right]$$

这样求矩阵$\mathbf{Q}$比较麻烦，我们可以通过下面的方式快速求出来。

如果动态模型不包含控制输入，那么我们可以直接通过状态转移矩阵将加速度的随机方差${\sigma }_a^2$映射到动态模型中。定义矩阵${\mathbf{Q}}_a$为:

$${\mathbf{Q}}_a=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right]{\sigma }_a^2$$

那么过程噪声协方差矩阵$\mathbf{Q}$可由下面的公式得到

$$\mathbf{Q}=\mathbf{F}{\mathbf{Q}}_a{\mathbf{F}}^{\mathbf{T}}$$

由运动模型可知状态转移矩阵为

$$\mathbf{F}=\left[\begin{array}{ccc}1& \Delta t& \frac{\Delta t^2}{2}\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]$$

则

如果动态模型包含控制输入，那么过程噪声协方差矩阵$\mathbf{Q}$可由下面的公式得到：

$$\mathbf{Q}=\mathbf{G}{\sigma }_a^2{\mathbf{G}}^{\mathbf{T}}$$

其中$\mathbf{G}$为控制矩阵（或者叫输入转移矩阵）。由运动模型可知

$$\mathbf{G}=\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]$$

那么

$$\mathbf{Q}=\mathbf{G}{\sigma }_a^2{\mathbf{G}}^{\mathbf{T}}={\sigma }_a^2\mathbf{G}{\mathbf{G}}^{\mathbf{T}}={\sigma }_a^2\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]\left[\begin{array}{cc}\frac{\Delta t^2}{2}& \Delta t\end{array}\right]={\sigma }_a^2\left[\begin{array}{cc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2\end{array}\right]$$

状态更新方程

状态更新方程的表达式如下所示：

$${\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}={\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}+{\mathbf{K}}_{\mathbf{n}}({\mathbf{z}}_{\mathbf{n}}-\mathbf{H}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1})$$

其中${\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}}$是$n$时刻估计的系统状态向量；${\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}$是在$n-1$时刻预测的系统状态向量；${\mathbf{K}}_{\mathbf{n}}$是卡尔曼增益；${\mathbf{z}}_{\mathbf{n}}$是测量值；$H$为观测矩阵。

在前面测量金条重量的例子中我们已经介绍了状态更新方程，在这里就不做更多介绍了。在多维的情况下，我们需要注意的是矩阵的维度。举个例子，假如系统的状态是一个5维的向量，但是只有第1、3、5维是可测量的：

$$\mathbf{x}(\mathbf{n})=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\\ x_5\end{array}\right]\mathbf{z}(\mathbf{n})=\left[\begin{array}{c}{z}_1\\ z_3\\ z_5\end{array}\right]$$

那么观测矩阵应该是一个$3\times 5$的矩阵：

$$\mathbf{H}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right]$$

那么观测残差$\left({\mathbf{z}}_{\mathbf{n}}-\mathbf{H}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)$为

$$\left({\mathbf{z}}_{\mathbf{n}}-\mathbf{H}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)=\left[\begin{array}{c}{z}_1\\ z_3\\ z_5\end{array}\right]-\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{x}}_1\\ {\hat{x}}_2\\ {\hat{x}}_3\\ {\hat{x}}_4\\ {\hat{x}}_5\end{array}\right]=\left[\begin{array}{c}(z_1-{\hat{x}}_1)\\ (z_3-{\hat{x}}_3)\\ (z_5-{\hat{x}}_5)\end{array}\right]$$

此时卡尔曼增益的维度应该是$5\times 3$。

协方差更新方程

协方差更新方程的表达式如下：

$${\mathbf{P}}_{\mathbf{n},\mathbf{n}}=\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right){\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}$$

其中，${\mathbf{P}}_{\mathbf{n},\mathbf{n}}$为当前状态估计值的协方差矩阵；${\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}$是当前状态的先验估计（基于前一个状态的预测）的协方差矩阵；${\mathbf{K}}_{\mathbf{n}}$为卡尔曼增益；$H$为观测矩阵；$R_n$为测量噪声的协方差矩阵。

接下来，我们将对这个公式进行详细推导。推导过程中会用到下面几个公式：

对于每一个估计，估计误差${\mathbf{e}}_{\mathbf{n}}$为

$$\begin{gathered} {\mathbf{e}}_{\mathbf{n}}={\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}} \\ ={\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{K}}_{\mathbf{n}}\left({\mathbf{z}}_{\mathbf{n}}-\mathbf{H}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right) \\ ={\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{x}}_{\mathbf{n}}+{\mathbf{v}}_{\mathbf{n}}-\mathbf{H}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right) \\ ={\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{x}}_{\mathbf{n}}-{\mathbf{K}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}+{\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1} \\ ={\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)-{\mathbf{K}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}} \\ =\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)-{\mathbf{K}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}} \end{gathered}$$

再由上式来推导估计值的协方差矩阵${\mathbf{P}}_{\mathbf{n},\mathbf{n}}$

$({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1})$是先验估计与真实值之间的误差，它与当前时刻的测量噪声${\mathbf{v}}_{\mathbf{n}}$是不相关的。有前面的背景知识我们知道，两个相互独立变量乘积的期望值为零，所以

$$\begin{gathered} E\left(\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right){\left({\mathbf{K}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}\right)}^{\mathbf{T}}\right)=0 \\ E\left({\mathbf{K}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}{\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)}^{\mathbf{T}}{\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}\right)=0 \end{gathered}$$

那么

$$\begin{gathered} {\mathbf{P}}_{\mathbf{n},\mathbf{n}}=E\left(\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right){\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)}^{\mathbf{T}}{\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}\right)+E\left({\mathbf{K}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right) \\ =\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)E\left(\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right){\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)}^{\mathbf{T}}\right){\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}+{\mathbf{K}}_{\mathbf{n}}E\left({\mathbf{v}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}^{\mathbf{T}}\right){\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \end{gathered}$$

由

$$\begin{gathered} E\left(\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right){\left({\mathbf{x}}_{\mathbf{n}}-{\hat{\mathbf{x}}}_{\mathbf{n},\mathbf{n}-1}\right)}^{\mathbf{T}}\right)={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1} \\ E\left({\mathbf{v}}_{\mathbf{n}}{\mathbf{v}}_{\mathbf{n}}^{\mathbf{T}}\right)={\mathbf{R}}_{\mathbf{n}} \end{gathered}$$

可得

$${\mathbf{P}}_{\mathbf{n},\mathbf{n}}=\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right){\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}$$

好了，终于把这个公式推导完了。

卡尔曼增益

卡尔曼增益的表达式如下：

$${\mathbf{K}}_{\mathbf{n}}={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}{\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right)}^{-1}$$

其中，${\mathbf{K}}_{\mathbf{n}}$为卡尔曼增益；${\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}$是当前状态的先验估计（基于前一个状态的预测）的协方差矩阵；$\mathbf{H}$为观测矩阵；${\mathbf{R}}_{\mathbf{n}}$为测量噪声的协方差矩阵。

在开始推导卡尔曼增益之前，让我们先对协方差更新公式再做一些变换：

$$\begin{gathered} {\mathbf{P}}_{\mathbf{n},\mathbf{n}}=\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right){\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \\ =\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right){\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\left(\mathbf{I}-{\left({\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right)}^{\mathbf{T}}\right)+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \\ =\left(\mathbf{I}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}\right){\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\left(\mathbf{I}-{\mathbf{H}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right)+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \\ =\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)\left(\mathbf{I}-{\mathbf{H}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right)+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \\ ={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}+{\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}+{\mathbf{K}}_{\mathbf{n}}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \\ ={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}-{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}-{\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}+{\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right){\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}} \end{gathered}$$

卡尔曼滤波器是最优滤波器，因此我们需要寻求能最小化估计方差的卡尔曼增益。为了最小化估计方差，我们需要最小化状态协方差矩阵${\mathbf{P}}_{\mathbf{n},\mathbf{n}}$的主对角线，也就是最小化它的迹$tr({\mathbf{P}}_{\mathbf{n},\mathbf{n}})$。为了求得$tr({\mathbf{P}}_{\mathbf{n},\mathbf{n}})$的极小值，我们需要求迹$tr({\mathbf{P}}_{\mathbf{n},\mathbf{n}})$关于卡尔曼增益${\mathbf{K}}_{\mathbf{n}}$的导数，并设导数为零。

$$\begin{gathered} tr\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}}\right)=tr\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)-tr\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}{\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right)-tr\left({\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)+tr\left({\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right){\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right) \\ =tr\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)-2tr\left({\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)+tr\left({\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right){\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right) \end{gathered}$$

求导数并令其为零：

$$\begin{gathered} \frac{d\left(tr\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}}\right)\right)}{d{\mathbf{K}}_{\mathbf{n}}}=\frac{d\left(tr\left({\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)\right)}{d{\mathbf{K}}_{\mathbf{n}}}-\frac{d\left(2tr\left({\mathbf{K}}_{\mathbf{n}}\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)\right)}{d{\mathbf{K}}_{\mathbf{n}}}+\frac{d\left(tr\left({\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right){\mathbf{K}}_{\mathbf{n}}^{\mathbf{T}}\right)\right)}{d{\mathbf{K}}_{\mathbf{n}}} \\ =0-2{\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)}^T+2{\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right) \\ =0 \end{gathered}$$

这里用到了两个计算公式：
$$\begin{gathered} \frac{d}{d\mathbf{A}}\left(tr\left(\mathbf{A}\mathbf{B}\right)\right)={\mathbf{B}}^T \\ \frac{d}{d\mathbf{A}}\left(tr\left(\mathbf{A}\mathbf{B}{\mathbf{A}}^{\mathbf{T}}\right)\right)=2\mathbf{A}\mathbf{B} \end{gathered}$$

由上面的导数为零，可得

$${\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)}^T={\mathbf{K}}_{\mathbf{n}}\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right)$$

因此可求得卡尔曼增益${\mathbf{K}}_{\mathbf{n}}$：

$$\begin{gathered} {\mathbf{K}}_{\mathbf{n}}={\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}\right)}^T{\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right)}^{-1} \\ ={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}^{\mathbf{T}}{\mathbf{H}}^{\mathbf{T}}{\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right)}^{-1} \\ ={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}{\left(\mathbf{H}{\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}}\right)}^{-1} \end{gathered}$$

因为协方差矩阵是对称矩阵，所以${\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}^{\mathbf{T}}={\mathbf{P}}_{\mathbf{n},\mathbf{n}-1}$。