牢记公式，ardupilot EKF2就是纸老虎（三）！

最新推荐文章于 2024-07-15 18:23:06 发布

BreederBai

最新推荐文章于 2024-07-15 18:23:06 发布

阅读量3k

点赞数 3

分类专栏： APM EKF 文章标签： EKF 姿态解算扩展卡尔曼滤波器

本文链接：https://blog.csdn.net/BreederBai/article/details/85292629

版权

APM 同时被 2 个专栏收录

26 篇文章 19 订阅

订阅专栏

EKF

5 篇文章 1 订阅

订阅专栏

三、掀开EKF2的神秘面纱

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

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

${\ displaystyle {\ boldsymbol {x}} _ {k} = f（{\ boldsymbol {x}} _ {k-1}，{\ boldsymbol {u}} _ {k}）+ {\ boldsymbol {w} } _ {K}}$

${\ boldsymbol {z}} _ {{k}} = h（{\ boldsymbol {x}} _ {{k}}）+ {\ boldsymbol {v}} _ {{k}}$

上面这几个公式，一定要烂熟于心，下面给出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}$

更新

创新或计算剩余 ${\ tilde {{\ boldsymbol {y}}}} _ {{k}} = {\ boldsymbol {z}} _ {{k}} - h（{\ hat {{\ boldsymbol {x}}}} _ {{K | 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 M_{k}$ 被定义为以下雅克比行列式：

$\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$ 。这两个叫什么我目前也不清楚。