卡尔曼滤波

第一部分 数学基础

第一章 概率论回顾

  • 均值：即一组真实数据的平均值，其计算公式为：

    μˉ=1N∑i=1Nxi\bar{\mu}=\frac{1}{N}\sum_{i=1}^N{x_i}

  • 期望：即一组测量数据的平均值，其并不等同于真实数据的均值，因为测量会引入误差：x~f(x)

    x∼f(x)⇒E(x)={1N∑i=1Nxi∑i=1∞xiPi→离散形式∫−∞+∞xf(x)dx→连续形式x\sim f\left( x \right) \Rightarrow E\left( x \right) =\left\{ \begin{array}{l} \frac{1}{N}\sum_{i=1}^N{x_i}\\ \sum_{i=1}^{\infty}{x_i}P_i\rightarrow \text{离散形式}\\ \int\limits_{-\infty}^{+\infty}{xf\left( x \right) dx}\rightarrow \text{连续形式}\\ \end{array} \right.

    (x, y)∼f(x, y)⇒{E(x)=∫−∞+∞∫−∞+∞xf(x, y)dxdyE(y)=∫−∞+∞∫−∞+∞yf(x,  y)dxdy\left( x,\ y \right) \sim f\left( x,\ y \right) \Rightarrow \left\{ \begin{array}{l} E\left( x \right) =\int_{-\infty}^{+\infty}{\int_{-\infty}^{+\infty}{xf\left( x,\ y \right) dxdy}}\\ E\left( y \right) =\int_{-\infty}^{+\infty}{\int_{-\infty}^{+\infty}{yf\left( x,\,\,y \right) dxdy}}\\ \end{array} \right.

    • 常数的期望还是常数：$$E©=C$$

    • X是一个随机变量，C是常数：$$E(CX)=CE(X)$$

    • X，Y是两个随机变量，则$$E(X+Y)=E(X)+E(Y)$$

    • X，Y是两个对立变量，则$$E(XY)=E(X)E(Y)$$

  • 方差：用于衡量数据与均值的偏离程度：

    σ2=1N∑i=1N(xi−μ)2\sigma ^2=\frac{1}{N}\sum_{i=1}^N{\left( x_i-\mu \right) ^2}

    • 定义：设X是一个随机变量，若$$E{[X-E(X)]^2}$$存在，则称其为X的方差，即为$$D(X)$$

    • 计算公式：

      D(X)=E(x2)−E(x)2D\left( X \right) =E\left( x^2 \right) -E\left( x \right) ^2

    • 常数的方差是0，$$D©=0$$，$$D(C+X)=D(X-C)=D(X)$$

    • D(CX)=C2D(X)D(CX)=C^2D(X)

    • D(X+Y)=D(X)+D(Y)+2COV(X,Y)D(X+Y)=D(X)+D(Y)+2COV(X,Y)

    • D(X−Y)=D(X)+D(Y)−2COV(X,Y)D(X-Y)=D(X)+D(Y)-2COV(X,Y)

  • 标准差：即为方差的平方根：

    σ=1N∑i=1N(xi−μ)2\sigma =\sqrt{\frac{1}{N}\sum_{i=1}^N{\left( x_i-\mu \right) ^2}}

  • 协方差：有两个随机变量X、Y，其协方差可通过下面的公式进行计算：

    COV(X,Y)=E{[X−E(X)][Y−E(Y)]}COV\left( X,Y \right) =E\left\{ \left[ X-E\left( X \right) \right] \left[ Y-E\left( Y \right) \right] \right\}

    COV(X, Y)=E(XY)−μxμyCOV\left( X,\ Y \right) =E\left( XY \right) -\mu _x\mu _y

    • 如果随机变量X、Y为独立变量，那么其两者的协方差计算值为0

    • COV(X,X)=D(X)COV(X,X)=D(X)

    • COV(X,Y)=COV(Y,X)COV(X,Y)=COV(Y,X)

    • COV(aX,bY)=abCOV(X,Y)COV(aX,bY)=abCOV(X,Y)

    • COV(X1+X2,bY)=COV(X1,Y)+COV(X2,Y)COV(X_1+X_2,bY)=COV(X_1,Y)+COV(X_2,Y)

  • 相关系数的定义：

    ρXY=COV(X, Y)D(X)D(Y)\rho _{XY}=\frac{COV\left( X,\ Y \right)}{\sqrt{D\left( X \right)}\sqrt{D\left( Y \right)}}

  • 高斯正态分布：

    f(x, μ, σ2)=12πσ2e−(x−μ)22σ2f\left( x,\ \mu ,\ \sigma ^2 \right) =\frac{1}{\sqrt{2\pi \sigma ^2}}e^{\frac{-\left( x-\mu \right) ^2}{2\sigma ^2}}

第二章 基本定义

  • 准确度：表示传感器测量值与真实值的接近程度；

  • 精确度：表示传感器对一个参数进行测量时，其测量值的波动大小；

  • 静态系统

第二部分 一维卡尔曼滤波

  • 一维线性卡尔曼滤波假设条件：

  1. 引入的噪声符合高斯正态分布；
  2. 当前时刻状态只和上一时刻状态有关；
  3. 模型和系统均满足线性关系；

第一章 状态更新方程

第一节 应用场景

  • 假设：

  1. 传感器待测参数为静态值，其不随时间的变化而变化；
  2. 传感器测量偏差较小，但测量引入了随机噪声；

  • 均值法：一个很容易理解的方法就是根据所有的测量值求均值即可削减噪声的影响。但是在实际应用中，每次获得传感器数据后求取均值操作的运算量和所占内存都很多，因此不太实用；

    μˉ=1N∑i=1Nxi\bar{\mu}=\frac{1}{N}\sum_{i=1}^N{x_i}

  • 卡尔曼状态估计方程：为了利于编程将均值法改为递推法求取，以减少运算量和内存占用量：

第二节 公式基本推导

  • 假设：先验估计值等于估计值

  • 变量定义：

    • x$$：待测参数的真实值；

    • \hat{x}_{n|n}$$：传感器在n时刻测量出$$z_n$$后带入状态更新方程计算出来的n时刻的估计值；

  • 公式推导：

    x^n∣n=1n∑i=1nzi\hat{x}_{n|n}=\frac{1}{n}\sum_{i=1}^n{z_i}

    x^n∣n=1n(∑i=1n−1zi+zn)=1n∑i=1n−1zi+1nzn\hat{x}_{n|n}=\frac{1}{n}\left( \sum_{i=1}^{n-1}{z_i}+z_n \right) =\frac{1}{n}\sum_{i=1}^{n-1}{z_i}+\frac{1}{n}z_n

    x^n∣n=1nn−1n−1∑i=1n−1zi+1nzn=n−1n1n−1∑i=1n−1zi+1nzn\hat{x}_{n|n}=\frac{1}{n}\frac{n-1}{n-1}\sum_{i=1}^{n-1}{z_i}+\frac{1}{n}z_n=\frac{n-1}{n}\frac{1}{n-1}\sum_{i=1}^{n-1}{z_i}+\frac{1}{n}z_n

    x^n∣n=n−1nx^n∣n−1+1nzn=x^n∣n−1−1nx^n∣n−1+1nzn\hat{x}_{n|n}=\frac{n-1}{n}\hat{x}_{n|n-1}+\frac{1}{n}z_n=\hat{x}_{n|n-1}-\frac{1}{n}\hat{x}_{n|n-1}+\frac{1}{n}z_n

    x^n∣n=x^n∣n−1+1n(zn−x^n∣n−1)\hat{x}_{n|n}=\hat{x}_{n|n-1}+\frac{1}{n}\left( z_n-\hat{x}_{n|n-1} \right)

  • 程序流程图：

第二章 状态预测方程

前一章推导状态更新方程时，假设其为静态系统，即传感器待测参数不随时间发生变化，但是在某些情况下其待测参数是会随着时间的变化而发生改变的，因此需要引入状态外推方程，以根据当前时刻信息来预测下一时刻的信息。

第一节 应用场景

  • 有一个质点沿着某一条直线做匀速直线运动，此系统中存在某一个传感器其可测量出当前时刻下质点的位置信息，周期为Δt，依据描述可建立系统的描述方程组如下所示：

    {xn+1=xn+x˙nΔtx˙n+1=x˙n\left\{ \begin{array}{l} x_{n+1}=x_n+\dot{x}_n\varDelta t\\ \dot{x}_{n+1}=\dot{x}_n\\ \end{array} \right.

第二节 公式推导

  • 状态预测方程：即根据当前时刻的信息与系统的数学模型来外推下一时刻的输出。

  • 状态更新方程：由于传感器测量存在噪声，因此需要在状态更新方程中引入调整因子来衡量传感器测量和模型预算的所占比例，以不断迭代计算出准确的系统状态：

    x^n∣n=x^n∣n−1+Keq(zn−x^n∣n−1)\hat{x}_{n|n}=\hat{x}_{n|n-1}+K_{eq}\left( z_n-\hat{x}_{n|n-1} \right)

    当前时刻的估计值=前一时刻对当前时刻的先验估计值+卡尔曼增益(当前时刻测量值−前一时刻对当前时刻的先验估计值)\text{当前时刻的估计值}=\text{前一时刻对当前时刻的先验估计值}+\text{卡尔曼增益}\left( \text{当前时刻测量值}-\text{前一时刻对当前时刻的先验估计值} \right)

    • 测量残差：即当前时刻测量值与上一时刻对当前时刻的先验估计值做差，其包含了新的信息。

    • 第一次估计：由于采用的是地推方法，所以在第一次计算时其前一时刻的先验估计值可根据模型信息或前置定义来确定，也可以给任意值；

  • 卡尔曼滤波将测量、当前状态测量、下一时刻状态估计视为正态分布的随机变量，并将随机变量用均值和方差描述。

  • 本质上是一中最优滤波器，其将先验状态估计与测量结合起来，以最小化当前状态估计的不确定性。

以静态系统且不考虑过程噪声为例，依据上述给出的状态更新方程，即可估计出参数值，随着测量次数的不断增加，估计值逐渐收敛于真实值；

  • 测量误差：传感器测量误差可以使用反差进行描述，其值可由传感器供应商提供，其描述了测量的不确定性；

  • 估计不确定性：尽管随着测量次数的增加，估计值不断收敛于测量值，但是其与真实值之间的误差无法求取，但是可以使用一个概率值P以描述估计的不确定性。

  • 外推估计的不确定性：在每次估计值时需要通过系统模型外推下一时刻的系统状态值，因此估计的不确定存在传递现象。

  • 引入测量误差和估计不确定性后卡尔曼滤波状态更新方程：w1为权重因子

    x^n∣n=w1x^n∣n−1+(1−w1)x^n∣n−1\hat{x}_{n|n}=w_1\hat{x}_{n|n-1}+\left( 1-w_1 \right) \hat{x}_{n|n-1}

    Pn∣n=w12rn+(1−w1)2Pn∣n−1P_{n|n}=w_1^2r_n+\left( 1-w_1 \right) ^2P_{n|n-1} 

    1
    2
    3
    @Pn|n：第n次估计的不确定性(方差)
    @rn：传感器的测量方差
    @Pn|n-1：由n-1时刻估计出的估计的当前时刻估计不确定性值(方差)

  • 卡尔曼滤波要使其估计不确定最小，可对其求导：

    dPn∣ndw1=2w1rn+2(1−w1)Pn∣n−1=0\frac{dP_{n|n}}{dw_1}=2w_1r_n+2\left( 1-w_1 \right) P_{n|n-1}=0

    w1=Pn∣n−1Pn∣n−1+rnw_1=\frac{P_{n|n-1}}{P_{n|n-1}+r_n}

第三章 卡尔曼增益方程

  • 卡尔曼滤波要使其估计不确定最小，可对其求导：

    dPn∣ndw1=2w1rn+2(1−w1)Pn∣n−1=0\frac{dP_{n|n}}{dw_1}=2w_1r_n+2\left( 1-w_1 \right) P_{n|n-1}=0

    w1=Pn∣n−1Pn∣n−1+rnw_1=\frac{P_{n|n-1}}{P_{n|n-1}+r_n}

  • 整理卡尔曼状态更新方程为：

    x^n∣n=x^n∣n−1+Kn(zn−x^n∣n−1)\hat{x}_{n|n}=\hat{x}_{n|n-1}+K_{n}\left( z_n-\hat{x}_{n|n-1} \right)

  • 卡尔曼增益计算方程为：

    Kn=Pn∣n−1Pn∣n−1+rnK_n=\frac{P_{n|n-1}}{P_{n|n-1}+r_n}

    Kn=估计不确定性估计不确定性+测量不确定性\text{K}_{\text{n}}=\frac{\text{估计不确定性}}{\text{估计不确定性}+\text{测量不确定性}}

第四章 协方差更新方程

  • 将卡尔曼增益带入上述更新方程内部，即可得协方差更新方程为：

    Pn∣n=(1−Kn)Pn∣n−1P_{n|n}=\left( 1-K_n \right) P_{n|n-1}

第五章 协方差预测方程

  • 预测方程与系统模型相关，以第二章示例进行说明，其协方差外推方程为：

    {Pn+1∣nx=Pn∣nx+Δt2Pn∣nvPn+1∣nv=Pn∣nv\left\{ \begin{array}{l} P_{n+1|n}^{x}=P_{n|n}^{x}+\varDelta t^2P_{n|n}^{v}\\ P_{n+1|n}^{v}=P_{n|n}^{v}\\ \end{array} \right.

  • 综上可得不带过程噪声的卡尔曼滤波方程流程图如下所示：

第六章 过程噪声

前面五章已经推导出来一维卡尔曼滤波的全部方程，但是其假设条件中假设整个系统不存在过程噪声，即系统的描述方程与实际应用并不是完全对应的。

  • 过程噪声：以方差的形式进行标识，字母q，需要引入到协方差预测方程中，其放在系统的最高阶预测方程中。

    {Pn+1∣nx=Pn∣nx+Δt2Pn∣nvPn+1∣nv=Pn∣nv+qn\left\{ \begin{array}{l} P_{n+1|n}^{x}=P_{n|n}^{x}+\varDelta t^2P_{n|n}^{v}\\ P_{n+1|n}^{v}=P_{n|n}^{v}+q_n\\ \end{array} \right.

第三部分 线性卡尔曼滤波

  • 线性卡尔曼滤波假设条件：

  1. 引入的噪声符合高斯正态分布；

  2. 当前时刻状态只和上一时刻状态有关；

  3. 模型和系统均满足线性关系；

  4. 控制系统的状态空间表达式为：

    {x˙=Ax+Buy=Cx+Du\left\{ \begin{array}{l} \dot{x}=Ax+Bu\\ y=Cx+Du\\ \end{array} \right.

第一章 状态预测方程

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

1
2
3
4
5
@A：状态转移矩阵
@B：输入转移矩阵
@wn：过程噪声
@x：系统状态向量
@u：系统输入向量

  • 对于无输入系统而言，其系统方程为：

    x˙=Ax\dot{x}=Ax

  • 设两次测量时间间隔为：$$ \varDelta t$$，则有：

    dxdt=Ax\frac{dx}{dt}=Ax

    dxx=Adt\frac{dx}{x}=Adt

    ∫xnxn+11xdx=∫nΔt(n+1)ΔtAdt\int_{x_n}^{x_{n+1}}{\frac{1}{x}dx}=\int_{n\varDelta t}^{\left( n+1 \right) \varDelta t}{Adt}

    ln⁡(xn+1)−ln⁡(xn)=AΔt\ln \left( x_{n+1} \right) -\ln \left( x_n \right) =A\varDelta t

    xn+1=xneAΔtx_{n+1}=x_ne^{A\varDelta t}

    F=eAΔt=I+AΔt+(AΔt)22!+(AΔt)33!+⋯F=e^{A\varDelta t}=I+A\varDelta t+\frac{\left( A\varDelta t \right) ^2}{2!}+\frac{\left( A\varDelta t \right) ^3}{3!}+\cdots

  • 考虑有输入系统，其系统方程为：

    x˙=Ax+Bu\dot{x}=Ax+Bu

    xn+1=xneAΔt+∫nΔt(n+1)ΔteAtdtBu(t)x_{n+1}=x_ne^{A\varDelta t}+\int_{n\varDelta t}^{\left( n+1 \right) \varDelta t}{e^{At}dtBu\left( t \right)}

    {F=eAΔt=I+AΔt+(AΔt)22!+(AΔt)33!+⋯G=B∫nΔt(n+1)ΔteAtdt=Δt(I+AΔt2!+(AΔt)23!+⋯ )\left\{ \begin{array}{l} F=e^{A\varDelta t}=I+A\varDelta t+\frac{\left( A\varDelta t \right) ^2}{2!}+\frac{\left( A\varDelta t \right) ^3}{3!}+\cdots\\ G=B\int_{n\varDelta t}^{\left( n+1 \right) \varDelta t}{e^{At}dt}=\varDelta t\left( I+\frac{A\varDelta t}{2!}+\frac{\left( A\varDelta t \right) ^2}{3!}+\cdots \right)\\ \end{array} \right.

第二章 协方差预测方程

Pn+1∣n=FPn∣nFT+QP_{n+1|n}=FP_{n|n}F^T+Q 

1
2
3
@P：协方差矩阵
@F：状态转移矩阵
@Q：过程噪声矩阵

  • 设状态向量x有n个元素，则有状态向量x的协方差矩阵为：

    COV(x⃗)=E([D(x1)COV(x1,x2)⋯COV(x1,xn)COV(x2,x2)D(x2)⋯COV(x2,xn)⋯⋯⋯⋯COV(xn,x1)COV(xn,x2)⋯D(xn)])COV\left( \vec{x} \right) =E\left( \left[ \begin{matrix} D\left( x_1 \right)& COV\left( x_1,x_2 \right)& \cdots& COV\left( x_1,x_n \right)\\ COV\left( x_2,x_2 \right)& D\left( x_2 \right)& \cdots& COV\left( x_2,x_n \right)\\ \cdots& \cdots& \cdots& \cdots\\ COV\left( x_n,x_1 \right)& COV\left( x_n,x_2 \right)& \cdots& D\left( x_n \right)\\ \end{matrix} \right] \right)

  • 不考虑过程噪声：

    • 已知协方差状态转移矩阵计算公式如下：

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

    • 则可得：

      Pn+1∣n=E((x^n+1,n−μxn+1,n)(x^n+1,n−μxn+1,n)T)P_{n+1|n}=E\left( \left( \hat{x}_{n+1,n}-\mu _{x_{n+1,n}} \right) \left( \hat{x}_{n+1,n}-\mu _{x_{n+1,n}} \right) ^T \right)

      Pn+1∣n=E((Fx^n∣n+Gun∣n−Fμxn,n−Gun∣n)(Fx^n∣n+Gun−Fμxn,n−Gun∣n)T)P_{n+1|n}=E\left( \left( F\hat{x}_{n|n}+Gu_{n|n}-F\mu _{x_{n,n}}-Gu_{n|n} \right) \left( F\hat{x}_{n|n}+Gu_n-F\mu _{x_{n,n}}-Gu_{n|n} \right) ^T \right)

      Pn+1∣n=E(F(x^n∣n−μxn,n)(x^n∣n−μxn,n)TFT)P_{n+1|n}=E\left( F\left( \hat{x}_{n|n}-\mu _{x_{n,n}} \right) \left( \hat{x}_{n|n}-\mu _{x_{n,n}} \right) ^TF^T \right)

      Pn+1∣n=FE((x^n∣n−μxn,n)(x^n∣n−μxn,n)T)FTP_{n+1|n}=FE\left( \left( \hat{x}_{n|n}-\mu _{x_{n,n}} \right) \left( \hat{x}_{n|n}-\mu _{x_{n,n}} \right) ^T \right) F^T

      Pn+1∣n=FPn∣nFTP_{n+1|n}=FP_{n|n}F^T

  • 考虑过程噪声：

    • 一般在状态预测方程中并不考虑过程噪声的作用，而在协方差预测方程中进行考虑(直接加进去)，其以噪声矩阵Q的形式来表示：

      Qn=E(wnwnT)Q_n=E\left( w_nw_{n}^{T} \right)

      Q=[q110⋯00q22⋯0⋯⋯⋯⋯00⋯qnn]Q=\left[ \begin{matrix} q_{11}& 0& \cdots& 0\\ 0& q_{22}& \cdots& 0\\ \cdots& \cdots& \cdots& \cdots\\ 0& 0& \cdots& q_{nn}\\ \end{matrix} \right] 

      1
      @每个状态变量的过程噪声可以是独立的，其取决于状态变量之间是否存在一定的作用关系。若状态变量之间互相独立则其仅有主对角线上有元素。

第三章 状态测量方程

zn=Hxn+vnz_n=Hx_n+v_n 

1
2
3
4
@Zn：观测向量
@H：观测矩阵
@xn：真实系统状态
@vn：随机噪声向量

  • 有的时候状态向量可能并不能直接用传感器设备测量出来，其一般通过传感器测量其他物理量并通过观察矩阵以实现物理量之间的耦合表示。

  • 随机噪声向量是不可知的，因此一般不在这考虑噪声而在协方差矩阵方程中考虑噪声的不确定性。

  • 测量不确定性：

    Rn=E(vnvnT)R_n=E\left( v_nv_{n}^{T} \right)

第四章 状态更新方程

x^n∣n=x^n∣n−1+Kn(zn−Hx^n∣n−1)\hat{x}_{n|n}=\hat{x}_{n|n-1}+K_n\left( z_n-H\hat{x}_{n|n-1} \right) 

1
2
3
4
5
@xnn：当前时刻状态值的估计值
@xnn-1：前一时刻对当前时刻状态值的预测
@Kn：卡尔曼增益
@zn：测量值
@H：观测矩阵

第五章 协方差更新方程

Pn∣n=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-K_nH \right) ^T+K_nR_nK_{n}^{T} 

1
2
3
4
@Pnn：协方差矩阵
@Kn：卡尔曼增益
@H：观察矩阵
@Rn：测量不确定性

  • 公式推导：

    • 定义状态误差为状态真值与状态估计值的差：

      en=xn−x^n∣ne_n=x_n-\hat{x}_{n|n}

    en=xn−x^n∣n−1−Kn(zn−Hx^n∣n−1)e_n=x_n-\hat{x}_{n|n-1}-K_n\left( z_n-H\hat{x}_{n|n-1} \right)

    en=xn−x^n∣n−1−Kn(Hxn+vn−Hx^n∣n−1)e_n=x_n-\hat{x}_{n|n-1}-K_n\left( Hx_n+v_n-H\hat{x}_{n|n-1} \right)

    en=xn−x^n∣n−1−KnHxn−Knvn+KnHx^n∣n−1e_n=x_n-\hat{x}_{n|n-1}-K_nHx_n-K_nv_n+K_nH\hat{x}_{n|n-1}

    en=xn−x^n∣n−1−KnH(xn−x^n∣n−1)−Knvne_n=x_n-\hat{x}_{n|n-1}-K_nH\left( x_n-\hat{x}_{n|n-1} \right) -K_nv_n

    en=(I−KnH)(xn−x^n∣n−1)−Knvne_n=\left( I-K_nH \right) \left( x_n-\hat{x}_{n|n-1} \right) -K_nv_n

    Pn∣n=E(enenT)=E((xn−x^n∣n)(xn−x^n∣n)T)P_{n|n}=E\left( e_ne_{n}^{T} \right) =E\left( \left( x_n-\hat{x}_{n|n} \right) \left( x_n-\hat{x}_{n|n} \right) ^T \right)

    Pn∣n=E(((I−KnH)(xn−x^n∣n−1)−Knvn)((I−KnH)(xn−x^n∣n−1)−Knvn)T)P_{n|n}=E\left( \left( \left( I-K_nH \right) \left( x_n-\hat{x}_{n|n-1} \right) -K_nv_n \right) \left( \left( I-K_nH \right) \left( x_n-\hat{x}_{n|n-1} \right) -K_nv_n \right) ^T \right)

    Pn∣n=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)P_{n|n}=E\left( \left( I-K_nH \right) \left( x_n-\hat{x}_{n|n-1} \right) \left( x_n-\hat{x}_{n|n-1} \right) ^T\left( I-K_nH \right) ^T \right) -E\left( \left( I-K_nH \right) \left( x_n-\hat{x}_{n|n-1} \right) \left( K_nv_n \right) ^T \right) -E\left( K_nv_n\left( x_n-\hat{x}_{n|n-1} \right) ^T\left( I-K_nH \right) ^T \right) +E\left( K_nv_n\left( K_nv_n \right) ^T \right)

    • 由于变量$$ x_n-\hat{x}_{n|n}$$与变量观测噪声$$ v_n$$不相关，所以期望为0：

    Pn∣n=E((I−KnH)(xn−x^n∣n−1)(xn−x^n∣n−1)T(I−KnH)T)+E(KnvnvnTKnT)P_{n|n}=E\left( \left( I-K_nH \right) \left( x_n-\hat{x}_{n|n-1} \right) \left( x_n-\hat{x}_{n|n-1} \right) ^T\left( I-K_nH \right) ^T \right) +E\left( K_nv_nv_{n}^{T}K_{n}^{T} \right)

    Pn∣n=(I−KnH)E((xn−x^n∣n−1)(xn−x^n∣n−1)T)(I−KnH)T+KnE(vnvnT)KnTP_{n|n}=\left( I-K_nH \right) E\left( \left( x_n-\hat{x}_{n|n-1} \right) \left( x_n-\hat{x}_{n|n-1} \right) ^T \right) \left( I-K_nH \right) ^T+K_nE\left( v_nv_{n}^{T} \right) K_{n}^{T}

    Pn∣n=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-K_nH \right) ^T+K_nR_nK_{n}^{T}

第六章 卡尔曼增益方程

Kn=Pn∣n−1HT(HPn∣n−1HT+Rn)−1K_n=P_{n|n-1}H^T\left( HP_{n|n-1}H^T+R_n \right) ^{-1} 

1
2
3
4
@Kn：卡尔曼滤波增益
@H：观察矩阵
@Pnn-1：状态的协方差矩阵
@Rn：测量噪声的协方差矩阵

  • 为了最小化当前状态的协方差矩阵最小，那么需要最小化协方差矩阵的主对角线元素最小，由于主对角线元素的和成为方阵的迹，因此需要对迹Pnn求关于Kn的微分：

    Pn∣n=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-K_nH \right) ^T+K_nR_nK_{n}^{T}

    Pn∣n=(I−KnH)Pn∣n−1(I−HTKNT)+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-H^TK_{N}^{T} \right) +K_nR_nK_{n}^{T}

    Pn∣n=(Pn∣n−1−KnHPn∣n−1)(I−HTKNT)+KnRnKnTP_{n|n}=\left( P_{n|n-1}-K_nHP_{n|n-1} \right) \left( I-H^TK_{N}^{T} \right) +K_nR_nK_{n}^{T}

    Pn∣n=Pn∣n−1−Pn∣n−1HTKNT−KnHPn∣n−1+KnHPn∣n−1HTKNT+KnRnKnTP_{n|n}=P_{n|n-1}-P_{n|n-1}H^TK_{N}^{T}-K_nHP_{n|n-1}+K_nHP_{n|n-1}H^TK_{N}^{T}+K_nR_nK_{n}^{T}

    Pn∣n=Pn∣n−1−Pn∣n−1HTKNT−KnHPn∣n−1+Kn(HPn∣n−1HT+Rn)KNTP_{n|n}=P_{n|n-1}-P_{n|n-1}H^TK_{N}^{T}-K_nHP_{n|n-1}+K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T}

  • 协方差矩阵是对阵矩阵，所以其转置等于他本身；

  • 转置矩阵的迹和其矩阵的迹相等；

  • 对上式求迹可得：

    tr(Pn∣n)=tr(Pn∣n−1)−tr(Pn∣n−1HTKNT)−tr(KnHPn∣n−1)+tr(Kn(HPn∣n−1HT+Rn)KNT) tr\left( P_{n|n} \right) =tr\left( P_{n|n-1} \right) -tr\left( P_{n|n-1}H^TK_{N}^{T} \right) -tr\left( K_nHP_{n|n-1} \right) +tr\left( K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T} \right) \

    tr(Pn∣n)=tr(Pn∣n−1)−tr(KnHPn∣n−1)−tr(KnHPn∣n−1)+tr(Kn(HPn∣n−1HT+Rn)KNT)  tr\left( P_{n|n} \right) =tr\left( P_{n|n-1} \right) -tr\left( K_nHP_{n|n-1} \right) -tr\left( K_nHP_{n|n-1} \right) +tr\left( K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T} \right) \,\,

    tr(Pn∣n)=tr(Pn∣n−1)−2tr(KnHPn∣n−1)+tr(Kn(HPn∣n−1HT+Rn)KNT)  tr\left( P_{n|n} \right) =tr\left( P_{n|n-1} \right) -2tr\left( K_nHP_{n|n-1} \right) +tr\left( K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T} \right) \,\,

  • 对上式求微分可得：

    tr(Pn∣n)=tr(Pn∣n−1)−2tr(KnHPn∣n−1)+tr(Kn(HPn∣n−1HT+Rn)KNT)  tr\left( P_{n|n} \right) =tr\left( P_{n|n-1} \right) -2tr\left( K_nHP_{n|n-1} \right) +tr\left( K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T} \right) \,\,

    dtr(Pn∣n)dKn=dtr(Pn∣n−1)dKn−d2tr(KnHPn∣n−1)dKn+dtr(Kn(HPn∣n−1HT+Rn)KNT)  dKn=0\frac{dtr\left( P_{n|n} \right)}{dK_n}=\frac{dtr\left( P_{n|n-1} \right)}{dK_n}-\frac{d2tr\left( K_nHP_{n|n-1} \right)}{dK_n}+\frac{dtr\left( K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T} \right) \,\,}{dK_n}=0

    dtr(Pn∣n)dKn=0−d2tr(KnHPn∣n−1)dKn+dtr(Kn(HPn∣n−1HT+Rn)KNT)  dKn=0\frac{dtr\left( P_{n|n} \right)}{dK_n}=0-\frac{d2tr\left( K_nHP_{n|n-1} \right)}{dK_n}+\frac{dtr\left( K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T} \right) \,\,}{dK_n}=0

  • 引入迹微分性质：

    ddA(tr(AB))=BT\frac{d}{dA}\left( tr\left( AB \right) \right) =B^T

    ddA(tr(ABAT))=2AB\frac{d}{dA}\left( tr\left( ABA^T \right) \right) =2AB

  • 化简可得：

    dtr(Pn∣n)dKn=−2(HPn∣n−1)T+2Kn(HPn∣n−1HT+Rn)=0\frac{dtr\left( P_{n|n} \right)}{dK_n}=-2\left( HP_{n|n-1} \right) ^T+2K_n\left( HP_{n|n-1}H^T+R_n \right) =0

    (HPn∣n−1)T=Kn(HPn∣n−1HT+Rn)\left( HP_{n|n-1} \right) ^T=K_n\left( HP_{n|n-1}H^T+R_n \right)

    Kn=(H)T(HPn∣n−1HT+Rn)−1K_n=\left( H \right) ^T\left( HP_{n|n-1}H^T+R_n \right) ^{-1}

    Kn=Pn∣n−1THT(HPn∣n−1HT+Rn)−1K_n=P_{n|n-1}^{T}H^T\left( HP_{n|n-1}H^T+R_n \right) ^{-1}

    Kn=Pn∣n−1HT(HPn∣n−1HT+Rn)−1K_n=P_{n|n-1}H^T\left( HP_{n|n-1}H^T+R_n \right) ^{-1}

第七章 协方差更新方程的简化版本

未简化：Pn∣n=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnT未简化：P_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-K_nH \right) ^T+K_nR_nK_{n}^{T}

简化后：Pn∣n=(I−KnH)Pn∣n−1简化后：P_{n|n}=\left( I-K_nH \right) P_{n|n-1}

  • 可以将卡尔曼增益方程代入到协方差更新方程中以简化卡尔曼增益方程：

    Pn∣n=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-K_nH \right) ^T+K_nR_nK_{n}^{T}

    Pn∣n=(I−KnH)Pn∣n−1(I−HTKNT)+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-H^TK_{N}^{T} \right) +K_nR_nK_{n}^{T}

    Pn∣n=(Pn∣n−1−KnHPn∣n−1)(I−HTKNT)+KnRnKnTP_{n|n}=\left( P_{n|n-1}-K_nHP_{n|n-1} \right) \left( I-H^TK_{N}^{T} \right) +K_nR_nK_{n}^{T}

    Pn∣n=Pn∣n−1−Pn∣n−1HTKNT−KnHPn∣n−1+KnHPn∣n−1HTKNT+KnRnKnTP_{n|n}=P_{n|n-1}-P_{n|n-1}H^TK_{N}^{T}-K_nHP_{n|n-1}+K_nHP_{n|n-1}H^TK_{N}^{T}+K_nR_nK_{n}^{T}

    Pn∣n=Pn∣n−1−Pn∣n−1HTKNT−KnHPn∣n−1+Kn(HPn∣n−1HT+Rn)KNTP_{n|n}=P_{n|n-1}-P_{n|n-1}H^TK_{N}^{T}-K_nHP_{n|n-1}+K_n\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T}

  • 将卡尔曼增益方程代入可得：

    Pn∣n=Pn∣n−1−Pn∣n−1HTKNT−KnHPn∣n−1+Pn∣n−1HT(HPn∣n−1HT+Rn)−1(HPn∣n−1HT+Rn)KNTP_{n|n}=P_{n|n-1}-P_{n|n-1}H^TK_{N}^{T}-K_nHP_{n|n-1}+P_{n|n-1}H^T\left( HP_{n|n-1}H^T+R_n \right) ^{-1}\left( HP_{n|n-1}H^T+R_n \right) K_{N}^{T}

    Pn∣n=Pn∣n−1−Pn∣n−1HTKNT−KnHPn∣n−1+Pn∣n−1HTKNTP_{n|n}=P_{n|n-1}-P_{n|n-1}H^TK_{N}^{T}-K_nHP_{n|n-1}+P_{n|n-1}H^TK_{N}^{T}

    Pn∣n=Pn∣n−1−KnHPn∣n−1P_{n|n}=P_{n|n-1}-K_nHP_{n|n-1}

    Pn∣n=(I−KnH)Pn∣n−1P_{n|n}=\left( I-K_nH \right) P_{n|n-1}

第八章 卡尔曼滤波整理

  • 六个方程组：

    • 状态预测方程：

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

    • 协方差预测方程：

      Pn+1∣n=FPn∣nFT+QP_{n+1|n}=FP_{n|n}F^T+Q

    • 状态测量方程：

      zn=Hxn+vnz_n=Hx_n+v_n

    • 状态更新方程：

      x^n∣n=x^n∣n−1+Kn(zn−Hx^n∣n−1)\hat{x}_{n|n}=\hat{x}_{n|n-1}+K_n\left( z_n-H\hat{x}_{n|n-1} \right)

    • 协方差更新方程：

      Pn∣n=(I−KnH)Pn∣n−1(I−KnH)T+KnRnKnTP_{n|n}=\left( I-K_nH \right) P_{n|n-1}\left( I-K_nH \right) ^T+K_nR_nK_{n}^{T}

      Pn∣n=(I−KnH)Pn∣n−1P_{n|n}=\left( I-K_nH \right) P_{n|n-1}

    • 卡尔曼增益方程：

      Kn=Pn∣n−1HT(HPn∣n−1HT+Rn)−1K_n=P_{n|n-1}H^T\left( HP_{n|n-1}H^T+R_n \right) ^{-1}