文章目录
常用方法
- 一阶泰勒展开:exp(x) = 1 + x
一、惯性导航备注
1)坐标系之间换算
一般有两个坐标系:大地基准坐标系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是可逆矩阵。
2)几种表示姿态的方法
1. 四元数表示角度
更新
可以这样表示:
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]×]
2. 李代数表示角度
旋转向量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
3. 链式求导方法
这个方法好像在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上的一些代码。
1)科式加速度
科式加速度的表示
参考网页;
科式加速度主要用来描述参考系与惯性系之间的关系。
我们来逐项分析上面这个式子。第一项中 αα 为 {B} 的角加速度,所以第一项的物理意义是 {B} 旋转所造成的 P 的切向加速度。第二项是 {B} 旋转所造成的向心加速度。第四项为 P 相对于 {B} 的加速度,但在惯性系 {A} 下表达——类似于 vrvr,定义相对加速度 arar。第三项比较特殊,为 {B} 的旋转运动与 P 相对 {B} 的平移运动耦合产生的加速度,称为「科氏加速度」。可以看到,除了第四项外,另外三项都和 {B} 的旋转有关。
什么时候能够用到科式加速度呢?
在MSCKF1中,考虑的是参考系(b),因此需要考虑;但在MSCKF2中,考虑的是惯性系(a),所以就不需要考虑科式加速度了。
这里存在一个问题:既然不考虑科式加速度又简便也符合逻辑,那么为什么一些文献中采用考虑科式加速度的做法呢?这个问题保留。
2)误差状态更新方法
惯性导航算法常常用误差状态量来表示。实际上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量。
3)卡尔曼滤波算法部分
1. 确定方差矩阵
算法部分常用卡尔曼滤波,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为标准。
2. 如何确定协方差矩阵的更新?
要推导预积分量的协方差,我们需要知道 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只是在定义误差量的过程噪声,所以是比较合理的。
3. 如何确定控制量u和状态量x?
最终在MEKF的博客中找到答案;
在文献1中找到下面的佐证(附录部分)。下面的差分化是利用了第二种状态方程的递推方法,然后进行离散化。
由此便得知协方差Q矩阵的确定过程。实际上在实验中,并没有特别严格的定义,实际上两种都可以。
4. 为什么要用误差状态量去表示呢?
这部分参考Quaternion kinematics for the error-state Kalman filte和博客。
- 定向误差状态是最小的,避免了与过度参数化相关的问题,以及相关协方差矩阵奇异性的风险,这通常是由强制约束引起的
- 误差状态系统总是在接近原始系统的情况下运行,因此远离可能的参数奇点、万向锁问题等,从而保证线性化的有效性始终保持不变
- 错误状态总是很小,这意味着所有二阶部分都可以忽略不计。这使得雅可比矩阵的计算变得非常简单和快速。有些雅可比数甚至可能是常数或等于可用状态量。
- 误差动力学是缓慢的,因为所有的大信号动力学都已集成到标称状态。这意味着我们可以以低于预测的速度应用KF修正
什么是动力学是缓慢的?变化是缓慢的?是不是用来描述振动变化,有着更为出色的性能?如果是的话,这也是一个可以发论文的点。是不是和李代数去表示是有关系的?只有在小量的时候,在切空间的性质才可以成立。
5. ESKF完整内容
主要参考文献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⎦⎤
总结来说,求取差分的偏导数如下,需要注意的是,我用到的是左栏的结果,右栏中未作考虑。
4)kalman滤波实例
参考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滤波更新即可。
5)实现代码
参考项目;
% =========================================================================
%
% 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
参考文献: