文章目录

Introduction
- 0.1 为什么要用到四元数？
- 0.2 先说一说四元数的Hamilton和JPL流派
1 Quaternion definition and properties
- 1-1 基本定义
- 1-2 主要性质
- - >>> 加减乘除
  - >>> 四元数乘法(Product)
  - >>> Identity 四元数、共轭、范数、逆
  - >>> 单位或标准化四元数(unit or normalized quaternion:star:)
- 1-3 其他性质
- - >>> Quaternion commutator
  - >>> 纯四元数的乘法(Product of pure qurternions)
2 Rotations and cross-relations
- 2-1 三维矢量旋转公式
- 2-2 The rotation group SO(3)
- 2-3 The rotation group and the rotation matrix
- - 2.3.1 指数映射
  - 2.3.2 Exp函数映射
  - 2.3.3 旋转向量和旋转矩阵：罗德里格斯旋转公式
  - 2.3.4 对数映射
  - 2.3.5 旋转行为(The rotation action)
- 2-4 The rotation group and the quaternion
- - 2.4.1 指数映射
  - >>> 2.4.2 Exp指数函数映射
  - >>> 2.4.3 四元数和旋转向量
  - >>> 2.4.4 对数映射
  - >>> 2.4.5 旋转动作
- 2.5 旋转矩阵和四元数
3 两种四元数表示(Hamilton和JPL)
4 扰动、导数和积分(重要，但略)
5 IMU驱动系统的误差状态运动学
- 5.2 ESKF的解释介绍
- 5.3 连续时间下系统的运动模型
- - >>> 5.3.1 The true-state kinematics
  - >>> 5.3.2 The nominal-state kinematics
  - >>> 5.3.3 The error-state kinematics（包括证明）
- 5.4 离散时间下系统的运动模型
- - >>> 5.4.1 标称状态运动模型(The nominal state kinematics)
  - >>> 5.4.2 The error-state kinemantics
  - >>> 5.4.3 误差状态雅克比和扰动矩阵
参考资料

Introduction

本文是针对对papers [Quaternion kinematics for the error-state Kalman filter]的非正式阅读笔记。

原papers写的非常好，通俗易懂，本文笔记记录过程中总是丢了连贯性，强烈推荐阅读英文原版。之所以说是非正式，一方面是因为与其说是笔记，倒不如说是公式速查笔记，丢了公式的一些来龙去脉，另一方面原因是里面保留着一些来至于翻译软件的直接翻译（太懒敲字），以及个人随性的笔记。为了便于对照原文，这里保留了与原paper相同的公式序标，方便一一对应。

0.1 为什么要用到四元数？

欧拉角：存在ambiguity。通过轴角法引出了四元数

轴角表示(Axis-Angle-Representation)。跟欧拉角不同的是，我们这次不再采取多次旋转的方式来找到目标方向，而是找到一根旋转轴，只通过绕这根轴旋转一次就可以得到目标方向。这样就不会产生Ambiguity了吗？是的，证明方法很简单，首先以目标矩阵原点为一角，三轴为三边建立一个立方体，这个立方体中通过原点的对角线就是我们要找的旋转轴，显然，这个旋转轴是唯一的，而我们知道，绕一个旋转轴旋转不同角度，对应的方向也是不同的。

上面几句来至于知乎问题[如何通俗地解释欧拉角？之后为何要引入四元数？] 里面的大佬大脸怪，这能深层次理解四元数的各个参数起源。

0.2 先说一说四元数的Hamilton和JPL流派

参考链接：

[JPL四元数和Hamilton四元数的区别]

如果不做说明，一般默认使用的都是Hamilton四元数表示（博客上基本都是默认使用的Hamilton表示）

本文档除非特殊说明，默认采用的是Hamilton表示方法

注意不同参考文献中使用的四元数形式不同，这里列些一些我所知道的：

论文：Quaternion kinematics for the error-state Kalman filter四元数是Hamilton表示方法
MARS实验室Indirect Kalman Filter for 3D Attitude Estimation-A Tutorial for Quaternion Algebra 四元数使用的是JPL
MSCKF参考的是上面MARS实验室的，所以使用的也是JPL形式的
Eigen库中使用的好像是Hamilton格式

1 Quaternion definition and properties

四元数的定义与性质

1-1 基本定义

纯四元数(pure quaternions)：实部为0

实部在前
Q=[q4q1q2q3]TorQ=[qwqv]T(1)Q = \left[\begin{array}{llll}q_{4} & q_{1} & q_{2} & q_{3}\end{array}\right]^{\mathrm{T}} \space \space or \space \space Q=\left[\begin{array}{ll}q_{w} & q_{v}\end{array}\right]^{\mathrm{T}} \tag{1} Q=[q4q1q2q3]T or Q=[qwqv]T(1)

右手坐标系
ij=k,jk=i,ki=j,i2=j2=k2=−1(2)i j=k, j k=i, k i=j, i^{2}=j^{2}=k^{2}=-1 \tag{2} ij=k,jk=i,ki=j,i2=j2=k2=−1(2)

四维表示
q≜[qwqv]=[qwqxqyqz](7)\mathbf{q} \triangleq\left[\begin{array}{l} q_{w} \\ \mathbf{q}_{v} \end{array}\right]=\left[\begin{array}{l} q_{w} \\ q_{x} \\ q_{y} \\ q_{z} \end{array}\right] \tag{7} q≜[qwqv]=⎣⎢⎢⎡qwqxqyqz⎦⎥⎥⎤(7)

1-2 主要性质

>>> 加减乘除

加法
p±q=[pwpv]±[qwqv]=[pw±qwpv±qv](9)\mathbf{p} \pm \mathbf{q}=\left[\begin{array}{c} p_{w} \\ \mathbf{p}_{v} \end{array}\right] \pm\left[\begin{array}{l} q_{w} \\ \mathbf{q}_{v} \end{array}\right]=\left[\begin{array}{l} p_{w} \pm q_{w} \\ \mathbf{p}_{v} \pm \mathbf{q}_{v} \end{array}\right] \tag{9} p±q=[pwpv]±[qwqv]=[pw±qwpv±qv](9)

>>> 四元数乘法(Product)

满足分配率，不满足交换率

p⊗q=[pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw](12)\mathbf{p} \otimes \mathbf{q}=\left[\begin{array}{l} p_{w} q_{w}-p_{x} q_{x}-p_{y} q_{y}-p_{z} q_{z} \\ p_{w} q_{x}+p_{x} q_{w}+p_{y} q_{z}-p_{z} q_{y} \\ p_{w} q_{y}-p_{x} q_{z}+p_{y} q_{w}+p_{z} q_{x} \\ p_{w} q_{z}+p_{x} q_{y}-p_{y} q_{x}+p_{z} q_{w} \end{array}\right] \tag{12} p⊗q=⎣⎢⎢⎡pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw⎦⎥⎥⎤(12)

p⊗q=[pwqw−pv⊤qvpwqv+qwpv+pv×qv](13)\mathbf{p} \otimes \mathbf{q}=\left[\begin{array}{c} p_{w} q_{w}-\mathbf{p}_{v}^{\top} \mathbf{q}_{v} \\ p_{w} \mathbf{q}_{v}+q_{w} \mathbf{p}_{v}+\mathbf{p}_{v} \times \mathbf{q}_{v} \end{array}\right] \tag{13} p⊗q=[pwqw−pv⊤qvpwqv+qwpv+pv×qv](13)

q1⊗q2=[q1]Lq2and q1⊗q2=[q2]Rq1(17)\mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{1}\right]_{L} \mathbf{q}_{2} \quad \text { and } \quad \mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{2}\right]_{R} \mathbf{q}_{1} \tag{17} q1⊗q2=[q1]Lq2 and q1⊗q2=[q2]Rq1(17)

其中四元数左乘右乘如下：
[q]L=[qw−qx−qy−qzqxqw−qzqyqyqzqw−qxqz−qyqxqw],[q]R=[qw−qx−qy−qzqxqwqz−qyqy−qzqwqxqzqy−qxqw](18)[\mathbf{q}]_{L}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & -q_{z} & q_{y} \\ q_{y} & q_{z} & q_{w} & -q_{x} \\ q_{z} & -q_{y} & q_{x} & q_{w} \end{array}\right], \quad[\mathbf{q}]_{R}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & q_{z} & -q_{y} \\ q_{y} & -q_{z} & q_{w} & q_{x} \\ q_{z} & q_{y} & -q_{x} & q_{w} \end{array}\right] \tag{18} [q]L=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤,[q]R=⎣⎢⎢⎡qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw⎦⎥⎥⎤(18)
或者
[q]L=qwI+[0−qv⊤qv[qv]×],[q]R=qwI+[0−qv⊤qv−[qv]×](19)[\mathbf{q}]_{L}=q_{w} \mathbf{I}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{\top} \\ \mathbf{q}_{v} & {\left[\mathbf{q}_{v}\right]_{\times}} \end{array}\right], \quad[\mathbf{q}]_{R}=q_{w} \mathbf{I}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{\top} \\ \mathbf{q}_{v} & -\left[\mathbf{q}_{v}\right]_{\times} \end{array}\right] \tag{19} [q]L=qwI+[0qv−qv⊤[qv]×],[q]R=qwI+[0qv−qv⊤−[qv]×](19)
其中，向量叉乘公式：
[a]×≜[0−azayaz0−ax−ayax0](20)[\mathbf{a}]_{\times} \triangleq\left[\begin{array}{ccc} 0 & -a_{z} & a_{y} \\ a_{z} & 0 & -a_{x} \\ -a_{y} & a_{x} & 0 \end{array}\right] \tag{20} [a]×≜⎣⎡0az−ay−az0axay−ax0⎦⎤(20)

有结论
[p]R[q]L=[q]L[p]R(23)[\mathbf{p}]_{R}[\mathbf{q}]_{L}=[\mathbf{q}]_{L}[\mathbf{p}]_{R} \tag{23} [p]R[q]L=[q]L[p]R(23)

>>> Identity 四元数、共轭、范数、逆

Identity 四元数：

满足q1⊗q=q⊗q1=q\mathbf{q}_{1} \otimes \mathbf{q}=\mathbf{q} \otimes \mathbf{q}_{1}=\mathbf{q}q1⊗q=q⊗q1=q，其中单位四元数 q1=1=[10v]\mathbf{q}_{1}=1=\left[\begin{array}{c}1 \\ \mathbf{0}_{v}\end{array}\right]q1=1=[10v]

共轭

q∗≜qw−qv=[qw−qv](24)\mathbf{q}^{*} \triangleq q_{w}-\mathbf{q}_{v}=\left[\begin{array}{c} q_{w} \\ -\mathbf{q}_{v} \end{array}\right] \tag{24} q∗≜qw−qv=[qw−qv](24)

q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2=[qw2+qx2+qy2+qz20v](25)\mathbf{q} \otimes \mathbf{q}^{*}=\mathbf{q}^{*} \otimes \mathbf{q}=q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}=\left[\begin{array}{c} q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2} \\ \mathbf{0}_{v} \end{array}\right] \tag{25} q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2=[qw2+qx2+qy2+qz20v](25)

(p⊗q)∗=q∗⊗p∗(26)(\mathbf{p} \otimes \mathbf{q})^{*}=\mathbf{q}^{*} \otimes \mathbf{p}^{*} \tag{26} (p⊗q)∗=q∗⊗p∗(26)

范数

∥q∥≜q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2∈R(27)\|\mathbf{q}\| \triangleq \sqrt{\mathbf{q} \otimes \mathbf{q}^{*}}=\sqrt{\mathbf{q}^{*} \otimes \mathbf{q}}=\sqrt{q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}} \in \mathbb{R} \tag{27} ∥q∥≜q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2∈R(27)

∥p⊗q∥=∥q⊗p∥=∥p∥∥q∥(28)\|\mathbf{p} \otimes \mathbf{q}\|=\|\mathbf{q} \otimes \mathbf{p}\|=\|\mathbf{p}\|\|\mathbf{q}\| \tag{28} ∥p⊗q∥=∥q⊗p∥=∥p∥∥q∥(28)

逆
q⊗q−1=q−1⊗q=q1(29)\mathbf{q} \otimes \mathbf{q}^{-1}=\mathbf{q}^{-1} \otimes \mathbf{q}=\mathbf{q}_{\mathbf{1}} \tag{29} q⊗q−1=q−1⊗q=q1(29)

q−1=q∗/∥q∥2(30)\mathbf{q}^{-1}=\mathbf{q}^{*} /\|\mathbf{q}\|^{2} \tag{30} q−1=q∗/∥q∥2(30)

>>> 单位或标准化四元数(unit or normalized quaternion⭐️)

对于单位四元数，其范数为1，因此：
q−1=q∗(31)\mathrm{q}^{-1}=\mathrm{q}^{*} \tag{31} q−1=q∗(31)

当将单位四元数解释为方向性质或旋转运算符时，此属性意味着可以使用共轭四元数实现反向旋转。单位四元数总是可以写成，
q=[cos⁡θusin⁡θ](32)\mathbf{q}=\left[\begin{array}{c} \cos \theta \\ \mathbf{u} \sin \theta \end{array}\right] \tag{32} q=[cosθusinθ](32)

u=uxi+uyj+uzk\mathbf{u}=u_{x} i+u_{y} j+u_{z} ku=uxi+uyj+uzk ：为单位向量
θ\thetaθ : 常量（四元数旋转的实际角度为2θ\thetaθ）

[为什么实际旋转角度是四元数里面的角度的两倍？有什么数学上的原因吗？]

在2.8章节也会有官方的解释

1-3 其他性质

>>> Quaternion commutator

通过公式(13)得到两个公式：
p⊗q−q⊗p=2pv×qv(33)\mathbf{p} \otimes \mathbf{q}-\mathbf{q} \otimes \mathbf{p}=2 \mathbf{p}_{v} \times \mathbf{q}_{v} \tag{33} p⊗q−q⊗p=2pv×qv(33)

pv⊗qv−qv⊗pv=2pv×qv(34)\mathbf{p}_{v} \otimes \mathbf{q}_{v}-\mathbf{q}_{v} \otimes \mathbf{p}_{v}=2 \mathbf{p}_{v} \times \mathbf{q}_{v} \tag{34} pv⊗qv−qv⊗pv=2pv×qv(34)

>>> 纯四元数的乘法(Product of pure qurternions)

通过公式(13)得到：

pv⊗qv=−pv⊤qv+pv×qv=[−pv⊤qvpv×qv](35)\mathbf{p}_{v} \otimes \mathbf{q}_{v}=-\mathbf{p}_{v}^{\top} \mathbf{q}_{v}+\mathbf{p}_{v} \times \mathbf{q}_{v}=\left[\begin{array}{c} -\mathbf{p}_{v}^{\top} \mathbf{q}_{v} \\ \mathbf{p}_{v} \times \mathbf{q}_{v} \end{array}\right] \tag{35} pv⊗qv=−pv⊤qv+pv×qv=[−pv⊤qvpv×qv](35)

qv⊗qv=−qv⊤qv=−∥qv∥2(36)\mathbf{q}_{v} \otimes \mathbf{q}_{v}=-\mathbf{q}_{v}^{\top} \mathbf{q}_{v}=-\left\|\mathbf{q}_{v}\right\|^{2} \tag{36} qv⊗qv=−qv⊤qv=−∥qv∥2(36)

对纯单位四元数(pure unitary quaternions) u∈Hp,∥u∥=1\mathbf{u} \in \mathbb{H}_{p},\|\mathbf{u}\|=1u∈Hp,∥u∥=1 :
u⊗u=−1(37)\mathbf{u} \otimes \mathbf{u}=-1 \tag{37} u⊗u=−1(37)

这里利用到了i∗i=−1i*i=-1i∗i=−1 的性质

2 Rotations and cross-relations

2-1 三维矢量旋转公式

其中旋转轴向量u为单位向量，得到的结论如下：
x′=x∥+x⊥cos⁡ϕ+(u×x)sin⁡ϕ(54)\mathbf{x}^{\prime}=\mathbf{x}_{\|}+\mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi \tag{54} x′=x∥+x⊥cosϕ+(u×x)sinϕ(54)

2-2 The rotation group SO(3)

2-3 The rotation group and the rotation matrix

2.3.1 指数映射

The exponential map

有结论：
R˙=R[ω]×(67)\dot{\mathbf{R}}=\mathbf{R}[\boldsymbol{\omega}]_{\times} \tag{67} R˙=R[ω]×(67)

公式有：
exp⁡:so(3)→SO(3);[v]×↦exp⁡([v]×)=e[v]×(70)\exp : \mathfrak{s o}(3) \rightarrow S O(3) ;[\mathbf{v}]_{\times} \mapsto \exp \left([\mathbf{v}]_{\times}\right)=e^{[\mathbf{v}]_{\times}} \tag{70} exp:so(3)→SO(3);[v]×↦exp([v]×)=e[v]×(70)

2.3.2 Exp函数映射

The capitalized exponential map

公式有：
Exp⁡:R3→SO(3);v↦Exp⁡(v)=e[v]×(71)\operatorname{Exp}: \mathbb{R}^{3} \rightarrow S O(3) ; \mathbf{v} \mapsto \operatorname{Exp}(\mathbf{v})=e^{[\mathbf{v}]_{\times}} \tag{71} Exp:R3→SO(3);v↦Exp(v)=e[v]×(71)
即：
Exp⁡(v)≜exp⁡([v]x)(72)\operatorname{Exp}(\mathbf{v}) \triangleq \exp \left([\mathbf{v}]_{x}\right) \tag{72} Exp(v)≜exp([v]x)(72)

2.3.3 旋转向量和旋转矩阵：罗德里格斯旋转公式

Rotation matrix and rotation vector: the Rodrigues rotation formula

[u]×2=uu⊤−I[u]×3=−[u]×(74–75)\begin{aligned} &{[\mathbf{u}]_{\times}^{2}=\mathbf{u u}^{\top}-\mathbf{I}} \\ &{[\mathbf{u}]_{\times}^{3}=-[\mathbf{u}]_{\times}} \end{aligned} \tag{74--75} [u]×2=uu⊤−I[u]×3=−[u]×(74–75)

u\mathbf{u}u ：单位向量（旋转轴）
ϕ\phiϕ : 模长（旋转角度）

Rodrigues rotation formula
R=I+sin⁡ϕ[u]×+(1−cos⁡ϕ)[u]×2(77)\mathbf{R}=\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2} \tag{77} R=I+sinϕ[u]×+(1−cosϕ)[u]×2(77)

变化式如下：
R=Icos⁡ϕ+[u]×sin⁡ϕ+uu⊤(1−cos⁡ϕ)(78)\mathbf{R}=\mathbf{I} \cos \phi+[\mathbf{u}]_{\times} \sin \phi+\mathbf{u u}^{\top}(1-\cos \phi) \tag{78} R=Icosϕ+[u]×sinϕ+uu⊤(1−cosϕ)(78)

2.3.4 对数映射

The logarithmic maps

注意：

这里的log⁡\loglog 是特指（为对应公式）

这里的Log\mathbf{Log}Log 是泛指，代值功能函数

有函数：
log⁡:SO(3)→so(3);R↦log⁡(R)=[uϕ]×(79)\log : S O(3) \rightarrow \mathfrak{s o}(3) ; \mathbf{R} \mapsto \log (\mathbf{R})=[\mathbf{u} \phi]_{\times} \tag{79} log:SO(3)→so(3);R↦log(R)=[uϕ]×(79)
其中：

ϕ=arccos⁡(trace⁡(R)−12)(80)\phi=\arccos \left(\frac{\operatorname{trace}(\mathbf{R})-1}{2}\right) \tag{80} ϕ=arccos(2trace(R)−1)(80)

u=(R−R⊤)∨2sin⁡ϕ(81)\mathbf{u}=\frac{\left(\mathbf{R}-\mathbf{R}^{\top}\right)^{\vee}}{2 \sin \phi} \tag{81} u=2sinϕ(R−R⊤)∨(81)

where ∙∨{\bullet}^{\vee}∙∨ is the inverse of [∙]×[\boldsymbol{\bullet}]_{\times}[∙]×, that is, ([v]×)∨=v\left([\mathbf{v}]_{\times}\right)^{\vee}=\mathbf{v}([v]×)∨=v and [V∨]x=V\left[\mathbf{V}^{\vee}\right]_{x}=\mathbf{V}[V∨]x=V.

反向求解操作如下：

Log:SO(3)→R3;R↦Log(R)=uϕ(82)\mathbf{Log} : S O(3) \rightarrow \mathbb{R}^{3} ; \mathbf{R} \mapsto \mathbf{Log} (\mathbf{R})=\mathbf{u} \phi \tag{82} Log:SO(3)→R3;R↦Log(R)=uϕ(82)
其中对应关系如下
Log(R)≜(log⁡(R))∨\mathbf{Log} (\mathbf{R}) \triangleq(\log (\mathbf{R}))^{\vee} Log(R)≜(log(R))∨

2.3.5 旋转行为(The rotation action)

x′=x∥+x⊥cos⁡ϕ+(u×x)sin⁡ϕ(54)\mathbf{x}^{\prime}=\mathbf{x}_{\|}+\mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi \tag{54} x′=x∥+x⊥cosϕ+(u×x)sinϕ(54)

2-4 The rotation group and the quaternion

2.4.1 指数映射

q=eV(95)\mathbf{q}=e^{\mathbf{V}} \tag{95} q=eV(95)

exp⁡:Hp→S3;V↦exp⁡(V)=eV(96)\exp : \mathbb{H}_{p} \rightarrow S^{3} ; \mathbf{V} \mapsto \exp (\mathbf{V})=e^{\mathbf{V}} \tag{96} exp:Hp→S3;V↦exp(V)=eV(96)

>>> 2.4.2 Exp指数函数映射

Exp⁡:R3→S3;v↦Exp⁡(v)=ev/2(97)\operatorname{Exp}: \mathbf{R}^{3} \rightarrow S^{3} ; \mathbf{v} \mapsto \operatorname{Exp}(\mathbf{v})=e^{\mathbf{v} / 2} \tag{97} Exp:R3→S3;v↦Exp(v)=ev/2(97)

Exp⁡(v)≜exp⁡(v/2)(98)\operatorname{Exp}(\mathbf{v}) \triangleq \exp (\mathbf{v} / 2) \tag{98} Exp(v)≜exp(v/2)(98)

q˙=12q⊗ωq=eωt/2(99–100)\begin{aligned} &\dot{\mathbf{q}}=\frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega} \\ &\mathbf{q}=e^{\omega t / 2} \end{aligned} \tag{99--100} q˙=21q⊗ωq=eωt/2(99–100)

>>> 2.4.3 四元数和旋转向量

q≜Exp⁡(ϕu)=eϕu/2=cos⁡ϕ2+usin⁡ϕ2=[cos⁡(ϕ/2)usin⁡(ϕ/2)](101)\mathbf{q} \triangleq \operatorname{Exp}(\phi \mathbf{u})=e^{\phi \mathbf{u} / 2}=\cos \frac{\phi}{2}+\mathbf{u} \sin \frac{\phi}{2}=\left[\begin{array}{c} \cos (\phi / 2) \\ \mathbf{u} \sin (\phi / 2) \end{array}\right] \tag{101} q≜Exp(ϕu)=eϕu/2=cos2ϕ+usin2ϕ=[cos(ϕ/2)usin(ϕ/2)](101)

>>> 2.4.4 对数映射

The logarithmic maps

log⁡:S3→Hp;q↦log⁡(q)=uθ(102)\log : S^{3} \rightarrow \mathbb{H}_{p} ; \mathbf{q} \mapsto \log (\mathbf{q})=\mathbf{u} \theta \tag{102} log:S3→Hp;q↦log(q)=uθ(102)

反向操作如下：
Log:S3→R3;q↦Log(q)=uϕ(103)\mathbf{Log} : S^{3} \rightarrow \mathbb{R}^{3} ; \mathbf{q} \mapsto \mathbf{Log} (\mathbf{q})=\mathbf{u} \phi \tag{103} Log:S3→R3;q↦Log(q)=uϕ(103)

其中：
Log(q)≜2log⁡(q)(104)\mathbf{Log} (\mathbf{q}) \triangleq 2 \log (\mathbf{q}) \tag{104} Log(q)≜2log(q)(104)

ϕ=2arctan⁡(∥qv∥,qw)u=qv/∥qv∥(105)\begin{aligned} \phi &=2 \arctan \left(\left\|\mathbf{q}_{v}\right\|, q_{w}\right) \\ \mathbf{u} &=\mathbf{q}_{v} /\left\|\mathbf{q}_{v}\right\| \end{aligned} \tag{105} ϕu=2arctan(∥qv∥,qw)=qv/∥qv∥(105)

Log(q)=θu≈2qvqw(1−∥qv∥23qw2)(106)\mathbf{Log} (\mathbf{q})=\theta \mathbf{u} \approx 2 \frac{\mathbf{q}_{v}}{q_{w}}\left(1-\frac{\left\|\mathbf{q}_{v}\right\|^{2}}{3 q_{w}^{2}}\right) \tag{106} Log(q)=θu≈2qwqv(1−3qw2∥qv∥2)(106)

>>> 2.4.5 旋转动作

以 u\mathbf{u}u 为旋转轴，对向量x进行角度为ϕ\phiϕ的旋转操作，公式如下：
x′=q⊗x⊗q∗(107)\mathbf{x}^{\prime}=\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*} \tag{107} x′=q⊗x⊗q∗(107)

其中
x=xi+yj+zk=[0x]∈Hp(108)\mathbf{x}=x i+y j+z k=\left[\begin{array}{l} 0 \\ \mathbf{x} \end{array}\right] \in \mathbb{H}_{p} \tag{108} x=xi+yj+zk=[0x]∈Hp(108)

2.5 旋转矩阵和四元数

q⊗x⊗q∗=Rx\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}=\mathbf{R} \mathbf{x} q⊗x⊗q∗=Rx

R=[qw2+qx2−qy2−qz22(qxqy−qwqz)2(qxqz+qwqy)2(qxqy+qwqz)qw2−qx2+qy2−qz22(qyqz−qwqx)2(qxqz−qwqy)2(qyqz+qwqx)qw2−qx2−qy2+qz2]\mathbf{R}=\left[\begin{array}{ccc} q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2} & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{x} q_{z}+q_{w} q_{y}\right) \\ 2\left(q_{x} q_{y}+q_{w} q_{z}\right) & q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2} & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\ 2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{y} q_{z}+q_{w} q_{x}\right) & q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2} \end{array}\right] R=⎣⎡qw2+qx2−qy2−qz22(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)qw2−qx2+qy2−qz22(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)qw2−qx2−qy2+qz2⎦⎤

R=(qw2−qv⊤qv)I+2qvqv⊤+2qw[qv]×\mathbf{R}=\left(q_{w}^{2}-\mathbf{q}_{v}^{\top} \mathbf{q}_{v}\right) \mathbf{I}+2 \mathbf{q}_{v} \mathbf{q}_{v}^{\top}+2 q_{w}\left[\mathbf{q}_{v}\right]_{\times} R=(qw2−qv⊤qv)I+2qvqv⊤+2qw[qv]×

R{[1,0,0,0]⊤}=IR{−q}=R{q}R{q∗}=R{q}⊤R{q1⊗q2}=R{q1}R{q2}\begin{aligned} \mathbf{R}\left\{[1,0,0,0]^{\top}\right\} &=\mathbf{I} \\ \mathbf{R}\{-\mathbf{q}\} &=\mathbf{R}\{\mathbf{q}\} \\ \mathbf{R}\left\{\mathbf{q}^{*}\right\} &=\mathbf{R}\{\mathbf{q}\}^{\top} \\ \mathbf{R}\left\{\mathbf{q}_{1} \otimes \mathbf{q}_{2}\right\} &=\mathbf{R}\left\{\mathbf{q}_{1}\right\} \mathbf{R}\left\{\mathbf{q}_{2}\right\} \end{aligned} R{[1,0,0,0]⊤}R{−q}R{q∗}R{q1⊗q2}=I=R{q}=R{q}⊤=R{q1}R{q2}

3 两种四元数表示(Hamilton和JPL)

下面一位大佬已经总结的很详细了

参考链接：

[JPL四元数和Hamilton四元数的区别]

4 扰动、导数和积分(重要，但略)

Perturbations, derivatives and integrals

这一部分最好直接查看官方文档，总觉得直接的翻译失了些味道。其实个人认为，对四元数的了解在查看了本文的已经翻译的部分后，已经对四元数有了直观的感觉，看这部分已经问题不大~

5 IMU驱动系统的误差状态运动学

关于ESKF（文档直译）

方向误差状态最小（即，其参数数量与自由度相同），避免了与过度参数化（或冗余）相关的问题，以及相关协方差矩阵奇异性的后续风险，通常由强制约束引起。
误差状态系统始终在原点附近运行，因此远离可能的参数奇异性、万向节锁问题等，从而保证线性化有效性始终保持。
误差状态总是很小，这意味着所有二阶产品都可以忽略不计。这使得雅可比矩阵的计算非常简单和快速。一些雅可比矩阵甚至可能是常数或等于可用的状态量。
误差动态缓慢，因为所有大信号动态都已在标称状态下集成。这意味着我们可以以低于预测的速率应用KF校正（这是观察误差的唯一方法）。

5.2 ESKF的解释介绍

在 error-state ﬁlter（误差状态滤波器）公式中，我们谈到true-state（真值状态）、nominal-state（标称状态）和error-state（误差状态），真实状态表示为标称和误差状态的适当组合（线性和、四元数乘积或矩阵积）。其思想是考虑nominal-state（标称状态）作为大信号（以非线性方式可积）和error-state（误差状态）作为小信号（从而线性可积，适合线性高斯滤波）处理。

error-state ﬁlter（误差状态滤波器）可解释如下：

一方面，高频IMU数据um\mathbf{u}_{m}um被集成到标称状态x\mathbf{x}x中。该标称状态不考虑噪声项w和其他可能的模型缺陷。因此，它会累积错误。在误差状态δx中收集这些误差，并使用误差状态卡尔曼滤波器（ESKF）进行估计，这一次包括所有噪声和扰动。误差状态由小信号量组成，其演化函数由（时变）线性动态系统正确定义，其动态、控制和测量矩阵由标称状态的值计算得出。在整合标称状态的同时，ESKF预测误差状态的高斯估计。它只是预测，因为到目前为止，还没有其他衡量标准来修正这些估计。滤波校正是在IMU以外的信息（如GPS、视觉等）到达时执行的，这些信息能够使误差可见，并且通常以比积分阶段低得多的速率发生。这种校正提供了误差状态的后验高斯估计。
在此之后，误差状态的平均值被注入标称状态，然后重置为零。错误状态的协方差矩阵可以方便地更新，以反映这种重置。这个系统永远都是这样。

5.3 连续时间下系统的运动模型

表3总结了所有相关变量的定义。关于公约的两项重要决定值得一提

角速率ω是相对于局部(locally)标称四元数定义的。这允许我们直接使用陀螺仪测量um\mathbf{u}_{m}um，因为它们提供了body-referenced angular rates。
角度误差δθ\delta \boldsymbol{\theta}δθ也可根据标称方向进行局部定义。这不一定是进行集成的最佳方式，但它符合大多数IMU集成工作中的选择——我们可以称之为经典方法。有证据（Li和Mourikis，2012）表明，全球定义(globally-defined)的角度误差具有更好的特性。本文件第7节也将探讨这一点，但这里的大多数开发、示例和算法都基于本地定义的角度误差

>>> 5.3.1 The true-state kinematics

p˙t=vtv˙t=atq˙t=12qt⊗ωta˙bt=awω˙bt=ωwg˙t=0(229)\begin{aligned} \dot{\mathbf{p}}_{t} &=\mathbf{v}_{t} \\ \dot{\mathbf{v}}_{t} &=\mathbf{a}_{t} \\ \dot{\mathbf{q}}_{t} &=\frac{1}{2} \mathbf{q}_{t} \otimes \boldsymbol{\omega}_{t} \\ \dot{\mathbf{a}}_{b t} &=\mathbf{a}_{w} \\ \dot{\boldsymbol{\omega}}_{b t} &=\boldsymbol{\omega}_{w} \\ \dot{\mathrm{g}}_{t} &=0 \end{aligned} \tag{229} p˙tv˙tq˙ta˙btω˙btg˙t=vt=at=21qt⊗ωt=aw=ωw=0(229)

为什么bias的导数不为0？，是在考虑哪一种误差影响？

答：偏置被建模为随机游走，其导数为高斯性的。参考VIN-mono论文中的公式2也是如此处理

am=Rt⊤(at−gt)+abt+anωm=ωt+ωbt+ωn(230–231)\begin{aligned} \mathbf{a}_{m} &=\mathbf{R}_{t}^{\top}\left(\mathbf{a}_{t}-\mathbf{g}_{t}\right)+\mathbf{a}_{b t}+\mathbf{a}_{n} \\ \boldsymbol{\omega}_{m} &=\boldsymbol{\omega}_{t}+\boldsymbol{\omega}_{b t}+\boldsymbol{\omega}_{n} \end{aligned} \tag{230--231} amωm=Rt⊤(at−gt)+abt+an=ωt+ωbt+ωn(230–231)

倒换一下
at=Rt(am−abt−an)+gtωt=ωm−ωbt−ωn(232–233)\begin{aligned} \mathbf{a}_{t} &=\mathbf{R}_{t}\left(\mathbf{a}_{m}-\mathbf{a}_{b t}-\mathbf{a}_{n}\right)+\mathbf{g}_{t} \\ \boldsymbol{\omega}_{t} &=\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b t}-\boldsymbol{\omega}_{n} \end{aligned} \tag{232--233} atωt=Rt(am−abt−an)+gt=ωm−ωbt−ωn(232–233)

综上得(⭐️)：
p˙t=vtv˙t=Rt(am−abt−an)+gtq˙t=12qt⊗(ωm−ωbt−ωn)a˙bt=awω˙bt=ωwg˙t=0(234)\begin{aligned} \dot{\mathbf{p}}_{t} &=\mathbf{v}_{t} \\ \dot{\mathbf{v}}_{t} &=\mathbf{R}_{t}\left(\mathbf{a}_{m}-\mathbf{a}_{b t}-\mathbf{a}_{n}\right)+\mathbf{g}_{t} \\ \dot{\mathbf{q}}_{t} &=\frac{1}{2} \mathbf{q}_{t} \otimes\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b t}-\boldsymbol{\omega}_{n}\right) \\ \dot{\mathbf{a}}_{b t} &=\mathbf{a}_{w} \\ \dot{\boldsymbol{\omega}}_{b t} &=\boldsymbol{\omega}_{w} \\ \dot{\mathrm{g}}_{t} &=0 \end{aligned} \tag{234} p˙tv˙tq˙ta˙btω˙btg˙t=vt=Rt(am−abt−an)+gt=21qt⊗(ωm−ωbt−ωn)=aw=ωw=0(234)

我们写成函数表达式x˙t=ft(xt,u,w)\dot{\mathbf{x}}_{t}=f_{t}\left(\mathbf{x}_{t}, \mathbf{u}, \mathbf{w}\right)x˙t=ft(xt,u,w) ，该系统具有状态xt，由IMU噪声读数um\mathbf{u}_{m}um 控制，并受到高斯白噪声w的干扰，其中各个变量如下：
xt=[ptvtqtabtωbtgt]u=[am−anωm−ωn]w=[awωw](235)\mathbf{x}_{t}=\left[\begin{array}{c} \mathbf{p}_{t} \\ \mathbf{v}_{t} \\ \mathbf{q}_{t} \\ \mathbf{a}_{b t} \\ \boldsymbol{\omega}_{b t} \\ \mathbf{g}_{t} \end{array}\right] \quad \mathbf{u}=\left[\begin{array}{c} \mathbf{a}_{m}-\mathbf{a}_{n} \\ \boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{n} \end{array}\right] \quad \mathbf{w}=\left[\begin{array}{l} \mathbf{a}_{w} \\ \boldsymbol{\omega}_{w} \end{array}\right] \tag{235} xt=⎣⎢⎢⎢⎢⎢⎢⎡ptvtqtabtωbtgt⎦⎥⎥⎥⎥⎥⎥⎤u=[am−anωm−ωn]w=[awωw](235)

>>> 5.3.2 The nominal-state kinematics

标称状态运动学对应于无噪声或扰动的建模系统:
p˙=vv˙=R(am−ab)+gq˙=12q⊗(ωm−ωb)a˙b=0ω˙b=0g˙=0(236)\begin{aligned} \dot{\mathbf{p}} &=\mathbf{v} \\ \dot{\mathbf{v}} &=\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g} \\ \dot{\mathbf{q}} &=\frac{1}{2} \mathbf{q} \otimes\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \\ \dot{\mathbf{a}}_{b} &=0 \\ \dot{\boldsymbol{\omega}}_{b} &=0 \\ \dot{\mathbf{g}} &=0 \end{aligned} \tag{236} p˙v˙q˙a˙bω˙bg˙=v=R(am−ab)+g=21q⊗(ωm−ωb)=0=0=0(236)

>>> 5.3.3 The error-state kinematics（包括证明）

目标是确定误差状态(error-state)的线性化动态。对于每个状态方程，我们将其组成（见表3），求解误差状态并简化所有二阶单元。我们在这里给出了完整的误差状态(error-state)动态系统，然后进行了评论和证明。
δp˙=δvδv˙=−R[am−ab]×δθ−Rδab+δg−Ranδθ˙=−[ωm−ωb]×δθ−δωb−ωnδ˙ab=awδω˙b=ωwδg˙=0(237)\begin{aligned} \dot{\delta \mathbf{p}} &=\delta \mathbf{v} \\ \dot{\delta \mathbf{v}} &=-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}-\mathbf{R} \mathbf{a}_{n} \\ \dot{\delta \boldsymbol{\theta}} &=-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b}-\omega_{n} \\ \dot{\delta} \mathbf{a}_{b} &=\mathbf{a}_{w} \\ \delta \dot{\boldsymbol{\omega}}_{b} &=\boldsymbol{\omega}_{w} \\ \dot{\delta \mathbf{g}} &=0 \end{aligned} \tag{237} δp˙δv˙δθ˙δ˙abδω˙bδg˙=δv=−R[am−ab]×δθ−Rδab+δg−Ran=−[ωm−ωb]×δθ−δωb−ωn=aw=ωw=0(237)

证明公式237(b)：

首先有下面两公式：
Rt=R(I+[δθ]×)+O(∥δθ∥2)v˙=RaB+g,(238–239)\begin{aligned} \mathbf{R}_{t} &=\mathbf{R}\left(\mathbf{I}+[\delta \boldsymbol{\theta}]_{\times}\right)+O\left(\|\delta \boldsymbol{\theta}\|^{2}\right) \\ \dot{\mathbf{v}} &=\mathbf{R a}_{\mathcal{B}}+\mathbf{g}, \end{aligned} \tag{238--239} Rtv˙=R(I+[δθ]×)+O(∥δθ∥2)=RaB+g,(238–239)

238：为旋转的小信号扰动公式

239：对236(b)进行等效替代（引入加速度的两个变量，定义如下）

aB≜am−abδaB≜−δab−an(240–241)\begin{gathered} \mathbf{a}_{\mathcal{B}} \triangleq \mathbf{a}_{m}-\mathbf{a}_{b} \\ \delta \mathbf{a}_{\mathcal{B}} \triangleq-\delta \mathbf{a}_{b}-\mathbf{a}_{n} \end{gathered} \tag{240--241} aB≜am−abδaB≜−δab−an(240–241)

这样定义的依据是：将加速度量表示为：真正加速度 = 大信号量+小信号量。

对(232)重写如下：
at=Rt(aB+δaB)+g+δg(242)\mathbf{a}_{t}=\mathbf{R}_{t}\left(\mathbf{a}_{\mathcal{B}}+\delta \mathbf{a}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g} \tag{242} at=Rt(aB+δaB)+g+δg(242)

对(234b)的表达式，写成两种不同的格式（分别在等号的左边和右边）
v˙+δ˙v=v˙t=R(I+[δθ]×)(aB+δaB)+g+δgRaB+g+δ˙v==RaB+RδaB+R[δθ]×aB+R[δθ]×δaB+g+δg\begin{aligned} \dot{\mathbf{v}}+\dot{\delta} \mathbf{v}=\dot{\mathbf{v}}_{t} &=\mathbf{R}\left(\mathbf{I}+[\delta \boldsymbol{\theta}]_{\times}\right)\left(\mathbf{a}_{\mathcal{B}}+\delta \mathbf{a}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g} \\ \mathbf{R} \mathbf{a}_{\mathcal{B}}+\mathbf{g}+\dot{\delta} \mathbf{v} &=\quad=\mathbf{R} \mathbf{a}_{\mathcal{B}}+\mathbf{R} \delta \mathbf{a}_{\mathcal{B}}+\mathbf{R}[\delta \boldsymbol{\theta}]_{\times} \mathbf{a}_{\mathcal{B}}+\mathbf{R}[\delta \boldsymbol{\theta}]_{\times} \delta \mathbf{a}_{\mathcal{B}}+\mathbf{g}+\delta \mathbf{g} \end{aligned} v˙+δ˙v=v˙tRaB+g+δ˙v=R(I+[δθ]×)(aB+δaB)+g+δg==RaB+RδaB+R[δθ]×aB+R[δθ]×δaB+g+δg
移除等号两边的RaB+g\mathrm{Ra}_{\mathcal{B}}+\mathrm{g}RaB+g 得到：
δv˙=R(δaB+[δθ]×aB)+R[δθ]×δaB+δg(243)\dot{\delta \mathbf{v}}=\mathbf{R}\left(\delta \mathbf{a}_{\mathcal{B}}+[\delta \boldsymbol{\theta}]_{\times} \mathbf{a}_{\mathcal{B}}\right)+\mathbf{R}[\delta \boldsymbol{\theta}]_{\times} \delta \mathbf{a}_{\mathcal{B}}+\delta \mathbf{g} \tag{243} δv˙=R(δaB+[δθ]×aB)+R[δθ]×δaB+δg(243)
舍弃掉两个小量的乘积R[δθ]×δaB\mathbf{R}[\delta \boldsymbol{\theta}]_{\times} \delta \mathbf{a}_{\mathcal{B}}R[δθ]×δaB（旋转矩阵R是刚性的，不改变大小），并利用叉乘性质[a]×b=−[b]×a[\mathrm{a}]_{\times} \mathrm{b}=-[\mathrm{b}]_{\times} \mathrm{a}[a]×b=−[b]×a可得到下式：
δv˙=R(δaB−[aB]×δθ)+δg(244)\dot{\delta \mathbf{v}}=\mathbf{R}\left(\delta \mathbf{a}_{\mathcal{B}}-\left[\mathbf{a}_{\mathcal{B}}\right]_{\times} \delta \boldsymbol{\theta}\right)+\delta \mathbf{g} \tag{244} δv˙=R(δaB−[aB]×δθ)+δg(244)
代入新定义的表达式(240–241)，得到：
δv˙=R(−[am−ab]×δθ−δab−an)+δg(245)\dot{\delta \mathbf{v}}=\mathbf{R}\left(-\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\delta \mathbf{a}_{b}-\mathbf{a}_{n}\right)+\delta \mathbf{g} \tag{245} δv˙=R(−[am−ab]×δθ−δab−an)+δg(245)
因此，倒换一下后，得证如下：
δv˙=−R[am−ab]×δθ−Rδab+δg−Ran(246)\dot{\delta \mathbf{v}}=-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}-\mathbf{R} \mathbf{a}_{n} \tag{246} δv˙=−R[am−ab]×δθ−Rδab+δg−Ran(246)

证明公式237©：

思想相同：
q˙t=12qt⊗ωtq˙=12q⊗ω,(250–251)\begin{aligned} \dot{\mathbf{q}}_{t} &=\frac{1}{2} \mathbf{q}_{t} \otimes \boldsymbol{\omega}_{t} \\ \dot{\mathbf{q}} &=\frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega}, \end{aligned} \tag{250--251} q˙tq˙=21qt⊗ωt=21q⊗ω,(250–251)
对236©进行等效替代（引入角速度的两个变量，定义如下）
ω≜ωm−ωbδω≜−δωb−ωn(252–253)\begin{gathered} \boldsymbol{\omega} \triangleq \boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b} \\ \delta \boldsymbol{\omega} \triangleq-\delta \boldsymbol{\omega}_{b}-\boldsymbol{\omega}_{n} \end{gathered} \tag{252--253} ω≜ωm−ωbδω≜−δωb−ωn(252–253)
真实值 = 大信号 + 小信号，所以有下面：
ωt=ω+δω(254)\boldsymbol{\omega}_{t}=\boldsymbol{\omega}+\delta \boldsymbol{\omega} \tag{254} ωt=ω+δω(254)
写成两种不同的格式（分别在等号的左边和右边）

下面公式(255)的推导过程，利用了四元数公式(19)性质，以及矩阵的结合律，另外最后一步代入了公式(254)

舍弃小量的乘积，整理得到：
δθ˙=−[ω]×δθ+δω(257)\dot{\delta \boldsymbol{\theta}}=-[\boldsymbol{\omega}]_{\times} \delta \boldsymbol{\theta}+\delta \boldsymbol{\omega} \tag{257} δθ˙=−[ω]×δθ+δω(257)
得证：
δθ˙=−[ωm−ωb]×δθ−δωb−ωn(258)\dot{\delta \boldsymbol{\theta}}=-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b}-\boldsymbol{\omega}_{n} \tag{258} δθ˙=−[ωm−ωb]×δθ−δωb−ωn(258)

5.4 离散时间下系统的运动模型

上述微分方程需要整合到微分方程中，以考虑离散时间间隔Δt>0。集成方法可能会有所不同。在某些情况下，人们将能够使用精确的闭式解（closed-form solutions）。在其他情况下，可以采用不同精度的数值积分。有关集成方法的相关详细信息，请参阅附录。

>>> 5.4.1 标称状态运动模型(The nominal state kinematics)

>>> 5.4.2 The error-state kinemantics

δp←δp+δvΔtδv←δv+(−R[am−ab]×δθ−Rδab+δg)Δt+viδθ←R⊤{(ωm−ωb)Δt}δθ−δωbΔt+θiδab←δab+aiδωb←δωb+ωiδg←δg(260)\begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \\ \delta \mathbf{g} & \leftarrow \delta \mathbf{g} \end{aligned} \tag{260} δpδvδθδabδωbδg←δp+δvΔt←δv+(−R[am−ab]×δθ−Rδab+δg)Δt+vi←R⊤{(ωm−ωb)Δt}δθ−δωbΔt+θi←δab+ai←δωb+ωi←δg(260)

这里，vi,θi,ai\mathbf{v_{i}, \theta_{i}, a_{i}}vi,θi,ai and ωi\mathbf{\omega_{i}}ωi是应用于速度、方向和偏差估计的随机脉冲，建模为白高斯，均值为0，它们的协方差矩阵是通过积分Δt时间间隔上的an,ωn,aw\mathbf{a}_{n}, \boldsymbol{\omega}_{n}, \mathbf{a}_{w}an,ωn,aw and ωw\boldsymbol{\omega}_{w}ωw的协方差得到的。（详见附录E）

>>> 5.4.3 误差状态雅克比和扰动矩阵

The error-state Jacobian and perturbation matrices

雅可比矩阵是简单通过前一节中的误差状态差方程得到的。

为了以紧凑形式写出这些方程，我们考虑标称状态向量x、误差状态向量δx、输入向量u m和扰动脉冲向量i(the perturbation impulses vector )，如下所示（参见App.E.1的细节和证明）
x=[pvqabωbg],δx=[δpδvδθδabδωbδg],um=[amωm],i=[viθiaiωi](265)\mathbf{x}=\left[\begin{array}{c} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} \\ \mathbf{g} \end{array}\right] \quad, \quad \delta \mathbf{x}=\left[\begin{array}{c} \delta \mathbf{p} \\ \delta \mathbf{v} \\ \delta \boldsymbol{\theta} \\ \delta \mathbf{a}_{b} \\ \delta \boldsymbol{\omega}_{b} \\ \delta \mathbf{g} \end{array}\right] \quad, \quad \mathbf{u}_{m}=\left[\begin{array}{c} \mathbf{a}_{m} \\ \boldsymbol{\omega}_{m} \end{array}\right] \quad, \quad \mathbf{i}=\left[\begin{array}{l} \mathbf{v}_{\mathbf{i}} \\ \boldsymbol{\theta}_{\mathbf{i}} \\ \mathbf{a}_{\mathbf{i}} \\ \boldsymbol{\omega}_{\mathbf{i}} \end{array}\right] \tag{265} x=⎣⎢⎢⎢⎢⎢⎢⎡pvqabωbg⎦⎥⎥⎥⎥⎥⎥⎤,δx=⎣⎢⎢⎢⎢⎢⎢⎡δpδvδθδabδωbδg⎦⎥⎥⎥⎥⎥⎥⎤,um=[amωm],i=⎣⎢⎢⎡viθiaiωi⎦⎥⎥⎤(265)
因此error-state system 为：
δx←f(x,δx,um,i)=Fx(x,um)⋅δx+Fi⋅i(266)\delta \mathbf{x} \leftarrow f\left(\mathbf{x}, \delta \mathbf{x}, \mathbf{u}_{m}, \mathbf{i}\right)=\mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \delta \mathbf{x}+\mathbf{F}_{\mathbf{i}} \cdot \mathbf{i} \tag{266} δx←f(x,δx,um,i)=Fx(x,um)⋅δx+Fi⋅i(266)
ESKF预测公式为：
δx^←Fx(x,um)⋅δx^P←FxPFx⊤+FiQiFi⊤(267–268)\begin{aligned} \hat{\delta \mathbf{x}} & \leftarrow \mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \hat{\delta \mathbf{x}} \\ \mathbf{P} & \leftarrow \mathbf{F}_{\mathbf{x}} \mathbf{P} \mathbf{F}_{\mathbf{x}}^{\top}+\mathbf{F}_{\mathbf{i}} \mathbf{Q}_{\mathbf{i}} \mathbf{F}_{\mathbf{i}}^{\top} \end{aligned} \tag{267--268} δx^P←Fx(x,um)⋅δx^←FxPFx⊤+FiQiFi⊤(267–268)

δx∼N{δx^,P}\delta \mathbf{x} \sim \mathcal{N}\{\hat{\delta \mathbf{x}}, \mathbf{P}\}δx∼N{δx^,P}

Fx\mathbf{F}_{\mathbf{x}}Fx ：为f()f()f()关于误差的雅克比

Fi\mathbf{F}_{\mathbf{i}}Fi ：为f()f()f()关于the perturbation impulses vector i 的雅克比

下面详细介绍了上述雅可比矩阵和协方差矩阵的表达式。本文中出现的所有状态相关值均直接从标称状态中提取.

其中：
Fx=∂f∂δx∣x,um=[IIΔt00000I−R[am−ab]×Δt−RΔt0IΔt00R⊤{(ωm−ωb)Δt}0−IΔt0000I000000I000000I](269)\mathbf{F}_{\mathbf{x}}=\left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{cccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 & \mathbf{I} \Delta t \\ 0 & 0 & \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} & 0 & -\mathbf{I} \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] \tag{269} Fx=∂δx∂f∣∣∣∣x,um=⎣⎢⎢⎢⎢⎢⎢⎡I00000IΔtI00000−R[am−ab]×ΔtR⊤{(ωm−ωb)Δt}0000−RΔt0I0000−IΔt0I00IΔt000I⎦⎥⎥⎥⎥⎥⎥⎤(269)

以及
Fi=∂f∂i∣x,um=[0000I0000I0000I0000I0000],Qi=[Vi0000Θi0000Ai0000Ωi](270)\mathbf{F}_{\mathbf{i}}=\left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\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] \quad, \quad \mathbf{Q}_{\mathbf{i}}=\left[\begin{array}{cccc} \mathbf{V}_{\mathbf{i}} & 0 & 0 & 0 \\ 0 & \Theta_{\mathbf{i}} & 0 & 0 \\ 0 & 0 & \mathbf{A}_{\mathbf{i}} & 0 \\ 0 & 0 & 0 & \Omega_{\mathbf{i}} \end{array}\right] \tag{270} Fi=∂i∂f∣∣∣∣x,um=⎣⎢⎢⎢⎢⎢⎢⎡0I000000I000000I000000I0⎦⎥⎥⎥⎥⎥⎥⎤,Qi=⎣⎢⎢⎡Vi0000Θi0000Ai0000Ωi⎦⎥⎥⎤(270)

二手翻译（来自对官方文件翻译—不要见笑）：

请特别注意，Fx是系统的转移矩阵，可以通过多种方式近似到不同的精度水平。这里我们展示了它最简单的一种形式（欧拉形式）。参见附录B至D，以供进一步参考。

还请注意，作为初始化为零的误差δx的平均值，线性方程（267）总是返回零。当然，您应该跳过代码中的第（267）行。不过，我建议你把它写下来，但是你要把它注释掉，这样你就可以确定你没有忘记任何事情。

最后，请注意，你不应该跳过协方差预测（268）！！实际上，项FiQiFi⊤\mathbf{F}_{\mathbf{i}} \mathbf{Q}_{\mathbf{i}} \mathbf{F}_{\mathbf{i}}^{\top}FiQiFi⊤不是空的，因此这种协方差会持续增长——在任何预测步骤中都必须如此。

参考资料

papers

[Quaternion kinematics for the error-state Kalman filter]（主要）

知乎

[如何通俗地解释欧拉角？之后为何要引入四元数？] ⭐️