2.编程实现–位置解算
2.2 位置解算
惯性器件均以增量形式输出量测信息,同时,之前的姿态和速度解算也只能给出更新时间点上的信息,都是离散的形式。
所以,位置解也只有离散解,只能根据离散时间点上的速度、姿态、角增量、速度增量求取。
【注】:本编程实现中,位置更新周期
[tl−1,tl]
与速度更新周期
[tm−1,tm]
、姿态更新周期
[tk−1,tk]
相同,均为6倍的采样时间间隔。
2.2.1 算法原理
位置解算的目标是求得
tl
时刻的经纬度信息,即,
Ll,λl
.
设
tl−1
时刻的经纬度信息已知,记为
L,λ
则,根据经纬度和位置矩阵的对应关系,可以写出位置矩阵
Cne
Cn(l−1)e=⎡⎣⎢−sinλ−sinLcosλcosLcosλcosλ−sinLsinλcosLsinλ0cosLsinL⎤⎦⎥
假设,又已知
[tl−1,tl]
时间段内坐标系
n(l−1)
到
n(l)
的旋转矢量
ξl
则,
Cn(l−1)n(l)=I+(ξl×)
,(此处和速度解算时一样取了近似,认为
ξl
是小量)
亦即,
Cn(l)n(l−1)=I−(ξl×)
这样,
tl
时刻的位置矩阵为:
Cn(l)e=Cn(l)n(l−1)Cn(l−1)e
最后,根据
经纬度和位置矩阵的对应关系,可以解出
tl
时刻的经纬度信息。
现在问题的关键落到,
[tl−1,tl]
坐标系
n(l−1)
到
n(l)
的旋转矢量
ξl
的求解上。
直观上可以想象,
n(l−1)
到
n(l)
的旋转矢量
ξl
,就是
[tl−1,tl]
内位置速率
ωen
的积分,即:
ξl=∫tltl−1ωnendt
这样,积分变为:
ξl=∫tltl−1ωnendt≈F^(l−1,l)/2∫tltl−1Vn(t)dt=F^(l−1,l)/2ΔRnl
现在问题的关键落到,
ΔRnl
的求解上。
理论上,可以经过
M
次速度更新(
Tm
速度更新周期)后进行一次位置更新(计算量更小些),即
ΔRnl=∫tltl−1Vn(t)dt=∑m=1M∫tl−1+mTmtl−1+(m−1)TmVn(t)dt=∑m=1MΔRnm