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

2.编程实现–位置解算

2.3 位置解算

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

2.3.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$，就是