写在前面的的话
最近学习组合导航，要对ECL代码有进一步的理解，暂时以此作为笔记
目录
主函数
function [states, correctedDelAng, correctedDelVel] = PredictStates( ...
states, ... % previous state vector (4x1 quaternion, 3x1 velocity, 3x1 position,
3x1 delAng bias, 3x1 delVel bias)
delAng, ... % IMU delta angle measurements, 3x1 (rad) 变化的角度观测值是用来计算四元数的中间值
delVel, ... % IMU delta velocity measurements 3x1 (m/s) 变化的速度值是用来计算速度与位置的中间值
dt, ... % accumulation time of the IMU measurement (sec) IMU测量的累积时间
gravity, ... % acceleration due to gravity (m/s/s) 重力
latitude) % WGS-84 latitude (rad) 维度 用来剪出地球自传速度
计算步骤如下：
- 先计算出改正的速度变化值与角度变化值
- 通过改正后的角度变化值计算四元素预测值
- 通过改正后的速度变化值计算导航坐标系下的位置速度预测值
三个全局变量
Δ
a
n
g
(
k
−
1
)
\Delta_{ang}(k-1)
Δang(k−1)
Δ
v
e
l
(
k
−
1
)
\Delta_{vel}(k-1)
Δvel(k−1)
[
T
]
B
N
(
k
−
1
)
[T]_B^N(k-1)
[T]BN(k−1)
全局变量的初始化
if(
Δ
a
n
g
(
k
−
1
)
\Delta_{ang}(k-1)
Δang(k−1)||
Δ
v
e
l
(
k
−
1
)
\Delta_{vel}(k-1)
Δvel(k−1)||
[
T
]
B
N
(
k
−
1
)
[T]_B^N(k-1)
[T]BN(k−1)为空):
Δ
a
n
g
(
k
−
1
)
=
Δ
a
n
g
\Delta_{ang}(k-1)=\Delta _{ang}
Δang(k−1)=Δang
Δ
v
e
l
(
k
−
1
)
=
Δ
v
e
l
\Delta_{vel}(k-1)=\Delta _{vel}
Δvel(k−1)=Δvel
[
T
]
B
N
(
k
−
1
)
=
Q
u
a
t
2
T
B
N
(
Q
u
a
t
(
k
−
1
)
)
[T]_B^N(k-1)=Quat2T_B^N(Quat(k-1))
[T]BN(k−1)=Quat2TBN(Quat(k−1))
减去ＩＭＵ传感器偏差（速度、角度变化值）
Δ
a
n
g
(
k
−
1
)
=
Δ
a
n
g
(
k
−
1
)
－
Δ
a
n
g
b
i
a
s
(
k
−
1
)
\Delta_{ang}(k-1)=\Delta_{ang}(k-1)－\Delta_{ang}bias(k-1)
Δang(k−1)=Δang(k−1)－Δangbias(k−1)
Δ
v
e
l
(
k
−
1
)
=
Δ
v
e
l
(
k
−
1
)
−
Δ
v
e
l
b
i
a
s
(
k
−
1
)
\Delta_{vel}(k-1)=\Delta _{vel}(k-1)-\Delta_{vel}bias(k-1)
Δvel(k−1)=Δvel(k−1)−Δvelbias(k−1)
改正变化的速度 for rotation and skulling
公式参考 《A sculling digital computation algorithm》式25
ecl代码中将改正注释，并未改正
改正coning 误差和地球自传
地球自转
当我们知道维度与地球自转速度时，便可改正地球自传对NU方向的影响
Δ
a
n
g
E
a
r
N
=
0.000072921
∗
c
o
s
(
l
a
t
i
t
u
d
e
)
∗
d
t
\Delta_{ang}Ear_N=0.000072921 * cos(latitude) * dt
ΔangEarN=0.000072921∗cos(latitude)∗dt
Δ
a
n
g
E
a
r
E
=
0
\Delta_{ang}Ear_E=0
ΔangEarE=0
Δ
a
n
g
E
a
r
U
=
0.000072921
∗
s
i
n
(
l
a
t
i
t
u
d
e
)
∗
d
t
\Delta_{ang}Ear_U=0.000072921 * sin(latitude) * dt
ΔangEarU=0.000072921∗sin(latitude)∗dt
coning 误差
公式参考《A new strapdown attitude algorithm》式11
ecl代码将其注释并未改正
将改正转换到body坐标系下
Δ a n g c o r = Δ a n g − ( [ T ] B N ) T ∗ Δ a n g E a r \Delta_{ang}cor=\Delta_{ang}-([T]_B^N)^T*\Delta_{ang}Ear Δangcor=Δang−([T]BN)T∗ΔangEar
四元素预测
将改正的旋转角度信息转换为四元素
Δ
Q
u
a
t
=
R
o
t
a
2
Q
u
a
t
(
Δ
a
n
g
C
o
r
)
\Delta_{Quat}=Rota2Quat(\Delta_{ang}Cor)
ΔQuat=Rota2Quat(ΔangCor)
Q
u
a
t
(
k
)
−
=
Q
u
a
t
(
k
−
1
)
∗
Δ
Q
u
a
t
Quat(k)^-=Quat(k-1)*\Delta_{Quat}
Quat(k)−=Quat(k−1)∗ΔQuat
四元素的乘法需要再研究
对四元数进行归一化
Q
u
a
t
(
k
)
−
=
Q
u
a
t
(
k
)
−
∣
Q
u
a
t
(
k
)
−
∣
Quat(k)^-=\frac{Quat(k)^-}{|Quat(k)^-|}
Quat(k)−=∣Quat(k)−∣Quat(k)−
四元素的预测完成(公式详见Process and Observation Moder.pdf)
计算旋转矩阵T
T
B
N
(
k
−
1
)
−
=
Q
u
a
t
2
T
B
N
(
Q
u
a
t
(
k
)
−
)
T_B^N(k-1)^-=Quat2T_B^N(Quat(k)^-)
TBN(k−1)−=Quat2TBN(Quat(k)−)
为何不使用平滑后的四元素来计算旋转矩阵
转换速度变化值至导航坐标系
Δ
v
e
l
N
E
D
=
T
B
N
∗
Δ
v
e
l
C
o
r
+
g
∗
d
t
\Delta_{vel}NED=T_B^N*\Delta_{vel}Cor+g*dt
ΔvelNED=TBN∗ΔvelCor+g∗dt
V
e
l
N
E
D
(
k
)
−
=
V
e
l
N
E
D
(
k
−
1
)
+
Δ
v
e
l
N
E
D
Vel_{NED}(k)^-=Vel_{NED}(k-1)+\Delta_{vel}NED
VelNED(k)−=VelNED(k−1)+ΔvelNED
P
o
s
N
E
D
(
k
)
−
=
P
o
s
k
−
1
+
1
/
2
∗
d
t
∗
(
V
e
l
N
E
D
(
k
−
1
)
+
V
e
l
N
E
D
(
k
−
1
)
−
)
Pos_{NED}(k)^-=Pos_{k-1}+1/2*dt*(Vel_{NED}(k-1)+Vel_{NED}(k-1)^-)
PosNED(k)−=Posk−1+1/2∗dt∗(VelNED(k−1)+VelNED(k−1)−)
保存三个全局变量
Δ
a
n
g
(
k
−
1
)
=
Δ
a
n
g
\Delta_{ang}(k-1)=\Delta _{ang}
Δang(k−1)=Δang
Δ
v
e
l
(
k
−
1
)
=
Δ
v
e
l
\Delta_{vel}(k-1)=\Delta _{vel}
Δvel(k−1)=Δvel
[
T
]
B
N
(
k
−
1
)
=
T
B
N
[T]_B^N(k-1)=T_B^N
[T]BN(k−1)=TBN
小结
- Δ a n g \Delta_{ang} Δang经过四个改正：系统偏差改正，地球自传改正，coning改正(未执行)，rotation and skuling 改正（未执行）
- Δ v e l \Delta_{vel} Δvel经过一个改正：系统偏差改正
疑问
- 为什么角度变化值改正时有两个量未改正？
- 既然旋转矩阵不高效，为什么在代码中一直在用 [ T ] B N [T]_B^N [T]BN来旋转角度，直接使用四元素来的会更加高效。
为何不使用平滑后的四元素来计算旋转矩阵- 本预测函数为何只对四元素、位置速度进行预测，未对其他状态量进行预测
761
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
