捷联惯导系统学习7.5(地磁场测量及误差方程 )

地磁北向与地理北向一般不重合，它们之间的夹角称为磁偏角，在我国一般为2～3度，最大可为10度。
地磁场为矢量场，强度大小为： 0.5 ∼ 0.6 G s ( 1 G s = 1 0 − 4 T ( 特 斯 拉 ) ) 0.5\sim 0.6Gs(1Gs=10^{-4}T(特斯拉)) 0.5∼0.6Gs(1Gs=10−4T(特斯拉))，顺着地磁方向为正。

上图中：
O X m , O Y m ： 指 向 磁 场 方 向 , O Z m OX_m,OY_m：指向磁场方向,OZ_m OXm​,OYm​：指向磁场方向,OZm​为磁传感器坐标轴
O X n , O Y n , O Z n OX_n,OY_n,OZ_n OXn​,OYn​,OZn​:地理坐标系
O X m , O Y m , O X n OX_m,OY_m,OX_n OXm​,OYm​,OXn​:共平面

1. 假设在小范围区域，此时地场矢量H可以当作矢量来看待，此时有地磁在m系的投影为: H m = [ 0 H 0 ] T H^m=\left[\begin{matrix} 0&H&0\end{matrix}\right]^T Hm=[0​H​0​]T

2. 磁倾角： η x : H 与 Y n O X n \eta_x:H与Y_nOX_n ηx​:H与Yn​OXn​之间的夹角
   磁偏角: η z : 地 理 北 向 与 H 的 水 平 线 之 间 的 夹 角 \eta_z:地理北向与H的水平线之间的夹角 ηz​:地理北向与H的水平线之间的夹角

3. o x n y n z n 绕 o z n 旋 转 − η z 角 度 , 再 绕 o x m 轴 旋 转 − η x 角 得 到 o x m y m z m ox_ny_nz_n绕oz_n旋转-\eta_z角度,再绕ox_m轴旋转-\eta_x角得到ox_my_mz_m oxn​yn​zn​绕ozn​旋转−ηz​角度,再绕oxm​轴旋转−ηx​角得到oxm​ym​zm​
   得到变换矩阵为：
   C m n = C − η z C − n η x = [ c o s η z s i n η z 0 − s i n η z c o s η z 0 0 0 1 ] [ 1 0 0 0 c o s η x s i n η x 0 − s i n η x c o s η x ] = [ c o s η x s i n η x c o s η x s i n η x s i n η x − s i n η x c o s η x c o s η x c o s η x s i n η x 0 − s i n η x c o s η x ] C^n_m=C_{-\eta_z}C_{-n\eta_x}=\left[\begin{matrix} cos_{\eta_z}&sin_{\eta_z}&0\\ -sin_{\eta_z}&cos_{\eta_z}&0\\ 0&0&1 \end{matrix}\right]\left[\begin{matrix} 1&0&0\\ 0&cos_{\eta_x}&sin_{\eta_x}\\ 0&-sin_{\eta_x}&cos_{\eta_x} \end{matrix}\right]=\left[\begin{matrix} cos_{\eta_x}&sin_{\eta_x}cos_{\eta_x}&sin_{\eta_x}sin_{\eta_x}\\ -sin_{\eta_x}&cos_{\eta_x}cos_{\eta_x}&cos_{\eta_x}sin_{\eta_x}\\ 0&-sin_{\eta_x}&cos_{\eta_x} \end{matrix}\right] Cmn​=C−ηz​​C−nηx​​=⎣⎡​cosηz​​−sinηz​​0​sinηz​​cosηz​​0​001​⎦⎤​⎣⎡​100​0cosηx​​−sinηx​​​0sinηx​​cosηx​​​⎦⎤​=⎣⎡​cosηx​​−sinηx​​0​sinηx​​cosηx​​cosηx​​cosηx​​−sinηx​​​sinηx​​sinηx​​cosηx​​sinηx​​cosηx​​​⎦⎤​

4. 在 实 际 中 η x , η z 在实际中\eta_x,\eta_z 在实际中ηx​,ηz​难以精确获得，使用 δ η x = η ~ x − η x , δ η z = η ~ z − η z \delta \eta_x=\tilde \eta_x-\eta_x,\delta \eta_z=\tilde \eta_z-\eta_z δηx​=η~​x​−ηx​,δηz​=η~​z​−ηz​,这里记为m’系:
   C m m ′ = C − η z C − n η x = [ c o s δ η x s i n δ η x c o s δ η x s i n δ η x s i n δ η x − s i n δ η x c o s δ η x c o s δ η x c o s δ η x s i n δ η x 0 − s i n δ η x c o s δ η x ] ≈ [ 1 δ η z 0 − δ η z 1 δ η z 0 − δ η x 1 ] = I − ( δ η × ) η = [ δ η x 0 δ η z ] T C_m^{m'}=C_{-\eta_z}C_{-n\eta_x}=\left[\begin{matrix} cos_{\delta\eta_x}&sin_{\delta\eta_x}cos_{\delta\eta_x}&sin_{\delta\eta_x}sin_{\delta\eta_x}\\ -sin_{\delta\eta_x}&cos_{\delta\eta_x}cos_{\delta\eta_x}&cos_{\delta\eta_x}sin_{\delta\eta_x}\\ 0&-sin_{\delta\eta_x}&cos_{\delta\eta_x} \end{matrix}\right] \approx \left[\begin{matrix} 1&\delta \eta_z&0\\-\delta \eta_z&1&\delta \eta_z\\ 0&-\delta \eta_x&1 \end{matrix}\right]\\ =I-(\delta \eta×)\\ \eta=\left[\begin{matrix} \delta \eta_x&0&\delta \eta_z \end{matrix}\right]^T Cmm′​=C−ηz​​C−nηx​​=⎣⎡​cosδηx​​−sinδηx​​0​sinδηx​​cosδηx​​cosδηx​​cosδηx​​−sinδηx​​​sinδηx​​sinδηx​​cosδηx​​sinδηx​​cosδηx​​​⎦⎤​≈⎣⎡​1−δηz​0​δηz​1−δηx​​0δηz​1​⎦⎤​=I−(δη×)η=[δηx​​0​δηz​​]T

5. 磁传感器与IMU之间会存在夹角
   γ = [ γ x γ y γ z ] T \gamma=\left[\begin{matrix} \gamma_x&\gamma_y&\gamma_z \end{matrix}\right]^T γ=[γx​​γy​​γz​​]T
   C b b m ≈ I + ( γ × ) = [ 1 − γ z γ y γ z 1 − γ x − γ y γ x 1 ] C_b^{b_m}\approx I+(\gamma×)= \left[\begin{matrix} 1&-\gamma_z&\gamma_y\\\gamma_z&1&-\gamma_x\\ -\gamma_y&\gamma_x&1 \end{matrix}\right] Cbbm​​≈I+(γ×)=⎣⎡​1γz​−γy​​−γz​1γx​​γy​−γx​1​⎦⎤​

6. 磁传感器测量得到的数据为在 b m b_m bm​系下的投影记: H ~ b m \tilde H^{b_m} H~bm​,得到磁场矢量的误差为:
   C ~ b n : 捷 联 惯 导 提 供 的 姿 态 矩 阵 计 算 值 \tilde C_b^n:捷联惯导提供的姿态矩阵计算值 C~bn​:捷联惯导提供的姿态矩阵计算值
   δ H n = C ~ b n H ~ b m − C m ′ m H m ≈ C ~ b n C b b m H ~ b − C m n C m ′ m H m ≈ ( I − ϕ × ) C b n ( I + γ × ) H ~ b − C m n ( I − δ η × ) H m ≈ [ C b n H ~ b + ( C b n H ~ b ) × ϕ − ( C b n H ~ b ) × ( C b n γ ) ] − [ H n + H n × ( C m n δ η ) ] \delta H^n=\tilde C^n_b \tilde H^{b_m}-C^m_{m'}H^m \approx \tilde C^n_bC_b^{b_m} \tilde H^{b}-C^n_mC^m_{m'}H^m\\ \approx (I-\phi×)C^n_b(I+\gamma×)\tilde H^b-C^n_m(I-\delta \eta×)H^m\\ \approx [C_b^n\tilde H^b+(C^n_b\tilde H^b)×\phi-(C^n_b\tilde H^b)×(C^n_b\gamma)]-[H^n+H^n×(C^n_m\delta \eta)] δHn=C~bn​H~bm​−Cm′m​Hm≈C~bn​Cbbm​​H~b−Cmn​Cm′m​Hm≈(I−ϕ×)Cbn​(I+γ×)H~b−Cmn​(I−δη×)Hm≈[Cbn​H~b+(Cbn​H~b)×ϕ−(Cbn​H~b)×(Cbn​γ)]−[Hn+Hn×(Cmn​δη)]

7. 当忽略传感器的测量误差(零偏和比力系数) H n ≈ C b n H ~ b H^n \approx C^n_b \tilde H^b Hn≈Cbn​H~b
   将第6步公式装化为： δ H n = C ~ b n H ~ b m − C m ′ m H m = H n × ( ϕ − C b n γ − C m n δ η ) \delta H^n=\tilde C^n_b \tilde H^{b_m}-C^m_{m'}H^m=H^n×(\phi-C^n_b\gamma-C^n_m\delta\eta) δHn=C~bn​H~bm​−Cm′m​Hm=Hn×(ϕ−Cbn​γ−Cmn​δη)

8. H = ∣ H ~ b m ∣ = ∣ H m ∣ = ∣ H n ∣ H=|\tilde H^{b_m}|=|H^m|=|H^n| H=∣H~bm​∣=∣Hm∣=∣Hn∣
   h ~ b m = H ~ b m / ∣ H ~ b m ∣ : 归 一 化 磁 场 测 量 矢 量 \tilde h^{b_m}=\tilde H^{b_m}/|\tilde H^{b_m}|:归一化磁场测量矢量 h~bm​=H~bm​/∣H~bm​∣:归一化磁场测量矢量
   h n = H n / ∣ H n ∣ h^n=H^n/|H^n| hn=Hn/∣Hn∣
   e 2 = [ 0 1 0 ] T , o y m ： 单 位 矢 量 e_2=\left[\begin{matrix} 0&1&0 \end{matrix}\right]^T,oy_m：单位矢量 e2​=[0​1​0​]T,oym​：单位矢量
   Z H = δ H n / H = C ~ b n h ~ b m − C m ′ m e 2 = h n × ( ϕ − C b n γ − C m n δ η ) Z_H=\delta H^n/H\\ =\tilde C^n_b\tilde h^{b_m}-C^m_{m'}e_2\\ =h^n×(\phi-C^n_b\gamma-C^n_m\delta \eta) ZH​=δHn/H=C~bn​h~bm​−Cm′m​e2​=hn×(ϕ−Cbn​γ−Cmn​δη)

9. 如果IMU与磁传感器坐标系重合，安装误差经过了失准角补偿(或安装误差经过了失准角补偿)，简化为:
   γ = δ η = 0 \gamma=\delta\eta=0 γ=δη=0
   Z H = C ~ b n h ~ b − C m n e 2 = h n × ϕ Z_H=\tilde C^n_b\tilde h^b-C^n_me_2=h^n×\phi ZH​=C~bn​h~b−Cmn​e2​=hn×ϕ