牢记公式,ardupilot EKF2就是纸老虎(三)!

版权声明:本文为博主原创文章,转载请附上博文链接!

三、掀开EKF2的神秘面纱

    EKF2是EKF算法在ardupilot上的代码实现。读到这里你也许已经忘了,EKF的5大公式(虽然下面是7个公式,但你将\large \hat{y}_{k}\large S_{k}带入公式后,就变成5个公式了)是什么了,我们先回顾下,维基百科给出的公式(公式前面的名词由谷歌翻译给出,也许有些不准确,但我目前不知道更加准确的词是什么):

牢记公式,ardupilot EKF2就是纸老虎(三)!_第1张图片

    状态转移模型和观测模型如下:

        

        

    上面这几个公式,一定要烂熟于心,下面给出EKF2使用的公式(这个几个公式是我读了GenerateNavFilterEquations.m后总结出来的):

    预测

    预测状态估计:                                         \large \hat{x}_{k\mid k-1}=f(\hat{x}_{k-1\mid k-1},u_{k},w_{k})

    预测协方差估计:                                     \large P_{k\mid k-1}=F_{k}P_{k-1\mid k-1}F^{T}_{k-1}+L_{k-1}Q_{k-1}L^{T}_{k-1}

    更新

    创新或计算剩余                                         

    创新(或残差)协方差                              \large S_{k}=H_{k}P_{k\mid k-1}H^{T}_{k}+M_{k}R_{k}M^{T}_{k}

    接近最优的卡尔曼增益                              \large K_{k}=P_{k\mid k-1}H^{T}_{k}S^{-1}_{k}

    更新状态估计                                             \large \hat{x}_{k\mid k}=\hat{x}_{k\mid k-1}+K_{k}\hat{y}_{k}

    更新的协方差估计                                      \large P_{k\mid k}=(I-K_{k}H_{k})P_{k\mid k-1}

    状态转移矩阵和观测矩阵被定义为以下雅克比行列式:

    \large F_{k}=\frac{\partial f}{\partial x}\mid _{\hat{x}_{k-1\mid k-1},u_{k}}

    \large H_{k}=\frac{\partial h}{\partial x}\mid _{\hat{x}_{k\mid k-1}}

    矩阵\large L_{k-1}^{}和被定义为以下雅克比行列式:

    \large L_{k-1}=\frac{\partial f}{\partial w}\mid _{\hat{x}_{k-1\mid k-1},u_{k}}

    \large M_{k}=\frac{\partial h}{\partial v}\mid _{\hat{x}_{k\mid k-1}}

    状态转移模型和观测模型如下:

    \large x_{k}=f(x_{k-1},u_{k-1},w_{k-1})

    \large \large z_{k} = h(x_{k})+v_{k}

    上述的13个公式,就是EKF2所使用的公式,这13个公式在维基百科中有个学名叫非加性噪声公式和方程。单看这个学名有点懵,实际他们的区别就在于状态转移模型和观测模型中噪声所在的位置。因为噪声不能直接加在\large f(x_{k-1})\large f(x_{k})后面,只能写成\large x_{k}=f(x_{k-1},u_{k-1},w_{k-1})\large z_{k}=f(x_{k},v_{k})的形式,所以叫做非加性噪声公式和方程。实际上\large x_{k}=f(x_{k-1},u_{k-1},w_{k-1})\large z_{k}=f(x_{k},v_{k})是方程\large x_{k}=f(x_{k-1},u_{k-1})+w_{k-1}\large z_{k}=f(x_{k})+v_{k}的一种更加通用的表示方式。由于状态转移模型和观测模型的不同,EKF2锁使用的方程中,多出了矩阵\large L\large M。这两个叫什么我目前也不清楚。

    在EKF2实际的应用中,过程噪声矩阵\large w中有些元素是非加性的,有些不是。观测噪声矩阵\large M中的元素都是加性的,所以上面的公式可以化简为如下公式:

    预测

    预测状态估计:                                         \large \hat{x}_{k\mid k-1}=f(\hat{x}_{k-1\mid k-1},u_{k},w_{k})

    预测协方差估计:                                     \large P_{k\mid k-1}=F_{k}P_{k-1\mid k-1}F^{T}_{k-1}+L_{k-1}Q_{k-1}L^{T}_{k-1}

    更新

    创新或计算剩余                                         

    创新(或残差)协方差                              \large S_{k}=H_{k}P_{k\mid k-1}H^{T}_{k}+R_{k}

    接近最优的卡尔曼增益                              \large K_{k}=P_{k\mid k-1}H^{T}_{k}S^{-1}_{k}

    更新状态估计                                             \large \hat{x}_{k\mid k}=\hat{x}_{k\mid k-1}+K_{k}\hat{y}_{k}

    更新的协方差估计                                      \large P_{k\mid k}=(I-K_{k}H_{k})P_{k\mid k-1}

    状态转移矩阵和观测矩阵被定义为以下雅克比行列式:

    \large F_{k}=\frac{\partial f}{\partial x}\mid _{\hat{x}_{k-1\mid k-1},u_{k}}

    \large H_{k}=\frac{\partial h}{\partial x}\mid _{\hat{x}_{k\mid k-1}}

    矩阵\large L_{k-1}^{}被定义为以下雅克比行列式:

    \large L_{k-1}=\frac{\partial f}{\partial w}\mid _{\hat{x}_{k-1\mid k-1},u_{k}}

    状态转移模型和观测模型如下:

    \large x_{k}=f(x_{k-1},u_{k-1},w_{k-1})

    \large \large z_{k} = h(x_{k})+v_{k}

    过程噪声矩阵\large w中有些元素是非加性的,有些是加性的,所以在EKF2的实现中,预测协方差估计方程中的协方差矩阵\large P有些元素加的是\large L_{k-1}Q_{k-1}L^{T}_{k-1}有些加的是\large Q

你可能感兴趣的:(APM,EKF)