文章目录

基于误差状态卡尔曼滤波器的四元数运动学7 使用全局角度误差的ESKF7.1 持续时间的系统运动学7.1.1 真实和标称状态运动学7.1.2 误差状态运动学 7.2 离散时间的系统运动学7.2.1 标称状态7.2.2 误差状态7.2.3 误差状态雅可比和扰动矩阵 7.3 与互补的传感数据融合7.3.1 误差状态观测7.3.2 将观测到的误差注入标称状态7.3.3 ESKF重置

基于误差状态卡尔曼滤波器的四元数运动学

7 使用全局角度误差的ESKF

我们将在本章中探讨在全局参考(系)中定义角度误差的含义，这与我们迄今为止使用的局部(参考系)定义相反。我们回顾了第5章和第6章的发展情况，并详细介绍了针对新定义进行更改的章节。

    角速度误差$\delta \mathbf{\theta }$的全局定义，意味着一个左手系的组件，即 $${\mathbf{q}}_t=\delta \mathbf{q}\otimes \mathbf{q}=\mathbf{q}\{\delta \mathbf{\theta }\}\otimes \mathbf{q}.$$

为了完整性起见，我们在连续时间内保持角速率向量$\mathbf{\omega }$的局部定义，即$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }$，因此在离散时间内保持$\mathbf{q}\leftarrow \mathbf{q}\otimes \mathbf{q}\{\mathbf{\omega }\Delta t\}$，而不管角度误差是全局定义的。之所以如此是为了方便起见，因为陀螺仪提供的角速率的测量值在本体框架中，即局部的。

7.1 持续时间的系统运动学

7.1.1 真实和标称状态运动学

真实和标称状态运动学不涉及到误差，它们的方程没有变化。

7.1.2 误差状态运动学

我们从写误差状态运动学方程开始，然后进行评论和证明。 $$\delta \dot{\mathbf{p}}=\delta \mathbf{v}\text{\hspace{1em}(294a)}$$

$$\dot{\delta \mathbf{v}}=-[\mathbf{R}({\mathbf{a}}_m-b{\mathbf{a}}_b){]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-{\mathbf{R}\mathbf{a}}_n\text{\hspace{1em}(294b)}$$

$$\dot{\delta \mathbf{\theta }}=-\mathbf{R}\delta {\mathbf{\omega }}_b-\mathbf{R}{\mathbf{\omega }}_n\text{\hspace{1em}(294c)}$$

$$\delta {\dot{\mathbf{a}}}_b={\mathbf{a}}_w\text{\hspace{1em}(294d)}$$

$$\delta {\dot{\mathbf{\omega }}}_b={\mathbf{\omega }}_w\text{\hspace{1em}(294e)}$$

$$\delta \dot{\mathbf{g}}=0.\text{\hspace{1em}(294f)}$$

【注，在式（294b）中写的是$[\mathbf{R}({\mathbf{a}}_m-b{\mathbf{a}}_b){]}_{\times }$，噪声${\mathbf{a}}_b$多了一个系数$b$，感觉有点突兀，文章中也没有解释$b$的含义，我有点怀疑是作者笔误（因为后面的证明结果式（299）中，并没有这个系数），但查看了其它几个文章版本，都是这么写的，又不像是作者笔误，这里存疑】

其中，再次重申，所有的方程中除了$\dot{\delta \mathbf{v}}$和$\dot{\delta \mathbf{\theta }}$都微不足道，重要的方程将在下面进一步展开。

方程（294b）：线速度误差 我们希望确定速度误差的动力学 $\dot{\delta \mathbf{\theta }}$。我们从如下关系开始

$${\mathbf{R}}_t=(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })\mathbf{R}+O(||\delta \mathbf{\theta }|{|}^2)\text{\hspace{1em}(295)}$$

$$\dot{\mathbf{v}}=\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{g},\text{\hspace{1em}(296)}$$

其中，式（295）是使用全局定义的误差的${\mathbf{R}}_t$的小信号近似值，且在（296）中，我们引入了${\mathbf{a}}_{\mathcal{B}}$和$\delta {\mathbf{a}}_{\mathcal{B}}$作为（240）和（241）中定义的本体系中的大信号和小信号加速度，就像我们对局部定义的情况所做的那样。

    我们继续写${\dot{\mathbf{v}}}_t$的表达式（234b），它以两种不同形式（左右展开）表示，其中项$O(||\delta \mathbf{\theta }|{|}^2)$忽略，

$$\dot{\mathbf{v}}+\delta \mathbf{v}={\dot{\mathbf{v}}}_t=(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })\mathbf{R}({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}})+\mathbf{g}+\delta \mathbf{g}$$

$$\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\dot{\delta \mathbf{v}}={\dot{\mathbf{v}}}_t=\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\delta \mathbf{g}$$

左右两边消掉$\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{g}$项有

$$\dot{\delta \mathbf{v}}=\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}})+\delta \mathbf{g}\text{\hspace{1em}(297)}$$

消掉二阶项并改写叉乘（$[\mathbf{a}{]}_{\times }\mathbf{b}=-[\mathbf{b}{]}_{\times }\mathbf{a}$），我们有

$$\dot{\delta \mathbf{v}}=\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}-[\mathbf{R}{\mathbf{a}}_{\mathcal{B}}{]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{g}\text{\hspace{1em}(298)}$$

最终，将（240）和（241）代入上式，我们得到了速度误差导数的表达式， $$\dot{\delta \mathbf{v}}=-[\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b){]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n\text{\hspace{1em}(299)}$$

方程（294c）：方向误差 我们从导数方程的真实和标称定义开始， $${\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\text{\hspace{1em}(300)}$$

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega },\text{\hspace{1em}(301)}$$

注意，我们使用的是全局定义的角速率误差，即 $${\mathbf{q}}_t=\delta \mathbf{q}\otimes \mathbf{q}.\text{\hspace{1em}(302)}$$

正如我们对局部定义的误差情况所做的那样，我们仍将大信号角速率和小信号角速率分组（252–253）。我们通过两种不同的方式（左右发展）计算${\mathbf{q}}_t$， $$\dot{(\delta \mathbf{q}\otimes \mathbf{q})}={\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t$$

$$\dot{\delta \mathbf{q}}\otimes \mathbf{q}+\delta \mathbf{q}\otimes \dot{\mathbf{q}}={\dot{\mathbf{q}}}_t=\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes {\mathbf{\omega }}_t$$

$$\dot{\delta \mathbf{q}}\otimes \mathbf{q}+\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes \mathbf{\omega }=$$

结合${\mathbf{\omega }}_t=\mathbf{\omega }+\delta \mathbf{\omega }$，可简化为

$$\dot{\delta \mathbf{q}}\otimes \mathbf{q}=\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes \delta \mathbf{\omega }.\text{\hspace{1em}(303)}$$

对左右两边同乘以${\mathbf{q}}^{\ast }$，并结合$\mathbf{q}\otimes \delta \mathbf{\omega }\otimes {\mathbf{q}}^{\ast }\equiv \mathbf{R}\delta \mathbf{\omega }$，我们可以进一步得到如下等式，

$$\begin{array}{rl}\dot{\delta \mathbf{q}}=& \frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes \delta \mathbf{\omega }\otimes {\mathbf{q}}^{\ast }\\ =& \frac{1}{2}\delta \mathbf{q}\otimes (\mathbf{R}\delta \mathbf{\omega })\\ =& \frac{1}{2}\delta \mathbf{q}\otimes \delta {\mathbf{\omega }}_G,\end{array}\text{\hspace{1em}(304)}$$

其中$\delta {\mathbf{\omega }}_G\triangleq \mathbf{R}\delta \mathbf{\omega }$是在全局系中表示的小角速率信号。那么，

$$\begin{array}{rl}\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]=2\dot{\delta \mathbf{q}}=& \delta \mathbf{q}\otimes \delta {\mathbf{\omega }}_G\\ =& \Omega (\delta {\mathbf{\omega }}_G)\delta \mathbf{q}\\ =& \left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}_G^{\top }\\ \delta {\mathbf{\omega }}_G& -[\delta {\mathbf{\omega }}_G{]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O(||\delta \mathbf{\theta }|{|}^2),\end{array}\text{\hspace{1em}(305)}$$

从而得到标量和向量方程 $$0=\delta {\mathbf{\omega }}_G^{\top }\delta \mathbf{\theta }+O(|\delta \mathbf{\theta }{|}^2)\text{\hspace{1em}(306a)}$$

$$\dot{\delta \mathbf{\theta }}=\delta {\mathbf{\omega }}_G-\frac{1}{2}[\delta {\mathbf{\omega }}_G{]}_{\times }\delta \mathbf{\theta }+O(||\delta \mathbf{\theta }|{|}^2).\text{\hspace{1em}(306b)}$$

从第一个方程得到$\delta {\mathbf{\omega }}_G^{\top }\delta \mathbf{\theta }=O(||\delta \mathbf{\theta }|{|}^2)$，它是由二阶无穷下的形式，不是很有用。对第二个等式，在忽略所有二阶项后得到， $$\dot{\delta \mathbf{\theta }}=\delta {\mathbf{\omega }}_G=\mathbf{R}\delta \mathbf{\omega }.\text{\hspace{1em}(307)}$$

最终，结合式（253），我们得到了全局角速度误差的线性的动力学， $$\dot{\delta \mathbf{\theta }}=-\mathbf{R}\delta {\mathbf{\omega }}_b-\mathbf{R}{\mathbf{\omega }}_n.\text{\hspace{1em}(308)}$$

7.2 离散时间的系统运动学

7.2.1 标称状态

标称状态方程不涉及到误差，因此它于局部定义的方程误差的情况相同。

7.2.2 误差状态

使用欧拉积分，我们得到了如下的差的方程集， $$\delta \mathbf{p}\leftarrow \delta \mathbf{p}+\delta \mathbf{v}\Delta t\text{\hspace{1em}(309a)}$$

$$\delta \mathbf{v}\leftarrow \delta \mathbf{v}+(-[\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b){]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g})\Delta t+{\mathbf{v}}_{\mathbf{i}}\text{\hspace{1em}(309b)}$$

$$\delta \mathbf{\theta }\leftarrow \delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{\omega }}_b\Delta t+{\mathbf{\theta }}_{\mathbf{i}}\text{\hspace{1em}(309c)}$$

$$\delta {\mathbf{a}}_b\leftarrow \delta {\mathbf{a}}_b+{\mathbf{a}}_{\mathbf{i}}\text{\hspace{1em}(309d)}$$

$$\delta {\mathbf{\omega }}_b\leftarrow \delta {\mathbf{\omega }}_b+{\mathbf{\omega }}_{\mathbf{i}}\text{\hspace{1em}(309e)}$$

$$\delta \mathbf{g}\leftarrow \delta \mathbf{g}.\text{\hspace{1em}(309f)}$$

7.2.3 误差状态雅可比和扰动矩阵

通过对上述方程进行简单的检查可得到转移矩阵， $${\mathbf{F}}_{\mathbf{x}}=\left[\begin{array}{cccccc}\mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0& 0\\ 0& \mathbf{I}& -[\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b){]}_{\times }\Delta t& -\mathbf{R}\Delta t& 0& \mathbf{I}\Delta t\\ 0& 0& \mathbf{I}& 0& -\mathbf{R}\Delta t& 0\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right].\text{\hspace{1em}(310)}$$

    我们发现它关于局部定义的角速率误差的三个不同之处（比较上面框出来的部分【注，即标红的部分】与式（269））；这些变化总结在表4中。     在考虑到各向同性噪声以及附录E的展开后，扰动雅可比和扰动矩阵没有变化， $${\mathbf{F}}_{\mathbf{i}}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\end{array}\right],{\mathbf{Q}}_{\mathbf{i}}=\left[\begin{array}{cccc}{\mathbf{V}}_{\mathbf{i}}& 0& 0& 0\\ 0& {\mathbf{\Theta }}_{\mathbf{i}}& 0& 0\\ 0& 0& {\mathbf{A}}_{\mathbf{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\mathbf{i}}\end{array}\right].\text{\hspace{1em}(311)}$$

7.3 与互补的传感数据融合

考虑全局角度误差时，涉及ESKF机械的融合方程仅略有变化。我们通过ESKF校正，将误差加入标称状态以及重置步骤，来修改误差状态观测中的这些变化。

7.3.1 误差状态观测

相较于局部误差定义的唯一不同是将方向关联到角度误差的观测函数的雅可比块。新的块推导如下。

    使用（17-19）以及一阶近似$\delta \mathbf{q}\to \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]$，四元数项${\mathbf{Q}}_{\delta \mathbf{\theta }}$的推导如下，

$${\mathbf{Q}}_{\delta \mathbf{\theta }}\triangleq \frac{\partial (\delta \mathbf{q}\otimes \mathbf{q})}{\partial \delta \mathbf{\theta }}{|}_{\mathbf{q}}=\frac{\partial (\delta \mathbf{q}\otimes \mathbf{q})}{\partial \delta \mathbf{q}}{|}_{\mathbf{q}}\frac{\partial \delta \mathbf{q}}{\partial \delta \mathbf{\theta }}{|}_{\widehat{\delta \mathbf{\theta }}=0}\text{\hspace{1em}(312a)}$$

$$=[\mathbf{q}{]}_R\frac{1}{2}\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(312b)}$$

$$=\frac{1}{2}\left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& q_z& -q_y\\ -q_z& q_w& q_x\\ q_y& -q_x& q_w\end{array}\right]\text{\hspace{1em}(312c)}$$

7.3.2 将观测到的误差注入标称状态

标称和误差状态的组成$\mathbf{x}\leftarrow \mathbf{x}\oplus \widehat{\delta \mathbf{x}}$，

$$\mathbf{p}\leftarrow \mathbf{p}+\delta \mathbf{p}\text{\hspace{1em}(313a)}$$

$$\mathbf{v}\leftarrow \mathbf{v}+\delta \mathbf{v}\text{\hspace{1em}(313b)}$$

$$\mathbf{q}\leftarrow \mathbf{q}\{\widehat{\delta \mathbf{\theta }}\}\otimes \mathbf{q}\text{\hspace{1em}(313c)}$$

$${\mathbf{a}}_b\leftarrow {\mathbf{a}}_b+\delta {\mathbf{a}}_b\text{\hspace{1em}(313d)}$$

$${\mathbf{\omega }}_b\leftarrow {\mathbf{\omega }}_b+\delta {\mathbf{\omega }}_b\text{\hspace{1em}(313e)}$$

$$\mathbf{g}\leftarrow \mathbf{g}+\delta \mathbf{g}.\text{\hspace{1em}(313f)}$$

只有四元数更新的方程被影响了。表四中对此进行了总结。

7.3.3 ESKF重置

根据下面进行ESKF误差均值重置，以及协方差更新， $$\widehat{\delta \mathbf{x}}\leftarrow 0\text{\hspace{1em}(314)}$$

$$\mathbf{P}\leftarrow \mathbf{G}\mathbf{P}{\mathbf{G}}^{\top }\text{\hspace{1em}(315)}$$

它的雅可比为 $$\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& \mathbf{I}+[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]\text{\hspace{1em}(316)}$$

它的有意义的项在下面会进一步展开。我们的目标是得到新的角速度误差$\delta {\mathbf{\theta }}^{+}$相对于相对于老的误差$\delta \mathbf{\theta }$。我们考虑到这些事实：

误差重置时真实方向不变，即${\mathbf{q}}_t^{+}\equiv {\mathbf{q}}_t$。那么有： $$\delta {\mathbf{q}}^{+}\otimes {\mathbf{q}}^{+}=\delta \mathbf{q}\otimes \mathbf{q}.\text{\hspace{1em}(317)}$$

观测误差已经被引入到标称状态中（见式（282c）和（123））： $${\mathbf{q}}^{+}=\widehat{\delta \mathbf{q}}\otimes \mathbf{q}.\text{\hspace{1em}(318)}$$

    结合这两个恒等式，我们得到了相对于老的方向向误差的新方向误差和观测到的误差$\widehat{\delta \mathbf{q}}$的表达式， $$\delta {\mathbf{q}}^{+}=\delta \mathbf{q}\otimes {\widehat{\delta \mathbf{q}}}^{\ast }=[{\widehat{\delta \mathbf{q}}}^{\ast }{]}_R\cdot \delta \mathbf{q}.\text{\hspace{1em}(319)}$$

考虑到${\widehat{\delta \mathbf{q}}}^{\ast }\approx \left[\begin{array}{c}1\\ -\frac{1}{2}\widehat{\delta \mathbf{\theta }}\end{array}\right]$，将上面的恒等式展开，

$$\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}^{+}\end{array}\right]=\left[\begin{array}{cc}1& \frac{1}{2}{\widehat{\delta \mathbf{\theta }}}^{\top }\\ -\frac{1}{2}\widehat{\delta \mathbf{\theta }}& \mathbf{I}+[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }\end{array}\right]\cdot \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]+\mathcal{O}(||\delta \mathbf{\theta }|{|}^2)\text{\hspace{1em}(320)}$$

从而得到一个标量方程和一个向量方程， $$\frac{1}{4}{\widehat{\delta \mathbf{\theta }}}^{\top }\delta \mathbf{\theta }=\mathcal{O}(||\delta \mathbf{\theta }|{|}^2)\text{\hspace{1em}(312a)}$$

$$\delta {\mathbf{\theta }}^{+}=-\widehat{\delta \mathbf{\theta }}+(\mathbf{I}+{\left[\begin{array}{c}\frac{1}{2}\widehat{\delta \mathbf{\theta }}\end{array}\right]}_{\times })\delta \mathbf{\theta }+\mathcal{O}(||\delta \mathbf{\theta }|{|}^2)\text{\hspace{1em}(312b)}$$

其中，第一个方程不是很重要，因为它只是无穷下的关系。从第二个方程可以看出${\widehat{\delta \mathbf{\theta }}}^{+}=0$，这正是我们从重置方程中期望得到的。对应的雅可比如下， $$\frac{\partial \delta {\mathbf{\theta }}^{+}}{\partial \delta \mathbf{\theta }}=\mathbf{I}+{\left[\begin{array}{c}\frac{1}{2}\widehat{\delta \mathbf{\theta }}\end{array}\right]}_{\times }.\text{\hspace{1em}(322)}$$

    在表4中总结了其相对于局部误差的区别。

表4：与方向误差定义有关的算法修改

相关：

文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第1章 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第2章 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第3章 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第4章 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第5章 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第6章 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第7章