扩展卡尔曼滤波

卡尔曼滤波仅适用于线性系统，但是在大部分实际应用中控制对象往往是非线性系统，故而引入扩展卡尔曼滤波算法。

第一部分 数学基础

  • 泰勒展开式：

    f(x)=f(x0)0!+f′(x0)1!(x−x0)+f′′(x0)2!(x−x0)2+⋯+f(n)(x0)n!(x−x0)n+Rn(x)f\left( x \right) =\frac{f\left( x_0 \right)}{0!}+\frac{f'\left( x_0 \right)}{1!}\left( x-x_0 \right) +\frac{f''\left( x_0 \right)}{2!}\left( x-x_0 \right) ^2+\cdots +\frac{f^{\left( n \right)}\left( x_0 \right)}{n!}\left( x-x_0 \right) ^n+R_n\left( x \right)

    Rn(x)=f(n+!)(ε)(n+1)!(x−x0)n+1R_n\left( x \right) =\frac{f^{\left( n+! \right)}\left( \varepsilon \right)}{\left( n+1 \right) !}\left( x-x_0 \right) ^{n+1} 

    1
    @ε是介于x和x0之间的一个数值

第二部分 扩展卡尔曼滤波

  • 假设：

  1. 引入的噪声符合高斯正态分布，但是其经过非线性传递后就不满足高斯正态分布了，所以需要将非线性系统线性化；
  2. 当前时刻状态只和上一时刻状态有关；

第一章 线性卡尔曼滤波方程组

  • 状态预测方程：

    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}

第二章 非线性离散系统的线性化

  • 已知线性系统的模型方程和观测方程为：

    {xn+1=Axn+Bun+wnzn=Hxn+vn\left\{ \begin{array}{l} x_{n+1}=Ax_n+Bu_n+w_n\\ z_n=Hx_n+v_n\\ \end{array} \right.

  • 定义非线性系统的状态预测方程和状态观测方程为：

    {xn=f(xn−1,  un)+wnzn=h(xn−1)+vn\left\{ \begin{array}{l} x_n=f\left( x_{n-1},\,\,u_n \right) +w_n\\ z_n=h\left( x_{n-1} \right) +v_n\\ \end{array} \right.

    • 过程噪声符合正态分布，其协方差矩阵为Q
    • 观测噪声符合正态分布，其协方差矩阵为R

  • 参考线性卡尔曼滤波的推导，仅在协方差方程中考虑噪声的作用，对上式进行化简：

    {xn+1=f(xn,  un)zn=h(xn)\left\{ \begin{array}{l} x_{n+1}=f\left( x_n,\,\,u_n \right)\\ z_n=h\left( x_n \right)\\ \end{array} \right.

  • 在上一次状态观测值处泰勒展开进行线性化：

    xn+1=f(x^n∣n,  un)+∂f(xn,  un)∂xn∣xn=x^n∣n(xn−x^n∣n)+∂f(xn,  un)∂un∣un=u^n∣n(un−u^n∣n)x_{n+1}=f\left( \hat{x}_{n|n},\,\,u_n \right) +\left. \frac{\partial f\left( x_n,\,\,u_n \right)}{\partial x_n} \right|_{x_n=\hat{x}_{n|n}}\left( x_n-\hat{x}_{n|n} \right) +\left. \frac{\partial f\left( x_n,\,\,u_n \right)}{\partial u_n} \right|_{u_n=\hat{u}_{n|n}}\left( u_n-\hat{u}_{n|n} \right)

    A~n=∂f(xn,  un)∂xn∣xn=x^n∣n\widetilde{A}_n=\left. \frac{\partial f\left( x_n,\,\,u_n \right)}{\partial x_n} \right|_{x_n=\hat{x}_{n|n}}

    B~n=∂f(xn,  un)∂un∣un=u^n∣n\widetilde{B}_n=\left. \frac{\partial f\left( x_n,\,\,u_n \right)}{\partial u_n} \right|_{u_n=\hat{u}_{n|n}}

    线性化后状态空间方程：xn+1=A~nxn+B~nun线性化后状态空间方程：x_{n+1}=\widetilde{A}_nx_n+\widetilde{B}_nu_n

    zn=h(x^n∣n)+∂h(x^n∣n)∂xn(xn−x^n∣n)z_n=h\left( \hat{x}_{n|n} \right) +\frac{\partial h\left( \hat{x}_{n|n} \right)}{\partial x_n}\left( x_n-\hat{x}_{n|n} \right)

    H~n=∂h(x^n∣n)∂xn\widetilde{H}_n=\frac{\partial h\left( \hat{x}_{n|n} \right)}{\partial x_n}

    线性化后测量方程：zn=H~nxn线性化后测量方程：z_n=\widetilde{H}_nx_n

  • 考虑采样时间$$\varDelta T$$的影响：

    xn+1=A~nxn+B~nunx_{n+1}=\widetilde{A}_nx_n+\widetilde{B}_nu_n

    x^n+1∣n−x^n∣nΔT=xn+1\frac{\hat{x}_{n+1|n}-\hat{x}_{n|n}}{\varDelta T}=x_{n+1}

    x^n+1∣n−x^n∣nΔT=A~nxn+B~nun\frac{\hat{x}_{n+1|n}-\hat{x}_{n|n}}{\varDelta T}=\widetilde{A}_nx_n+\widetilde{B}_nu_n

    x^n+1∣n=(A~nx^n∣n+B~nun)ΔT+x^n∣n\hat{x}_{n+1|n}=\left( \widetilde{A}_n\hat{x}_{n|n}+\widetilde{B}_nu_n \right) \varDelta T+\hat{x}_{n|n}

    x^n+1∣n=(I+A~nΔT)x^n∣n+B~nunΔT\hat{x}_{n+1|n}=\left( I+\widetilde{A}_n\varDelta T \right) \hat{x}_{n|n}+\widetilde{B}_nu_n\varDelta T

  • 离散化后的系数矩阵为：

    {A~n=I+A~nΔTB~n=B~nΔT\left\{ \begin{array}{l} \widetilde{A}_n=I+\widetilde{A}_n\varDelta T\\ \widetilde{B}_n=\widetilde{B}_n\varDelta T\\ \end{array} \right.

  • 上式中$$\widetilde{A}_n$$和$$\widetilde{H}_n$$为雅可比矩阵：

    雅可比矩阵：[∂f1∂x1∂f1∂x2⋯∂f1∂xn∂f2∂x1⋯⋯⋯⋯⋯⋯⋯∂fn∂x1⋯⋯∂fn∂x]\text{雅可比矩阵：}\left[ \begin{matrix} \frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots& \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \cdots& \cdots& \cdots\\ \cdots& \cdots& \cdots& \cdots\\ \frac{\partial f_n}{\partial x_1}& \cdots& \cdots& \frac{\partial f_n}{\partial x_{}}\\ \end{matrix} \right]

第三章 状态预测方程

非线性状态预测方程(进行状态预测时选取这个)：x^n+1∣n=f(x^n∣n,  un)非线性状态预测方程(进行状态预测时选取这个)：\hat{x}_{n+1|n}=f\left( \hat{x}_{n|n},\,\,u_n \right)

线性化状态预测方程x^n+1∣n=A~nx^n∣n+B~nun线性化状态预测方程\hat{x}_{n+1|n}=\widetilde{A}_n\hat{x}_{n|n}+\widetilde{B}_nu_n

  • 已知线性化后的系统模型为：

    xn+1=f(x^n∣n,  un)+∂f(xn,  un)∂xn∣xn=x^n∣n(xn−x^n∣n)+∂f(xn,  un)∂un∣un=u^n∣n(un−u^n∣n)x_{n+1}=f\left( \hat{x}_{n|n},\,\,u_n \right) +\left. \frac{\partial f\left( x_n,\,\,u_n \right)}{\partial x_n} \right|_{x_n=\hat{x}_{n|n}}\left( x_n-\hat{x}_{n|n} \right) +\left. \frac{\partial f\left( x_n,\,\,u_n \right)}{\partial u_n} \right|_{u_n=\hat{u}_{n|n}}\left( u_n-\hat{u}_{n|n} \right)

  • 当预测结果越准，则$$x_n$$和$$\hat{x}_{n|n}$$越接近，设输入为常数，则有：

    x^n+1∣n=f(x^n∣n,  un)\hat{x}_{n+1|n}=f\left( \hat{x}_{n|n},\,\,u_n \right)

  • 本质上状态预测方程与原先的非线性方程是一样的：直接将数值带进去计算就好了

第四章 协方差预测方程

Pn+1∣n=A~nPn∣nA~nT+QP_{n+1|n}=\widetilde{A}_nP_{n|n}\widetilde{A}_{n}^{T}+Q

第五章 状态测量方程

zn=H~(xn)z_n=\widetilde{H}\left( x_n \right)

第六章 状态更新方程

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

相较于线性卡尔曼滤波而言，扩展卡尔曼滤波将上一次的状态预测值带入到了一个非线性方程里面，注意这里不用代到线性化后的测量方程中

第六章 协方差更新方程

Pn∣n=(I−KnH~n)Pn∣n−1P_{n|n}=\left( I-K_n\widetilde{H}_n \right) P_{n|n-1}

第八章 卡尔曼增益方程

Kn=Pn∣n−1H~nT(H~nPn∣n−1H~nT+Rn)−1K_n=P_{n|n-1}\widetilde{H}_{n}^{T}\left( \widetilde{H}_nP_{n|n-1}\widetilde{H}_{n}^{T}+R_n \right) ^{-1}