一般有两个坐标系：大地基准坐标系w系（或者G系）与机器人本体坐标系b系（或者I系），两坐标系之间的旋转矩阵表示为：
R
=
b
w
R
=
Exp
[
G
I
δ
θ
]
=
I
G
δ
q
R={ }_{b}^{w} R=\operatorname{Exp}\left[\begin{array}{l} G \\ { }_{I} \end{array} \delta \theta\right]={ }_{I}^{G} \delta q
R=bwR=Exp[GIδθ]=IGδq
坐标系计算的matlab方法：
clear
syms x y z g
Rx = [1 0 0;
0 cos(x) -sin(x);
0 sin(x) cos(x)];
Ry = [cos(y) 0 sin(y);
0 1 0;
-sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
sin(z) cos(z) 0;
0 0 1];
Rwb = Rz*Ry*Rx;
Rwb.'*[0;0;g]
SLAM中的坐标系定义：
欧拉角角速度与陀螺仪测量的本体角速度之间（eulerRates与bodyRates）有如下关系：
d
d
t
[
ϕ
θ
ψ
]
=
[
1
sin
ϕ
tan
θ
cos
ϕ
tan
θ
0
cos
ϕ
−
sin
ϕ
0
sin
ϕ
/
cos
θ
cos
ϕ
/
cos
θ
]
ω
≜
W
(
θ
)
ω
\frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{l} \phi \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{ccc} 1 & \sin \phi \tan \theta & \cos \phi \tan \theta \\ 0 & \cos \phi & -\sin \phi \\ 0 & \sin \phi / \cos \theta & \cos \phi / \cos \theta \end{array}\right] \boldsymbol{\omega} \triangleq \boldsymbol{W}(\boldsymbol{\theta}) \boldsymbol{\omega}
dtd⎣⎡ϕθψ⎦⎤=⎣⎡100sinϕtanθcosϕsinϕ/cosθcosϕtanθ−sinϕcosϕ/cosθ⎦⎤ω≜W(θ)ω
这个关系常用于欧拉角的运动学更新中。其中，W是可逆矩阵。
可以这样表示：
q
˙
e
b
(
t
)
=
1
2
[
0
−
ω
x
−
ω
y
−
ω
z
ω
x
0
ω
z
−
ω
y
ω
y
−
ω
z
0
ω
x
ω
z
ω
y
−
ω
x
0
]
q
e
b
(
t
)
\dot{q}_{e}^{b}(t)=\frac{1}{2}\left[\begin{array}{cccc} 0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0 \end{array}\right] q_{e}^{b}(t)
q˙eb(t)=21⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤qeb(t)
在MEKF算法的差分化之后，有这样的结果：
q
(
t
+
Δ
t
)
=
q
(
t
)
⊗
e
1
2
q
ω
Δ
t
=
q
(
t
)
(
1
+
1
/
2
∗
Ω
Δ
t
)
\boldsymbol{q}(t+\Delta t)=\boldsymbol{q}(t) \otimes e^{\frac{1}{2} \boldsymbol{q}_{\omega} \Delta t}=q(t)(1 + 1/2 *\Omega \Delta t)
q(t+Δt)=q(t)⊗e21qωΔt=q(t)(1+1/2∗ΩΔt)
也可以表示为：
q
b
,
b
k
⊗
δ
q
b
k
b
k
′
=
δ
q
b
,
b
k
⊗
[
1
1
2
δ
θ
b
k
b
k
]
\mathbf{q}_{b, b_{k}} \otimes \delta \mathbf{q}_{b_{k} b_{k}^{\prime}}=\delta \mathbf{q}_{b, b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \theta_{b_{k} b_{k}} \end{array}\right]
qb,bk⊗δqbkbk′=δqb,bk⊗[121δθbkbk]
这里面一个很重要的性质就是：
δ
q
=
[
1
1
2
δ
θ
]
\delta q=\left[\begin{array}{l}1 \\ \frac{1}{2} \delta \theta\end{array}\right]
δq=[121δθ]
四元数相乘可以写成：
q
1
⊗
q
2
=
[
q
]
L
q
2
=
[
q
]
R
q
\mathbf{q}_{1} \otimes \mathbf{q}_{2}=[\mathbf{q}]_{L} \mathbf{q}_{2}=[\mathbf{q}]_{R} \mathbf{q}
q1⊗q2=[q]Lq2=[q]Rq
其中
[
q
]
L
=
[
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
]
=
q
w
I
+
[
0
−
q
v
T
q
v
[
q
v
]
×
]
[\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]=q_{w} \mathbf{I}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{\mathrm{T}} \\ \mathbf{q}_{v} & {\left[\mathbf{q}_{v}\right]_{\times}} \end{array}\right]
[q]L=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤=qwI+[0qv−qvT[qv]×]
旋转向量V与李群之间的转换，需要经过中间量“李代数”。
也可以用罗德里格斯旋转公式直接转换：
R
=
I
+
sin
ϕ
[
u
]
×
+
(
1
−
cos
ϕ
)
[
u
]
×
2
\mathbf{R}=\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2}
R=I+sinϕ[u]×+(1−cosϕ)[u]×2
描述旋转群与四元数之间的关系：参考文献1的 4.2.2
这个方法好像在eskf中用到的比较多，
H
≜
∂
h
∂
δ
x
∣
x
=
∂
h
∂
x
t
∣
x
∂
x
t
∂
δ
x
∣
x
=
H
x
X
δ
x
\left.\mathbf{H} \triangleq \frac{\partial h}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left.\left.\frac{\partial h}{\partial \mathbf{x}_{t}}\right|_{\mathbf{x}} \frac{\partial \mathbf{x}_{t}}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\mathbf{H}_{\mathbf{x}} \mathbf{X}_{\delta \mathbf{x}}
H≜∂δx∂h∣∣∣∣x=∂xt∂h∣∣∣∣x∂δx∂xt∣∣∣∣x=HxXδx
Q δ θ ≜ ∂ ( q ⊗ δ q ) ∂ δ θ ∣ q = ∂ ( q ⊗ δ q ) ∂ δ q ∣ q ∂ δ q ∂ δ θ ∣ δ ^ θ ^ = 0 \left.\mathbf{Q}_{\delta \boldsymbol{\theta}} \triangleq \frac{\partial(\mathbf{q} \otimes \delta \mathbf{q})}{\partial \delta \boldsymbol{\theta}}\right|_{\mathbf{q}}=\left.\left.\frac{\partial(\mathbf{q} \otimes \delta \mathbf{q})}{\partial \delta \mathbf{q}}\right|_{\mathbf{q}} \frac{\partial \delta \mathbf{q}}{\partial \delta \boldsymbol{\theta}}\right|_{\hat{\delta} \hat{\theta}=0} Qδθ≜∂δθ∂(q⊗δq)∣∣∣∣q=∂δq∂(q⊗δq)∣∣∣∣q∂δθ∂δq∣∣∣∣∣δ^θ^=0
主要讨论基于“误差状态”方法的kalman滤波设计方法。该算法为“ESKF”算法，主要参考文献1和github上的一些代码。
参考网页；
科式加速度主要用来描述参考系与惯性系之间的关系。
我们来逐项分析上面这个式子。第一项中 αα 为 {B} 的角加速度，所以第一项的物理意义是 {B} 旋转所造成的 P 的切向加速度。第二项是 {B} 旋转所造成的向心加速度。第四项为 P 相对于 {B} 的加速度，但在惯性系 {A} 下表达——类似于 vrvr，定义相对加速度 arar。第三项比较特殊，为 {B} 的旋转运动与 P 相对 {B} 的平移运动耦合产生的加速度，称为「科氏加速度」。可以看到，除了第四项外，另外三项都和 {B} 的旋转有关。
在MSCKF1中，考虑的是参考系（b），因此需要考虑；但在MSCKF2中，考虑的是惯性系（a），所以就不需要考虑科式加速度了。
这里存在一个问题：既然不考虑科式加速度又简便也符合逻辑，那么为什么一些文献中采用考虑科式加速度的做法呢？这个问题保留。
惯性导航算法常常用误差状态量来表示。实际上IMU组件给系统系统的运动学方程，在状态估计中可以认为是一种约束。同时测量方程也可以认为是另外一种约束。在优化系统中，将这两种约束都写入到优化函数中，并且保证惩罚函数是线性的，这样就可以利用梯度下降法简单求解了。
| 类别 | 方法 |
|---|---|
| 基于一阶泰勒展开误差递推方程：方法1 | 离散化–>差分化 |
| 基于误差随时间变化的递推方程：方法2 | 差分化–>离散化 |
| 备注 | 实际上就是两个部分：离散化，差分化。虽然都不是 什么复杂的数学理论，但是常常让人感到confused 另外，差分化就是将其写成误差状态的形式 |
但是因为方法2中需要知道状态量的连续导数关系（随时间变化的关系），很多系统都没办法求解这部分（在PVQ系统中可以做）。方法1中的离散形式比较符合对EKF的解释和EKF中协方差更新的方法，因此更加常用。
总结来说，如果系统是连续方程，那么需要先进行error state化之后再进行离散化。如果系统是离散方程，那么直接进行error state化即可。
下面是详细定义：
在上面的两种方法中，第一种是符合EKF的状态方程的递推关系的；第二种是目前VIO算法中的主流做法。应该是殊途同归的两种做法，但是第一种更为通用。
其中，下面的推导是怎么完成的？
δ
v
˙
=
R
δ
a
b
+
R
[
δ
θ
]
×
(
a
b
+
δ
a
b
)
+
δ
g
δ
v
˙
=
R
δ
a
b
−
R
[
a
b
]
×
δ
θ
+
δ
g
\begin{array}{l} \dot{\delta \mathbf{v}}=\mathbf{R} \delta \mathbf{a}^{b}+\mathbf{R}[\delta \boldsymbol{\theta}]_{\times}\left(\mathbf{a}^{b}+\delta \mathbf{a}^{b}\right)+\delta \mathbf{g} \\ \dot{\delta \mathbf{v}}=\mathbf{R} \delta \mathbf{a}^{\mathbf{b}}-\mathbf{R}\left[\mathbf{a}^{b}\right]_{\times} \delta \boldsymbol{\theta}+\delta \mathbf{g} \end{array}
δv˙=Rδab+R[δθ]×(ab+δab)+δgδv˙=Rδab−R[ab]×δθ+δg
最符合逻辑的就是：两个小量相乘，最后的结果就是无穷小，可以忽略。
与ESKF的区别是什么？下面给出ESKF的定义。
δ
v
˙
=
−
R
[
a
m
−
a
b
]
×
δ
θ
−
R
δ
a
b
+
δ
g
−
R
a
n
\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}
δv˙=−R[am−ab]×δθ−Rδab+δg−Ran
实际上是一样的，不过在ESKF中多了Ran量。
算法部分常用卡尔曼滤波，kalman滤波中需要确定几个参量：过程协方差矩阵Q、测量协方差矩阵R和不确定方差矩阵P。
这个分析过程需要随机过程的知识，主要参考博客与博客的内容。
这里的问题是：这个部分到底是不是为了确定Q的大小？
于是我们得到随机游走模型的完整表达。实际上，观察离散模型的表达式，可以发现它生动阐释了「随机游走」的含义：每一时刻都是上一个采样时刻加上一个高斯白噪声得到的，犹如一个游走的粒子，踏出的下一步永远是随机的。在我们前面给出的 IMU 的运动模型中，bias 就设定为服从随机游走模型2。
Q d = Q c Δ t Q c = blkdiag ( σ a c c 2 ) Q_{d}=Q_{c} \Delta t \quad Q_{c}=\text {blkdiag}\left(\sigma_{a c c}^{2}\right) Qd=QcΔtQc=blkdiag(σacc2)
为什么呢不一致呢？参考博客的内容，推断的计算方法如下：但是这里是白噪声还是随机游走？理论上应该是白噪声，因为没有讨论随机游走。
Q
c
Δ
t
∗
Δ
t
2
=
b
l
k
d
i
a
g
(
σ
g
y
r
o
2
)
Δ
t
∗
Δ
t
2
\frac{Q_{c}}{\Delta t} *{\Delta t}^{2}=\frac{b l k d i a g\left(\sigma_{g y r o}^{2}\right)}{\Delta t} *{\Delta t^{2}}
ΔtQc∗Δt2=Δtblkdiag(σgyro2)∗Δt2
V i = σ a n 2 Δ t 2 I Θ i = σ ω n 2 Δ t 2 I A i = σ a ω 2 Δ t I Ω i = σ ω ω 2 Δ t I \begin{array}{l} \boldsymbol{V}_{i}=\sigma_{a_{n}}^{2} \Delta t^{2} \boldsymbol{I} \\ \mathbf{\Theta}_{i}=\sigma_{\omega_{n}}^{2} \Delta t^{2} \boldsymbol{I} \\ \boldsymbol{A}_{i}=\sigma_{a_{\omega}}^{2} \Delta t \boldsymbol{I} \\ \boldsymbol{\Omega}_{i}=\sigma_{\omega_{\omega}}^{2} \Delta t \boldsymbol{I} \end{array} Vi=σan2Δt2IΘi=σωn2Δt2IAi=σaω2ΔtIΩi=σωω2ΔtI
这里描述的方法也不一致，这就需要了解什么是连续的标准差和离散的标准差了。暂且以文献1为标准。
要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量之间的线性递推关系。协方差矩阵可以这样推导：这个方差的积累公式需要注意一下，实际上状态估计大多都是这么做的。
Σ
i
k
=
F
k
−
1
Σ
i
k
−
1
F
k
−
1
⊤
+
G
k
−
1
Σ
n
G
k
−
1
⊤
\boldsymbol{\Sigma}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\Sigma}_{i k-1} \mathbf{F}_{k-1}^{\top}+\mathbf{G}_{k-1} \boldsymbol{\Sigma}_{\mathbf{n}} \mathbf{G}_{k-1}^{\top}
Σik=Fk−1Σik−1Fk−1⊤+Gk−1ΣnGk−1⊤
其中，
Σ
n
Σ_n
Σn 是测量噪声的协方差矩阵，方差从 i 时刻开始进行递推，
Σ
i
i
=
0
Σ_{ii} = 0
Σii=0。
其中F是非线性系统函数对状态量的雅可比矩阵，G代表对控制量的雅可比？
问题是：噪声现在是不是就是控制量？噪声可以做控制量，但是如果有明确含义的控制量，比如说：里程计的左右轮，那么左右轮的误差就是sigma_n。
但是在传统的卡尔曼滤波器的递推公式中，有下面的结果：
在这里更新的过程并没有考虑到控制量u的雅可比矩阵，这是为什么呢？因为u的雅可比矩阵只影响到P矩阵的更新，而并不影响标称值的更新。（标称值的更新依赖于更加准确的非线性状态更新方程）。
这里Q代表的过程误差方差矩阵，对应的维度是状态量。利用控制量与运动学方程，确实可以求出来状态量的误差方差矩阵。相当于状态量的方差有一部分是由控制量决定的。但是如果系统中没有控制量，只有状态量，那么就需要直接给出Q矩阵的值。实际上这种系统也是比较多的。
在一个开源项目中是这样确定Q的大小的：
sGPS = 0.5*8.8*dt**2 # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
sCourse = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehicle
sVelocity= 8.8*dt # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
sYaw = 1.0*dt # assume 1.0rad/s2 as the maximum turn rate acceleration for the vehicle
Q = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2, sYaw**2])
可以看到这里的Q是用来描述连续微分运动学方程中的微分量的。因为这里的Q只是在定义误差量的过程噪声，所以是比较合理的。
最终在MEKF的博客中找到答案；
在文献1中找到下面的佐证（附录部分）。下面的差分化是利用了第二种状态方程的递推方法，然后进行离散化。
由此便得知协方差Q矩阵的确定过程。实际上在实验中，并没有特别严格的定义，实际上两种都可以。
这部分参考Quaternion kinematics for the error-state Kalman filte和博客。
什么是动力学是缓慢的？变化是缓慢的？是不是用来描述振动变化，有着更为出色的性能？如果是的话，这也是一个可以发论文的点。是不是和李代数去表示是有关系的？只有在小量的时候，在切空间的性质才可以成立。
主要参考文献1
ESKF就是用了上述“基于误差随时间变化”求解方法，首先写成error state，然后进行离散化得到IMU的递推方程。
参数表如下：
三种状态之间的转换关系：
| English | 中文 |
|---|---|
| nominal state | 标称状态 |
| true state | 真实状态 |
| error state | 误差状态 |
给出下面：
其中 𝜃𝑖 𝑤𝑖 为高斯随机脉冲噪声，均值为0，协方差为 𝑤𝑛 ，𝑤𝑏𝑛 在 𝛥𝑡时间内的积分值。
因此给出误差传播方程：
δ
p
←
δ
p
+
δ
v
Δ
t
δ
v
←
δ
v
+
(
−
R
[
a
m
−
a
b
]
×
δ
θ
−
R
δ
a
b
+
δ
g
)
Δ
t
+
v
i
δ
θ
←
R
⊤
{
(
ω
m
−
ω
b
)
Δ
t
}
δ
θ
−
δ
ω
b
Δ
t
+
θ
i
δ
a
b
←
δ
a
b
+
a
i
δ
ω
b
←
δ
ω
b
+
ω
i
δ
g
←
δ
g
\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}
δ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
测量方程求取雅可比矩阵采用链式法则。
此外有：
X
δ
x
≜
∂
x
t
∂
δ
x
∣
x
=
[
I
6
0
0
0
Q
δ
θ
0
0
0
I
9
]
\left.\mathbf{X}_{\delta \mathbf{x}} \triangleq \frac{\partial \mathbf{x}_{t}}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left[\begin{array}{ccc} \mathbf{I}_{6} & 0 & 0 \\ 0 & \mathbf{Q}_{\delta \boldsymbol{\theta}} & 0 \\ 0 & 0 & \mathbf{I}_{9} \end{array}\right]
Xδx≜∂δx∂xt∣∣∣∣x=⎣⎡I6000Qδθ000I9⎦⎤
总结来说，求取差分的偏导数如下，需要注意的是，我用到的是左栏的结果，右栏中未作考虑。
参考github项目（yds16）
角速度积分表示：
I
G
R
˙
=
I
G
R
[
ω
m
−
b
g
−
n
w
]
×
b
˙
g
=
0
+
n
g
\begin{aligned} _{I}^{G} \dot{\boldsymbol{R}} &=_{I}^{G} \boldsymbol{R}\left[\boldsymbol{\omega}_{m}-\boldsymbol{b}_{g}-\boldsymbol{n}_{w}\right] \times \\ \dot{\boldsymbol{b}}_{g} &=\mathbf{0}+\boldsymbol{n}_{g} \end{aligned}
IGR˙b˙g=IGR[ωm−bg−nw]×=0+ng
下面是旋转矩阵的积分过程：
d
R
d
t
=
R
[
ω
]
×
\frac{\mathrm{d} \mathbf{R}}{\mathrm{d} t}=\mathbf{R}[\boldsymbol{\omega}]_{\times}
dtdR=R[ω]×
得到离散化之后的：
R
(
t
+
Δ
t
)
=
R
(
t
)
e
[
ω
]
×
Δ
t
\mathbf{R}(t+\Delta t)=\mathbf{R}(t) e^{[\omega]_{\times} \Delta t}
R(t+Δt)=R(t)e[ω]×Δt
参考EKF-Based IMU Orientation Estimation，有下面的运动学方程：
δ
θ
=
Exp
[
(
ω
m
−
b
g
)
Δ
t
]
T
δ
θ
−
δ
b
g
Δ
t
+
θ
i
δ
b
g
=
δ
b
g
+
ω
i
\begin{aligned} \delta \boldsymbol{\theta} &=\operatorname{Exp}\left[\left(\boldsymbol{\omega}_{m}-\boldsymbol{b}_{g}\right) \Delta t\right]^{T} \delta \boldsymbol{\theta}-\delta \boldsymbol{b}_{g} \Delta t+\boldsymbol{\theta}_{i} \\ \delta \boldsymbol{b}_{g} &=\delta \boldsymbol{b}_{g}+\boldsymbol{\omega}_{i} \end{aligned}
δθδbg=Exp[(ωm−bg)Δt]Tδθ−δbgΔt+θi=δbg+ωi
表示为：
[
δ
θ
δ
b
g
]
=
[
Exp
[
(
ω
m
−
b
g
)
Δ
t
]
T
−
I
Δ
t
0
I
]
⏟
F
x
[
δ
θ
δ
b
g
]
+
[
I
0
0
I
]
⏟
F
i
[
θ
i
ω
i
]
\left[\begin{array}{c} \delta \boldsymbol{\theta} \\ \delta \boldsymbol{b}_{g} \end{array}\right]=\underbrace{\left[\begin{array}{cc} \operatorname{Exp}\left[\left(\boldsymbol{\omega}_{m}-\boldsymbol{b}_{g}\right) \Delta t\right]^{T} & -\boldsymbol{I} \Delta t \\ \boldsymbol{0} & \boldsymbol{I} \end{array}\right]}_{\boldsymbol{F}_{\boldsymbol{x}}}\left[\begin{array}{c} \boldsymbol{\delta} \boldsymbol{\theta} \\ \delta \boldsymbol{b}_{g} \end{array}\right]+\underbrace{\left[\begin{array}{cc} \boldsymbol{I} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{I} \end{array}\right]}_{\boldsymbol{F}_{i}}\left[\begin{array}{c} \boldsymbol{\theta}_{i} \\ \boldsymbol{\omega}_{i} \end{array}\right]
[δθδbg]=Fx
[Exp[(ωm−bg)Δt]T0−IΔtI][δθδbg]+Fi
[I00I][θiωi]
进行kalman滤波更新即可。
参考项目；
% =========================================================================
%
% ESKF的实现
%
%
% =========================================================================
%
% (C)2020-2022 China Academy of Railway Sciences
% 版本：V1.0
% 日期：2020年 6月23日
% 作者：s.m.
%--------------------------------------------------------------------------
% 功能：1.实现ESKF算法，加深对于状态估计的理解
% 2.其中的问题：
% 1） 测量加上地磁计
% 2） 注意误差量与标称量
% 3） 四元数转角度时有误差，就是这个导致了误差量
%
%
%
%--------------------------------------------------------------------------
clear all;
close all;
addpath('../../ESKF-Attitude-Estimation-master')
addpath('../utils')
% -------------------- import data-------------------
fileName = '../NAV_1';
Data = importdata(sprintf('%s.mat',fileName));
lengthtp = size(Data,1);
time = zeros(lengthtp,1);
roll = zeros(lengthtp,1);
pitch = zeros(lengthtp,1);
yaw = zeros(lengthtp,1);
imuNominalStates = cell(1,lengthtp);
imuErrorStates = cell(1,lengthtp);
measurements = cell(1,lengthtp);
%groundTruth
for state_k = 1:lengthtp
measurements{state_k}.dt = 0.02; % sampling times 50Hz
measurements{state_k}.omega = Data(state_k,27:29)';
measurements{state_k}.acc = Data(state_k,9:11)';
measurements{state_k}.mag = Data(state_k,15:17)';
time(state_k)=state_k*0.02;
end
rad2deg = 180/pi;
rollRef = Data(:,30)*rad2deg;
pitchRef = Data(:,31)*rad2deg;
yawRef = Data(:,32)*rad2deg;
% --------------------Data analysis------------------
% ++++++++++++++++++++1.initialization++++++++++++++++
dt = measurements{1}.dt;
% 怎么处理初始化的theta？
omega_b = zeros(3,1);%%这个用到
theta = zeros(3,1);%%这个用不到
% error state initialization
dt_theta = zeros(3,1);
dt_omega_b = zeros(3,1);
% Keep updated status
err_state = [dt_theta;dt_omega_b];
quat = zeros(4,1);
% --------Refer to previous practice for initialization-----------------------------------
init_angle = [Data(1,30),Data(1,31),Data(1,32)]';
init_quat = oula2q(init_angle);
quat = init_quat';
% -------------------------2.covariance matrix ---------------------
p1 = 1e-5;p2 = 1e-7;
P = blkdiag(p1,p1,p1,p2,p2,p2);%%初始化
sigma_wn = 1e-5;
sigma_wbn = 1e-9;
Theta_i = sigma_wn*dt^2*eye(3);
Omega_i = sigma_wbn*dt^2*eye(3);
Fi = eye(6);
Qi = blkdiag(Theta_i , Omega_i);
Q = Fi*Qi*Fi';
sigma_acc = 1e-3;
sigma_mn = 1e-4;
R = blkdiag(eye(3)*sigma_acc,eye(3)*sigma_mn);
for index = 1:lengthtp-1
% --------------------------forecast------------
omega_m = (measurements{index+1}.omega + measurements{index}.omega)/2;
av = (omega_m - omega_b)*dt;
det_q = [1;0.5*av];
quat = quatLeftComp(quat)*det_q;
omega_b = omega_b;
% 计算标称值
F1 = Exp_lee((measurements{index+1}.omega - omega_b)*dt);
F1 = F1';
Fx = [F1 , -eye(3)*dt;
zeros(3) , eye(3)];
P_ = Fx*P*Fx' + Q;
% -----------------------observation---------------------
% Prediction results and observations
[H,detZ] = calH(quat, measurements{index+1});
% --------------------update-----------------
K = P_*H'*inv(H*P_*H' + R)/2;
err_state = K*detZ;
P = P_ - K*(H*P_*H' + R)*K';
% ----------------------update state----------------------
% 参考之前的函数，dt_theta-->quat, quat的左乘方法
dt_theta = err_state(1:3);
dt_omega_b = err_state(4:6);
dt_q = buildUpdateQuat(dt_theta);
quat = quatLeftComp(quat)*dt_q;
quat = quat/norm(quat);
omega_b = omega_b + dt_omega_b;
% ------save angle-----------------------------
[a1,a2,a3] = quattoeuler(quat);
oula(index+1,:) = [a1,a2,a3]/180*pi;
dt_theta_save(index+1,:) = err_state';
% ----------------------------reset-------------------
err_state = zeros(6,1);
G = blkdiag(eye(3) - omegaMatrix(dt_theta/2) ,eye(3));
P = G*P*G';
end
% figure;
% subplot(3,1,1)
% plot(pitchRef);
% hold on;plot(oula(:,2)/pi*180);
% subplot(3,1,2)
% plot(rollRef);
% hold on;plot(oula(:,1)/pi*180);
% subplot(3,1,3)
% plot(yawRef);
% hold on;plot(oula(:,3)/pi*180);
% legend 1 2
rotLim = [-5 5];
figure;
subplot(3,1,1)
plot(oula(:,1)/pi*180 - rollRef);
subplot(3,1,2)
plot(oula(:,2)/pi*180 - pitchRef);
subplot(3,1,3)
plot(oula(:,3)/pi*180 - yawRef);
legend 1 2
% ylim(rotLim)
function R = q2R(q)
%四元数转旋转矩阵
R=[ 2*q(1).^2-1+2*q(2)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
R2 = R;
end
function Q_dt_theta = cal_Q_dt_theta(quat)
Q_dt_theta = 0.5* [-quat(2) -quat(3) -quat(4); ...
quat(1) -quat(4) quat(3); ...
quat(4) quat(1) -quat(2); ...
-quat(3) quat(2) quat(1)];
end
function F = Exp_lee(in)
S = omegaMatrix(in);
normV = sqrt(S(1,2)^2+S(1,3)^2+S(1,3)^2);
F = eye(3)+sin(normV)/normV*S(:,:)+...
(1-cos(normV))/normV^2*S(:,:)^2;
end
function [omega]=omegaMatrix(data)
% wx=data(1)*pi/180;
% wy=data(2)*pi/180;
% wz=data(3)*pi/180;
wx=data(1);
wy=data(2);
wz=data(3);
omega=[
0,-wz,wy;
wz,0,-wx;
-wy,wx,0
];
end
function q = R2q(R)
%旋转矩阵转四元数
t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
Q1 = q;
end
function q = oula2q(in)
x = in(1);
y = in(2);
z = in(3);
%欧拉角转四元数
q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
end
function Ang3 = q2oula(q)
%四元数转欧拉角
x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
y = asin(2*(q(1)*q(3) - q(2)*q(4)));
z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
Ang3 = [x y z];
end
function updateQuat = buildUpdateQuat(deltaTheta)
deltaq = 0.5 * deltaTheta;
updateQuat = [1; deltaq];
updateQuat = updateQuat / norm(updateQuat);
end
function qLC = quatLeftComp(quat)
vector = quat(2:4);
scalar = quat(1);
qLC = [ scalar , -vector';
vector , scalar*eye(3) + crossMat(vector) ];
end
function [H,detZ] = calH(q,measurements_k)
% Normalise magnetometer measurement
if(norm(measurements_k.mag) == 0), return; end %
measurements_k.mag = measurements_k.mag / norm(measurements_k.mag); % normalise magnitude,very important!!!!
% Normalise accelerometer measurement
if(norm(measurements_k.acc) == 0), return; end % handle NaN
measurements_k.acc = measurements_k.acc / norm(measurements_k.acc); % normalise accelerometer ,very important!!!!
% Reference direction of Earth's magnetic feild
h = quaternProd(q, quaternProd([0; measurements_k.mag], quatInv(q)));
b = [0 norm([h(2) h(3)]) 0 h(4)];
Ha = [2*q(3), -2*q(4), 2*q(1), -2*q(2)
-2*q(2), -2*q(1), -2*q(4), -2*q(3)
0, 4*q(2), 4*q(3), 0];
Hm = [-2*b(4)*q(3), 2*b(4)*q(4), -4*b(2)*q(3)-2*b(4)*q(1), -4*b(2)*q(4)+2*b(4)*q(2)
-2*b(2)*q(4)+2*b(4)*q(2), 2*b(2)*q(3)+2*b(4)*q(1), 2*b(2)*q(2)+2*b(4)*q(4), -2*b(2)*q(1)+2*b(4)*q(3)
2*b(2)*q(3), 2*b(2)*q(4)-4*b(4)*q(2), 2*b(2)*q(1)-4*b(4)*q(3), 2*b(2)*q(2)];
Hx = [Ha, zeros(3,3)
Hm, zeros(3,3)];
% Hx = [Ha, zeros(3,3)];
Q_detTheta = [-q(2), -q(3), -q(4)
q(1), -q(4), q(3)
q(4), q(1), -q(2)
-q(3), q(2), q(1)];
Xx = [0.5*Q_detTheta , zeros(4,3)
zeros(3) , eye(3)];
H = Hx*Xx;
detZ_a = [ 2*(q(2)*q(4) - q(1)*q(3)) + measurements_k.acc(1)
2*(q(1)*q(2) + q(3)*q(4)) + measurements_k.acc(2)
2*(0.5 - q(2)^2 - q(3)^2) + measurements_k.acc(3)];
% detZ = detZ_a;
detZ_m =[((2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))) + measurements_k.mag(1))
((2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))) + measurements_k.mag(2))
((2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)) + measurements_k.mag(3))];
detZ = [detZ_a;detZ_m];
end
function [roll,pitch,yaw] = quattoeuler(q)
rad2deg=180/pi;
T=[ 1 - 2 * (q(4) *q(4) + q(3) * q(3)) 2 * (q(2) * q(3) +q(1) * q(4)) 2 * (q(2) * q(4)-q(1) * q(3));
2 * (q(2) * q(3)-q(1) * q(4)) 1 - 2 * (q(4) *q(4) + q(2) * q(2)) 2 * (q(3) * q(4)+q(1) * q(2));
2 * (q(2) * q(4) +q(1) * q(3)) 2 * (q(3) * q(4)-q(1) * q(2)) 1 - 2 * (q(2) *q(2) + q(3) * q(3))];%cnb
roll = atan2(T(2,3),T(3,3))*rad2deg;
pitch = asin(-T(1,3))*rad2deg;
yaw = atan2(T(1,2),T(1,1))*rad2deg-8.3;%%这个固定偏差是什么鬼
yaw = atan2(T(1,2),T(1,1))*rad2deg;%%这个固定偏差是什么鬼
end
参考文献：
Quaternion kinematics for the error-state Kalman Filter.pdf ↩︎ ↩︎
https://fzheng.me/2016/11/20/imu_model_eq/ ↩︎