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

一天到晚潜水的鱼
268 阅读8分钟

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

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

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

多维卡尔曼滤波器

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

[xyzx˙y˙z˙x¨y¨z¨]T\begin{bmatrix} x & y & z & \dot{x} & \dot{y} & \dot{z} & \ddot{x} & \ddot{y} & \ddot{z} \end{bmatrix}^{T}

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

{xn=xn−1+x˙n−1Δt+12x¨n−1Δt2yn=yn−1+y˙n−1Δt+12y¨n−1Δt2zn=zn−1+z˙n−1Δt+12z¨n−1Δt2x˙n=x˙n−1+x¨n−1Δty˙n=y˙n−1+y¨n−1Δtz˙n=z˙n−1+z¨n−1Δtx¨n=x¨n−1y¨n=y¨n−1z¨n=z¨n−1\begin{cases} 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{cases}

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

状态外推方程

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

x^n+1,n=Fx^n,n+Gun+wn\boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n}+w_{n}}

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

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

x^n=[x^ny^nz^nx˙^ny˙^nz˙^nx¨^ny¨^nz¨^n]T\boldsymbol{\hat{x}_{n}}= \begin{bmatrix} \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{bmatrix}^{T}

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

x^n+1,n=Fx^n,n\boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}}

状态转移方程为

F=[100Δt000.5Δt2000100Δt000.5Δt2000100Δt000.5Δt2000100Δt000000100Δt000000100Δt000000100000000010000000001]\boldsymbol{F}= \left[ \begin{matrix} 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{matrix} \right]

那么

[x^n+1,ny^n+1,nz^n+1,nx˙^n+1,ny˙^n+1,nz˙^n+1,nx¨^n+1,ny¨^n+1,nz¨^n+1,n]=[100Δt000.5Δt2000100Δt000.5Δt2000100Δt000.5Δt2000100Δt000000100Δt000000100Δt000000100000000010000000001][x^n,ny^n,nz^n,nx˙^n,ny˙^n,nz˙^n,nx¨^n,ny¨^n,nz¨^n,n]\left[ \begin{matrix} \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{matrix} \right] = \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} \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{matrix} \right]

算得结果

{x^n+1,n=x^n,n+x˙^n,nΔt+12x¨^n,nΔt2y^n+1,n=y^n,n+y˙^n,nΔt+12y¨^n,nΔt2z^n+1,n=z^n,n+z˙^n,nΔt+12z¨^n,nΔt2x˙^n+1,n=x˙^n,n+x¨^n,nΔty˙^n+1,n=y˙^n,n+y¨^n,nΔtz˙^n+1,n=z˙^n,n+z¨^n,nΔtx¨^n+1,n=x¨^n,ny¨^n+1,n=y¨^n,nz¨^n+1,n=z¨^n,n\begin{cases} \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{cases}

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

un=[x¨ny¨nz¨n]\boldsymbol{u_{n}}= \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right]

那么状态外推方程为

x^n+1,n=Fx^n,n+Gun,n\boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n,n}}

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

F=[100Δt000100Δt000100Δt000100000010000001]\boldsymbol{F}= \left[ \begin{matrix} 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{matrix} \right]
G=[0.5Δt20000.5Δt20000.5Δt2Δt000Δt000Δt]\boldsymbol{G}= \left[ \begin{matrix} 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{matrix} \right]

那么

[x^n+1,ny^n+1,nz^n+1,nx˙^n+1,ny˙^n+1,nz˙^n+1,n]=[100Δt000100Δt000100Δt000100000010000001][x^n,ny^n,nz^n,nx˙^n,ny˙^n,nz˙^n,n]+[0.5Δt20000.5Δt20000.5Δt2Δt000Δt000Δt][x¨ny¨nz¨n]\left[ \begin{matrix} \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{matrix} \right] = \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} \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{matrix} \right] + \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right]

协方差外推方程

协方差外推方程如下：

Pn+1,n=FPn,nFT+Q\boldsymbol{P_{n+1,n} = FP_{n,n}F^{T} + Q}

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

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

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

Pn+1,n=FPn,nFT\boldsymbol{P_{n+1,n} = FP_{n,n}F^{T}}

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

COV(x)=E((x−μx)(x−μx)T)COV(\boldsymbol{x}) = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right)

因此

Pn,n=E((x^n,n−μxn,n)(x^n,n−μxn,n)T)\boldsymbol{P_{n,n}} = E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right)

根据状态外推方程

x^n+1,n=Fx^n,n+Gu^n,n\boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+G\hat{u}_{n,n}}

可得

Pn+1,n=E((x^n+1,n−μxn+1,n)(x^n+1,n−μxn+1,n)T)=E((Fx^n,n+Gu^n,n−Fμxn,n−Gu^n,n)(Fx^n,n+Gu^n,n−Fμxn,n−Gu^n,n)T)=E(F(x^n,n−μxn,n)(F(x^n,n−μxn,n))T)=E(F(x^n,n−μxn,n)(x^n,n−μxn,n)TFT)=FE((x^n,n−μxn,n)(x^n,n−μxn,n)T)FT=FPn,nFT\begin{align*} \boldsymbol{P_{n+1,n}} &= E \left( \left( \boldsymbol{\hat{x}_{n+1,n} - \mu_{x_{n+1,n}}} \right) \left( \boldsymbol{\hat{x}_{n+1,n} - \mu_{x_{n+1,n}}} \right)^{T} \right) \\ &= E \left( \left( \boldsymbol{F\hat{x}_{n,n} + G\hat{u}_{n,n} - F\mu_{x_{n,n}} - G\hat{u}_{n,n}} \right) \left( \boldsymbol{F\hat{x}_{n,n} + G\hat{u}_{n,n} - F\mu_{x_{n,n}} - G\hat{u}_{n,n}} \right)^{T} \right) \\ &= E \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \right)^{T} \right) \\ &= E \left(\boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \boldsymbol{F^{T}} \right) \\ &= \boldsymbol{F} E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \boldsymbol{F^{T}} \\ &= \boldsymbol{F P_{n,n} F^{T}} \end{align*}

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

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

Q=[V(x)COV(x,v)COV(v,x)V(v)]\boldsymbol{Q} = \left[ \begin{matrix} V(x) & COV(x,v) \\ COV(v,x) & V(v) \\ \end{matrix} \right]

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

V(v)=σv2=E(v2)−μv2=E((aΔt)2)−(μaΔt)2=Δt2(E(a2)−μa2)=Δt2σa2V(v) = \sigma^{2}_{v} = 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^{2}_{a}
V(x)=σx2=E(x2)−μx2=E((12aΔt2)2)−(12μaΔt2)2=Δt44(E(a2)−μa2)=Δt44σa2V(x) = \sigma^{2}_{x} = 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^{2}_{a}
COV(x,v)=COV(v,x)=E(xv)−μxμv=E(12aΔt2aΔt)−(12μaΔt2μaΔt)=Δt32(E(a2)−μa2)=Δt32σa2COV(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^{2}_{a}

那么

Q=σa2[Δt44Δt32Δt32Δt2]\boldsymbol{Q} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right]

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

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

Qa=[000000001]σa2\boldsymbol{Q}_{a} = \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \sigma^{2}_{a}

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

Q=FQaFT\boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F^{T}}

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

F=[1ΔtΔt2201Δt001]\boldsymbol{F} = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right]

则

Q=FQaFT=[1ΔtΔt2201Δt001][000000001][100Δt10Δt22Δt1]σa2=[00Δt2200Δt001][100Δt10Δt22Δt1]σa2=[Δt44Δt32Δt22Δt32Δt2ΔtΔt22Δt1]σa2\begin{align*} \boldsymbol{Q} &= \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F^{T}} \\ &= \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \\ &= \left[ \begin{matrix} 0 & 0 & \frac{\Delta t^{2}}{2} \\ 0 & 0 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \\ &= \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} & \frac{\Delta t^{2}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} & \Delta t \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} \end{align*}

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

Q=Gσa2GT\boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}}

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

G=[Δt22Δt]\boldsymbol{G} = \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right]

那么

Q=Gσa2GT=σa2GGT=σa2[Δt22Δt][Δt22Δt]=σa2[Δt44Δt32Δt32Δt2]\boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} = \sigma^{2}_{a}\boldsymbol{G}\boldsymbol{G^{T}} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \frac{\Delta t^{2}}{2} & \Delta t \\ \end{matrix} \right] = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right]

状态更新方程

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

x^n,n=x^n,n−1+Kn(zn−Hx^n,n−1)\boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )}

其中x^n,n\boldsymbol{\hat{x}_{n,n}}是nn时刻估计的系统状态向量；x^n,n−1\boldsymbol{\hat{x}_{n,n-1}}是在n−1n-1时刻预测的系统状态向量；Kn\boldsymbol{K_{n}}是卡尔曼增益；zn\boldsymbol{z_{n}}是测量值；HH为观测矩阵。

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

x(n)=[x1x2x3x4x5]z(n)=[z1z3z5]\boldsymbol{x(n)} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ x_{3}\\ x_{4}\\ x_{5}\\ \end{matrix} \right] \boldsymbol{z(n)} =\left[ \begin{matrix} z_{1}\\ z_{3}\\ z_{5}\\ \end{matrix} \right]

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

H=[100000010000001]\boldsymbol{H}= \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right]

那么观测残差(zn−Hx^n,n−1)\left( \boldsymbol{ z_{n} - H \hat{x}_{n,n-1}} \right)为

(zn−Hx^n,n−1)=[z1z3z5]−[100000010000001][x^1x^2x^3x^4x^5]=[(z1−x^1)(z3−x^3)(z5−x^5)]\left( \boldsymbol{ z_{n} - H \hat{x}_{n,n-1}} \right) = \left[ \begin{matrix} z_{1}\\ z_{3}\\ z_{5}\\ \end{matrix} \right] - \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{1}\\ \hat{x}_{2}\\ \hat{x}_{3}\\ \hat{x}_{4}\\ \hat{x}_{5}\\ \end{matrix} \right] = \left[ \begin{matrix} (z_{1} - \hat{x}_{1})\\ (z_{3} - \hat{x}_{3})\\ (z_{5} - \hat{x}_{5})\\ \end{matrix} \right]

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

协方差更新方程

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

Pn,n=(I−KnH)Pn,n−1(I−KnH)T+KnRnKnT\boldsymbol{ P_{n,n} = \left( I - K_{n}H \right) P_{n,n-1} \left( I - K_{n}H \right)^{T} + K_{n}R_{n}K_{n}^{T} }

其中，Pn,n\boldsymbol{P_{n,n} }为当前状态估计值的协方差矩阵；Pn,n−1\boldsymbol{P_{n,n-1} }是当前状态的先验估计（基于前一个状态的预测）的协方差矩阵；Kn\boldsymbol{K_{n}}为卡尔曼增益；HH为观测矩阵；RnR_{n}为测量噪声的协方差矩阵。

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

方程说明
x^n,n=x^n,n−1+Kn(zn−Hx^n,n−1)\boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )}状态更新方程
zn=Hxn+vn\boldsymbol{z_{n} = Hx_{n} + v_{n}}测量方程，vn\boldsymbol{v_{n}}为测量噪声
Pn,n=E(enenT)=E((xn−x^n,n)(xn−x^n,n)T)\boldsymbol{P_{n,n}} = E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) = E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right)状态协方差矩阵
Rn=E(vnvnT)\boldsymbol{R_{n}} = E\left( \boldsymbol{v_{n}v_{n}^{T}} \right)测量噪声协方差矩阵

对于每一个估计，估计误差en\boldsymbol{e_{n}}为

en=xn−x^n,n=xn−x^n,n−1−Kn(zn−Hx^n,n−1)=xn−x^n,n−1−Kn(Hxn+vn−Hx^n,n−1)=xn−x^n,n−1−KnHxn−Knvn+KnHx^n,n−1=xn−x^n,n−1−KnH(xn−x^n,n−1)−Knvn=(I−KnH)(xn−x^n,n−1)−Knvn\begin{align*} \boldsymbol{e_{n}} &= \boldsymbol{x_{n} - \hat{x}_{n,n}} \\ &= \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n} \left( z_{n} - H \hat{x}_{n,n-1} \right)} \\ &= \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n} \left( Hx_{n} + v_{n} - H \hat{x}_{n,n-1} \right)} \\ &= \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n}Hx_{n} - K_{n}v_{n} + K_{n}H \hat{x}_{n,n-1}} \\ &= \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n}H\left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n}} \\ &= \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n}} \end{align*}

再由上式来推导估计值的协方差矩阵Pn,n\boldsymbol{P_{n,n}}

Pn,n=E(enenT)=E((xn−x^n,n)(xn−x^n,n)T)=E(((I−KnH)(xn−x^n,n−1)−Knvn)×((I−KnH)(xn−x^n,n−1)−Knvn)T)=E(((I−KnH)(xn−x^n,n−1)−Knvn)×(((I−KnH)(xn−x^n,n−1))T−(Knvn)T))=E(((I−KnH)(xn−x^n,n−1)−Knvn)×((xn−x^n,n−1)T(I−KnH)T−(Knvn)T))=E((I−KnH)(xn−x^n,n−1)(xn−x^n,n−1)T(I−KnH)T)−E((I−KnH)(xn−x^n,n−1)(Knvn)T)−E(Knvn(xn−x^n,n−1)T(I−KnH)T)+E(Knvn(Knvn)T)\begin{align*} \begin{split} \boldsymbol{P_{n,n}} &= E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) \\ &= E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right) \\ &= E\left( \boldsymbol{\left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right) \times \left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right)^{T}} \right) \\ &= E\left( \boldsymbol{\left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right) \times \left( \left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \right)^{T} - \left( K_{n}v_{n}\right) ^{T} \right)} \right) \\ &= E\left( \boldsymbol{\left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right) \times \left( \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} - \left( K_{n}v_{n}\right) ^{T} \right)} \right) \\ &= E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) \\ &- {E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( K_{n}v_{n} \right)^{T} }\right)} \\ &- {E \left( \boldsymbol{ K_{n}v_{n} \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right)} \\ &+ E \left( {\boldsymbol{ K_{n}v_{n} \left( K_{n}v_{n} \right)^{T} }}\right) \end{split} \end{align*}

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

E((I−KnH)(xn−x^n,n−1)(Knvn)T)=0E(Knvn(xn−x^n,n−1)T(I−KnH)T)=0{E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( K_{n}v_{n} \right)^{T} }\right) = 0} \\ {E \left( \boldsymbol{ K_{n}v_{n} \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) = 0}

那么

Pn,n=E((I−KnH)(xn−x^n,n−1)(xn−x^n,n−1)T(I−KnH)T)+E(KnvnvnTKnT)=(I−KnH)E((xn−x^n,n−1)(xn−x^n,n−1)T)(I−KnH)T+KnE(vnvnT)KnT\begin{align*} \boldsymbol{P_{n,n}} &= E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) + E \left({\boldsymbol{ K_{n}v_{n} v_{n}^{T} K_{n}^{T} }}\right) \\ &= \boldsymbol{ \left( I - K_{n}H \right)} {E \left( \boldsymbol{ \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} }\right)} \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \boldsymbol{K_{n}} { E \left( \boldsymbol{ v_{n} v_{n}^{T} }\right) } \boldsymbol{ K_{n}^{T} } \\ \end{align*}

由

E((xn−x^n,n−1)(xn−x^n,n−1)T)=Pn,n−1E(vnvnT)=Rn{E \left( \boldsymbol{ \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} }\right) = \boldsymbol{P_{n,n-1}}} \\ { E \left( \boldsymbol{ v_{n} v_{n}^{T} }\right) = \boldsymbol{R_{n}}}

可得

Pn,n=(I−KnH)Pn,n−1(I−KnH)T+KnRnKnT\boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} {\boldsymbol{ P_{n,n-1}} } \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \boldsymbol{K_{n}} { \boldsymbol{ R_{n} } } \boldsymbol{ K_{n}^{T} }

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

卡尔曼增益

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

Kn=Pn,n−1HT(HPn,n−1HT+Rn)−1\boldsymbol{ K_{n} = P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} }

其中，Kn\boldsymbol{K_{n}}为卡尔曼增益；Pn,n−1\boldsymbol{P_{n,n-1} }是当前状态的先验估计（基于前一个状态的预测）的协方差矩阵；H\boldsymbol{H}为观测矩阵；Rn\boldsymbol{R_{n}}为测量噪声的协方差矩阵。

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

Pn,n=(I−KnH)Pn,n−1(I−KnH)T+KnRnKnT=(I−KnH)Pn,n−1(I−(KnH)T)+KnRnKnT=(I−KnH)Pn,n−1(I−HTKnT)+KnRnKnT=(Pn,n−1−KnHPn,n−1)(I−HTKnT)+KnRnKnT=Pn,n−1−Pn,n−1HTKnT−KnHPn,n−1+KnHPn,n−1HTKnT+KnRnKnT=Pn,n−1−Pn,n−1HTKnT−KnHPn,n−1+Kn(HPn,n−1HT+Rn)KnT\begin{align*} \boldsymbol{P_{n,n}} &= \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} {\boldsymbol{ \left( I - K_{n}H \right)^{T}}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ &= \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} {\boldsymbol{ \left( I - \left( K_{n}H \right)^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ &= {\boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}}} {\boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ &= {\boldsymbol{ \left( P_{n,n-1} - K_{n}H P_{n,n-1} \right)}} \boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ &= \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + {\boldsymbol{K_{n}H P_{n,n-1}H^{T}K_{n}^{T} + K_{n} R_{n} K_{n}^{T} } } \\ &= \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + {\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } } \end{align*}

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

tr(Pn,n)=tr(Pn,n−1)−tr(Pn,n−1HTKnT)−tr(KnHPn,n−1)+tr(Kn(HPn,n−1HT+Rn)KnT)=tr(Pn,n−1)−2tr(KnHPn,n−1)+tr(Kn(HPn,n−1HT+Rn)KnT)\begin{align*} tr\left( \boldsymbol{P_{n,n}} \right) &= tr\left( \boldsymbol{P_{n,n-1}}\right) - {tr\left( \boldsymbol{ P_{n,n-1}H^{T}K_{n}^{T} }\right) - tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \\ &= tr\left( \boldsymbol{P_{n,n-1}}\right) - { 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \end{align*}

求导数并令其为零：

d(tr(Pn,n))dKn=d(tr(Pn,n−1))dKn−d(2tr(KnHPn,n−1))dKn+d(tr(Kn(HPn,n−1HT+Rn)KnT))dKn=0−2(HPn,n−1)T+2Kn(HPn,n−1HT+Rn)=0\begin{align*} \frac{d\left( tr\left( \boldsymbol{P_{n,n}} \right) \right)}{d\boldsymbol{K_{n}}} &= {\frac{d\left( tr\left( \boldsymbol{P_{n,n-1}}\right) \right)}{d\boldsymbol{K_{n}}}} - { \frac{d\left( 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right) \right) }{d\boldsymbol{K_{n}}} } + {\frac{ d\left( tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \right) }{d\boldsymbol{K_{n}}}} \\ &= {0} - { 2 \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } + {\boldsymbol{2K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } \\ &= 0 \end{align*}

这里用到了两个计算公式：

{\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{ABA^{T}} \right) \right) = 2\boldsymbol{AB} }$$

由上面的导数为零，可得

(HPn,n−1)T=Kn(HPn,n−1HT+Rn){ \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } = {\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } }

因此可求得卡尔曼增益Kn\boldsymbol{K_{n}}：

Kn=(HPn,n−1)T(HPn,n−1HT+Rn)−1=Pn,n−1THT(HPn,n−1HT+Rn)−1=Pn,n−1HT(HPn,n−1HT+Rn)−1\begin{align*} \boldsymbol{K_{n}} &= \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \\ &= \boldsymbol{ P_{n,n-1}^{T} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \\ &= \boldsymbol{ P_{n,n-1} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \end{align*}

因为协方差矩阵是对称矩阵，所以Pn,n−1T=Pn,n−1\boldsymbol{ P_{n,n-1}^{T}} = \boldsymbol{ P_{n,n-1}}。

简化的协方差更新方程

在很多资料中，协方差更新方程并不是前面我们推导出来的那样，而是将卡尔曼增益公式代入后再化简得到的。

Pn,n=(I−KnH)Pn,n−1(I−KnH)T+KnRnKnT=Pn,n−1−Pn,n−1HTKnT−KnHPn,n−1+Kn(HPn,n−1HT+Rn)KnT=Pn,n−1−Pn,n−1HTKnT−KnHPn,n−1+Pn,n−1HT(HPn,n−1HT+Rn)−1(HPn,n−1HT+Rn)KnT=Pn,n−1−Pn,n−1HTKnT−KnHPn,n−1+Pn,n−1HTKnT=Pn,n−1−KnHPn,n−1=(I−KnH)Pn,n−1\begin{align*} \boldsymbol{P_{n,n}} &= \boldsymbol{ \left( I - K_{n}H \right)} {\boldsymbol{ P_{n,n-1}} } \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \boldsymbol{K_{n}} { \boldsymbol{ R_{n} } } \boldsymbol{ K_{n}^{T} } \\ &= \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + {\boldsymbol{K_{n}}} \boldsymbol{ \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \\ &= \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + {\boldsymbol{P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} }} \boldsymbol{ \left( H P_{n,n-1}H^{T} + R_{n} \right)} \boldsymbol{ K_{n}^{T} } \\ &= \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + \boldsymbol{P_{n,n-1}H^{T} } \boldsymbol{ K_{n}^{T} } \\ &= \boldsymbol{ P_{n,n-1} - K_{n}H P_{n,n-1}} \\ &= \boldsymbol{\left( I - K_{n}H \right)P_{n,n-1}} \end{align*}

化简后的式子显得更加优雅而且方便记忆。然而需要注意的是，即使是计算卡尔曼增益时由于四舍五入引起的一点小误差也可能会导致巨大的计算误差，因为相减项(I−KnH)(I - K_{n} H)会因为浮点数精度误差而导致出现非对称矩阵。因此，这个方程是数值不稳定的！

总结

前面已经用矩阵形式详细推导了卡尔曼滤波器的5个方程，把这5个方程联系到一起可以知道，卡尔曼滤波器执行的操作就是一个不断“预测-更新”的迭代过程，如下图所示：

在初始化之后，卡尔曼滤波器将会预测系统在下一个时刻的状态，同时提供预测的不确定度。在得到测量值之后，卡尔曼滤波器会根据测量值对预测值进行更新（校正）从而估计出一个相对准确的当前状态及其不确定度。然后，卡尔曼滤波器将基于当前状态去预测下一时刻的系统状态，如此循环迭代下去......

下图展示了卡尔曼滤波器在执行”预测-更新“操作过程中所使用的5个方程：

下表对卡尔曼滤波器的5个方程进行了总结：

操作方程方程名
预测x^n+1,n=Fx^n,n+Gun\boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n}}状态外推方程
预测Pn+1,n=FPn,nFT+Q\boldsymbol{P_{n+1,n} = FP_{n,n}F^{T} + Q}协方差外推方程
更新x^n,n=x^n,n−1+Kn(zn−Hx^n,n−1)\boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )}状态更新方程
更新Pn,n=(I−KnH)Pn,n−1(I−KnH)T+KnRnKnT\boldsymbol{ P_{n,n} = \left( I - K_{n}H \right) P_{n,n-1} \left( I - K_{n}H \right)^{T} + K_{n}R_{n}K_{n}^{T} }协方差更新方程
更新Kn=Pn,n−1HT(HPn,n−1HT+Rn)−1\boldsymbol{ K_{n} = P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} }卡尔曼增益

其中

方程说明
zn=Hxn\boldsymbol{z_{n} = Hx_{n}}测量方程
Rn=E(vnvnT)\boldsymbol{R_{n}} = E\left( \boldsymbol{v_{n}v_{n}^{T}} \right)测量噪声协方差矩阵
Qn=E(wnwnT)\boldsymbol{Q_{n}} = E\left( \boldsymbol{w_{n}w_{n}^{T}} \right)过程噪声协方差矩阵
Pn,n=E(enenT)=E((xn−x^n,n)(xn−x^n,n)T)\boldsymbol{P_{n,n}} = E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) = E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right)状态协方差矩阵

下标是对5个方程中所用到的符号的说明：

符号说明
x\boldsymbol{x}状态向量
z\boldsymbol{z}观测向量
F\boldsymbol{F}状态转移矩阵
u\boldsymbol{u}输入向量
G\boldsymbol{G}控制矩阵
P\boldsymbol{P}状态协方差矩阵
Q\boldsymbol{Q}过程噪声协方差矩阵
R\boldsymbol{R}测量噪声协方差矩阵
w\boldsymbol{w}过程噪声向量
v\boldsymbol{v}测量噪声向量
H\boldsymbol{H}观测矩阵
K\boldsymbol{K}卡尔曼增益
n\boldsymbol{n}离散时间索引
avatar
文章
阅读
粉丝