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