惯导系统模型及其仿真（五）

2.编程实现–位置解算

2.2 位置解算

惯性器件均以增量形式输出量测信息，同时，之前的姿态和速度解算也只能给出更新时间点上的信息，都是离散的形式。
所以，位置解也只有离散解，只能根据离散时间点上的速度、姿态、角增量、速度增量求取。
【注】：本编程实现中，位置更新周期$[t_{l-1},t_l]$与速度更新周期$[t_{m-1},t_m]$、姿态更新周期$[t_{k-1},t_k]$相同，均为6倍的采样时间间隔。

2.2.1 算法原理

位置解算的目标是求得 $t_l$ 时刻的经纬度信息，即，$L_l,\lambda_l$.
设 $t_{l-1}$时刻的经纬度信息已知，记为 $L,\lambda$
则，根据经纬度和位置矩阵的对应关系，可以写出位置矩阵$C^n_e$

$$C^{n(l-1)}_e=\begin{bmatrix} -sin\lambda & cos \lambda & 0\\ -sinL cos\lambda &-sinLsin\lambda &cosL\\ cosLcos\lambda & cosLsin\lambda & sinL \end{bmatrix}$$
假设，又已知 $[t_{l-1},t_l]$时间段内坐标系 $n(l-1)$到 $n(l)$的旋转矢量 $\xi_l$
则， $C^{n(l-1)}_{n(l)}=I+(\xi_l\times)$，（此处和速度解算时一样取了近似，认为 $\xi_l$是小量）
亦即，
$$C^{n(l)}_{n(l-1)}=I-(\xi_l\times)$$
这样， $t_l$ 时刻的位置矩阵为：
$$C^{n(l)}_e=C^{n(l)}_{n(l-1)}C^{n(l-1)}_e$$
最后，根据 经纬度和位置矩阵的对应关系，可以解出 $t_l$时刻的经纬度信息。

现在问题的关键落到，$[t_{l-1},t_l]$坐标系$n(l-1)$到$n(l)$的旋转矢量$\xi_l$的求解上。

直观上可以想象，$n(l-1)$到$n(l)$的旋转矢量$\xi_l$，就是$[t_{l-1},t_l]$内位置速率$\omega_{en}$的积分，即：

$$\xi_l=\int^{t_l}_{t_{l-1}}\omega^{n}_{en}dt$$

• 其中，（非极区、选用地理坐标系作为导航坐标系）
  $$\omega^n_{en}=\begin{bmatrix} -\frac{V_N}{R_M+h}\\ \frac{V_E}{R_N+h}\\ \frac{V_E}{R_N+h}tanL \end{bmatrix}=F(t)V^n(t)\\ =\begin{bmatrix} 0 & -\frac{1}{R_M+h} & 0\\ \frac{1}{R_N+h} &0 &0\\ \frac{1}{R_N+h}tanL &0 &0 \end{bmatrix}\begin{bmatrix} V_E\\ V_N\\ V_U \end{bmatrix}$$
  且，认为在位置更新周期里，纬度变化不大，将$F(t)$以$[t_{l-1},t_l]$的中间时刻对应的纬度$\hat L_{(l-1,l)/2}$计算所得的常值矩阵$\hat F_{(l-1,l)/2}$作为其估计值。
  其中，$\hat L_{(l-1,l)/2}=\hat L_{l-1}+\frac{1}{2}(\hat L_{l-1}-\hat L_{l-2})$，即线性外推值。

这样，积分变为：

$$\xi_l=\int^{t_l}_{t_{l-1}}\omega^{n}_{en}dt\approx \hat F_{(l-1,l)/2}\int^{t_l}_{t_{l-1}}V^n(t)dt\\ =\hat F_{(l-1,l)/2} \Delta R^n_l$$

现在问题的关键落到，$\Delta R^n_l$的求解上。

理论上，可以经过$M$次速度更新($T_m$速度更新周期)后进行一次位置更新（计算量更小些），即

$$\Delta R^n_l=\int^{t_l}_{t_{l-1}}V^n(t)dt=\sum^{M}_{m=1}\int^{t_{l-1}+mT_m}_{t_{l-1}+(m-1)T_m}V^n(t)dt\\ =\sum^{M}_{m=1}\Delta R^n_m$$