捷联惯导系统学习7.5(低成本组合导航系统模型)

在静止条件下，运载体的线运动及其导数均为零： 0 = C b n f s b + g n 0=C^n_bf^b_s+g^n 0=Cbn​fsb​+gn

为了减小测量噪声和外界晃动干扰，常使用一小段时间内的平均比力进行运算,对式1进行移项可得： f s f b = − ( C b n ) T g n f^b_{sf}=-(C^n_b)^Tg^n fsfb​=−(Cbn​)Tgn 展开为: g : 当 地 重 力 大 小 g:当地重力大小 g:当地重力大小 [ f s f x b f s f y b f s f z b ] = − [ C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 ] [ 0 0 − g ] \left[\begin{matrix} f^b_{sfx}\\ f^b_{sfy}\\ f^b_{sfz}\\ \end{matrix}\right]=-\left[\begin{matrix} C_{11}&C_{12}&C_{13}\\ C_{21}&C_{22}&C_{23}\\ C_{31}&C_{32}&C_{33}\\ \end{matrix}\right]\left[\begin{matrix} 0\\0\\-g \end{matrix}\right] ⎣⎡​fsfxb​fsfyb​fsfzb​​⎦⎤​=−⎣⎡​C11​C21​C31​​C12​C22​C32​​C13​C23​C33​​⎦⎤​⎣⎡​00−g​⎦⎤​ 得到: [ C 31 C 32 C 33 ] = [ f s f x b / g f s f y b / g f s f z b / g ] \left[\begin{matrix} C_{31}\\C_{32}\\C_{33} \end{matrix}\right]=\left[\begin{matrix} f^b_{sfx}/g\\f^b_{sfy}/g\\f^b_{sfz}/g \end{matrix}\right] ⎣⎡​C31​C32​C33​​⎦⎤​=⎣⎡​fsfxb​/gfsfyb​/gfsfzb​/g​⎦⎤​ C b n C^n_b Cbn​第三行向量为归一化重力矢量在载体坐标下的恶投影，前两行元素无法求取

构造 C b n C^n_b Cbn​前两行的姿态阵，构造方法如下: 记姿态阵 C b n C^n_b Cbn​的三个行向量分别为: C 1 , C 2 , C 3 C_1,C_2,C_3 C1​,C2​,C3​即有 C b n = [ ( C 1 ) T ( C 2 ) T ( C 3 ) T ] C^n_b=\left[\begin{matrix}(C_1)^T&(C_2)^T&(C_3)^T\end{matrix}\right] Cbn​=[(C1​)T​(C2​)T​(C3​)T​] (1)构造: C 3 = [ C 31 C 32 C 33 ] = ( f s f b ) / ∣ f s f b ∣ C_3=\left[\begin{matrix}C_{31}&C_{32}&C_{33}\end{matrix}\right]=(f^b_{sf})/|f^b_{sf}| C3​=[C31​​C32​​C33​​]=(fsfb​)/∣fsfb​∣ (2) C 3 C_3 C3​为3维单位向量，其最大元素值应当不小于 3 / 3 ≈ 0.5 \sqrt{3}/3\approx 0.5 3 ​/3≈0.5,当 ∣ C 31 ∣ > 0.5 |C_{31}|>0.5 ∣C31​∣>0.5时，构造 C 2 ′ = [ C 32 − C 31 0 ] C_2'=\left[\begin{matrix}C_{32}&-C_{31}&0\end{matrix}\right] C2′​=[C32​​−C31​​0​]，否则构造 C 2 ′ = [ 0 C 33 − C 32 ] C_2'=\left[\begin{matrix}0&C_{33}&-C_{32}\end{matrix}\right] C2′​=[0​C33​​−C32​​],显然, C 2 ′ C 3 T C_2'C_3^T C2′​C3T​与原向量正交，再单位化 C 2 = C 2 ′ / ∣ C 2 ′ ∣ C_2=C'_2/|C'_2| C2​=C2′​/∣C2′​∣ (3)构造 C 1 = C 2 × C 3 C_1=C_2×C_3 C1​=C2​×C3​ 在地磁测量校准中会用到， C b h = C b n C_b^h=C_b^n Cbh​=Cbn​h表示当地水平坐标系