捷联惯导系统学习7.3(惯性/卫星组合导航 )

全球卫星组合导航系统：GPS（美国全球定位系统）、GLONASS（俄罗斯格洛纳斯）、BDS（中国北斗）、GALILEO(欧盟伽利略)。
系统构成：卫星、地面测控站、用户设备
卫星信号微弱，极易受到干扰，但提供的位置误差不会随时间积累，可以与惯导系统互补，发挥两种系统优点。

1. 卫星与惯导存在误差
   惯导（IMU）是以传感器几何中心作为导航定位和测速的基准的;
   卫星导航是以接收天线的相位中心作为参考点的。
   两者之间存在的偏差称为空间杆臂误差
   

2. $$\begin{gathered} R:惯组对于地心的矢量 \\ r:卫星天线中心对于地心的矢量 \\ \delta l:天线中心对于惯组集合中心的矢量 \end{gathered}$$
   $$R=r+\delta l$$

3. 天线和惯组之间的安装位置一般显贵固定不动，即$\delta l$在b坐标系下为常矢量，对地球坐标系求导可得：
   $$\frac{dr}{dt}{|}_e=\frac{dR}{dt}{|}_e+\frac{d(\delta l)}{dt}{|}_e=\frac{dR}{dt}{|}_e+\frac{d(\delta l)}{dt}{|}_b+w_{eb}\times \delta l=\frac{dR}{dt}{|}_e+w_{eb}\times \delta l$$
   即：
   $v_{en(GNSS)}=\frac{dr}{dt}{|}_e:卫星天线的地速$
   $v_{en(INS)}=\frac{dR}{dt}{|}_e:惯导的地速$
   $$v_{en(GNSS)}=v_{en(INS)}+w_{eb}\times \delta l$$

4. 由于$v_{en(GNSS)},v_{en(INS)}$两种地速定义的导航坐标系之间的误差很小，可以忽略不计，可以得到：
   $$v_{en(GNSS)}^n=v_{INS}^n+C_b^n(w_{eb}^b\times \delta l^b)$$
   实际中由于$w_{ie},w_{en}$影响很小，可以近似$w_{ib}^b\approx w_{ib}^b$或$w_{eb}^b=w_{nb}^b$

5. 得到杆臂速度误差：
   $$\delta v_L^n=v_{en(GNSS)}^n-v_{INS}^n=C_b^n(w_{eb}^b\times \delta l^b)=C_b^n(w_{eb}^b\times )\delta l^b$$

6. 惯导与卫星天线之间的位置误差可进似看为：
   $\delta l=\left[\begin{array}{ccc}\delta l_E& \delta l_N& \delta l_U\end{array}\right]=C_b^n\delta l^b$
   $R_{Mh}:子午圈主曲率半径$
   $R_{Nh}:卯酉圈主曲率半径$
   $$\{\begin{array}{l}{L}_{INS}-L_{GNSS}=-\delta l_N/R_{Mh}\\ {\lambda }_{INS}-{\lambda }_{GNSS}=-\delta l_{E}sec{L}_{INS}/R_{Nh}\\ h_{INS}-h_{GNSS}=-\delta l_U\end{array}$$

7. 从而得到惯导与卫导之间的杆臂误差向量为：
   $P_{INS}={\left[\begin{array}{ccc}{L}_{INS}& {\lambda }_{INS}& h_{INS}\end{array}\right]}^T$
   $P_{GNSS}={\left[\begin{array}{ccc}{L}_{GNSS}& {\lambda }_{GNSS}& h_{GNSS}\end{array}\right]}^T$
   $M_{pv}=\left[\begin{array}{ccc}0& 1/R_{Mh}& 0\\ secL/R_{Nh}& 0& 0\\ 0& 0& 1\end{array}\right]$
   $$\delta P_{GL}=P_{INS}-P_{GNSS}=-M_{pv}{C}_b^n\delta l^b$$

时间不同步误差

1. 卫星导航与惯导坐标系实际使用的数据，往往因为解算和传输，往往存在一定时间的滞后性，我们将其之间的时间误差记为$\delta t$
   

2. 卫星与惯导之间的对应关系如下：
   $a^n不同步附近的平均线加速度，通常用相邻时间内的平均变化表示,a^n\approx \frac{v_{INS(m)}^n-v_{INS(m-1)}^n}{T}.$
   $$v_{GNSS}^n+a^n\delta t=v_{INS}^n$$

3. 得到不同步速度误差$\delta v_{\delta t}^n$和位置误差$\delta P_{\delta t}$
   $$\begin{gathered} \delta v_{\delta t}^n=v_{INS}^n-v_{GNSS}^n=a^n\delta t \\ \delta P_{\delta t}=P_{INS}-P_{GNSS}=M_{pv}{v}_{INS}^n\delta t \end{gathered}$$

状态空间模型

综合考虑杆臂误差和时间不同步误差，可以得到速度和位置误差作为观测量的惯性/卫星组合导航状态空间方程：
$W_g^b:角速度测量噪声$
$W_a^b:比力测量白噪声$
$V_v:卫星接收机白噪声$
$V_p:位置测量白噪声$
$M_{aa}=-w_{in}^n惯导系计算值$
$M_{av}=\left[\begin{array}{ccc}0& -1/R_{Mh}& 0\\ 1/R_{Nh}& 0& 0\\ tanL/R_{Nh}& 0& 0\end{array}\right]$
$$\begin{gathered} M_{ap}=\left[\begin{array}{ccc}0& 0& 0\\ w_{ie}sinL& 0& 0\\ -w_{ie}sinL& 0& 0\end{array}\right]+\left[\begin{array}{ccc}0& 0& v_N/R_{Mh}^2\\ 0& 0& -v_E/R_{Nh}^2\\ 0v_{E}se{c}^{2}L/R_{Nh}& 0& -v_{E}tanL/R_{Nh}^2\end{array}\right] \\ {M}_{va}=(f_{sf}^n\times ) \\ {M}_{vv}=(v^n\times )M_{av}-[2w_{ie}^n+w_{en}^n\times ] \\ {M}_{vp}=(v^n\times )(2\left[\begin{array}{ccc}0& 0& 0\\ w_{ie}sinL& 0& 0\\ -w_{ie}sinL& 0& 0\end{array}\right]+\left[\begin{array}{ccc}0& 0& v_N/R_{Mh}^2\\ 0& 0& -v_E/R_{Nh}^2\\ 0v_{E}se{c}^{2}L/R_{Nh}& 0& -v_{E}tanL/R_{Nh}^2\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ -2{\beta }_{3}hcos2L& 0& -{\beta }_{3}sin2L\\ -g_0(\beta -4{\beta }_{1}cos2L)sin2L& 0& {\beta }_2\end{array}\right]) \\ {M}_{pv}=\left[\begin{array}{ccc}0& 1/R_{Mh}& 0\\ v_{E}secL/R_{Nh}& 0& 0\\ 0& 0& 1\end{array}\right] \\ {M}_{pp}=\left[\begin{array}{ccc}0& 0& -v_n/R_{Mh}^2\\ v_{E}secLtanL/R_{Nh}& 0& -v_{E}secL/R_{Nh}^2\\ 0& 0& 0\end{array}\right] \end{gathered}$$
$$\begin{gathered} \hat{X}=FX+G{W}^b \\ Z=\left[\begin{array}{c}{\tilde{v}}_{INS}^n-{\tilde{v}}_{GNSS}^n\\ {\tilde{P}}_{INS}-{\tilde{P}}_{GNSS}\end{array}\right]=HX+V \\ X={\left[\begin{array}{ccccccc}(\varphi {)}^T& (\delta v^n{)}^T& (\delta p{)}^T& ({\xi }^b{)}^T& ({▽}^b{)}^T& (\delta l^b{)}^T& \delta t\end{array}\right]}^T \\ F=\left[\begin{array}{cccccc}{M}_{aa}& M_{av}& M_{ap}& -C_b^n& 0_{3\times 3}& 0_{3\times 4}\\ M_{va}& M_{vv}& M_{vp}& 0_{3\times 3}& C_b^n& 0_{3\times 4}\\ 0_{3\times 3}& M_{pv}& M_{pp}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 4}\\ 0_{10\times 19}& & & & & \end{array}\right]，G=\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{13\times 6}& \end{array}\right],W^b=\left[\begin{array}{c}{W}_g^b\\ W_a^b\end{array}\right] \\ H=\left[\begin{array}{cccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 6}& -C_b^n(w_{eb}^b\times )& a^n\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}& 0_{3\times 6}& -M_{pv}{C}_b^n& M_{pv}{v}_{INS}^n\end{array}\right],V=\left[\begin{array}{c}{V}_v\\ V_p\end{array}\right] \end{gathered}$$