070-地固系下的精对准方案

对于地固系下的姿态与速度微分方程：

$${\dot{C}}_b^e=C_b^e\ast {\Omega }_{eb}^b$$

$${\dot{v}}^e=f_{ib}^e-2{\Omega }_{ie}^e{v}^e+g^e$$

对准时，令速度微分方程中$v^e=0$，那么对于姿态与速度误差方程，参考065-松组合备忘之捷联惯导不难得到：

$${\dot{\phi }}^e=-{\Omega }_{ie}^e{\phi }^e-C_b^{e}d-C_b^e{\epsilon }_g$$

$$\delta {\dot{v}}^e=[(C_b^e{f}_{ib}^b)\times ]{\phi }^e+C_b^{e}b+C_b^e{\epsilon }_a$$

为推导方便，去除观测噪声$\epsilon$，将上述两式改写一下：

$${\dot{\phi }}^e={\phi }^e\times \omega -d^e$$

$$\delta {\dot{v}}^e=f_{ib}^e\times {\phi }^e+b^e$$

其中的等价关系不再特意说明。
将上述姿态误差方程展开：

$$\{\begin{array}{l}{\dot{\varphi }}_x={\omega }_z{\varphi }_y-{\omega }_y{\varphi }_z-d_x\\ {\dot{\varphi }}_y={\omega }_x{\varphi }_z-{\omega }_z{\varphi }_x-d_y\\ {\dot{\varphi }}_z={\omega }_y{\varphi }_x-{\omega }_x{\varphi }_y-d_z\end{array}$$

注意：$\omega =[0;0;{\omega }^e]$，所以：

$$\{\begin{array}{l}{\dot{\varphi }}_x={\omega }_z{\varphi }_y-d_x\\ {\dot{\varphi }}_y=-{\omega }_z{\varphi }_x-d_y\\ {\dot{\varphi }}_z=-d_z\end{array}$$

可见，载体在e系中的z方向失准角不能进行估计；

对速度误差方程进行展开：

$$\{\begin{array}{l}\dot{\delta v_x}={\varphi }_z{f}_y-{\varphi }_y{f}_z+b_x\\ \dot{\delta v_y}={\varphi }_x{f}_z-{\varphi }_z{f}_x+b_y\\ \dot{\delta v_z}={\varphi }_y{f}_x-{\varphi }_x{f}_y+b_z\end{array}$$

静基座下，近似$f^e\approx -g^e$，$g^e=[g_x;g_y;g_z]$，那么：

$$\{\begin{array}{l}\dot{\delta v_x}={\varphi }_y{g}_z-{\varphi }_z{g}_y+b_x\\ \dot{\delta v_y}={\varphi }_z{g}_x-{\varphi }_x{g}_z+b_y\\ \dot{\delta v_z}={\varphi }_x{g}_y-{\varphi }_y{g}_x+b_z\end{array}$$

将b，d扩充为状态，建立初始对准状态空间模型：

$$\{\begin{array}{l}\dot{X}=FX\\ Z=HX\end{array}$$

其中：

$$X=\left[\begin{array}{c}{\varphi }_x\\ {\varphi }_y\\ {\varphi }_z\\ \delta v_x\\ \delta v_y\\ \delta v_z\\ d_x\\ d_y\\ d_z\\ b_x\\ b_y\\ b_z\end{array}\right],F=\left[\begin{array}{cccccccccccc}0& {\omega }_z& 0& 0& 0& 0& -1& 0& 0& 0& 0& 0\\ -{\omega }_z& 0& 0& 0& 0& 0& 0& -1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& -1& 0& 0& 0\\ 0& g_z& -g_y& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ -g_z& 0& g_x& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ g_y& -g_x& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ & & & & & 0_{6\times 12}& & & & & & \end{array}\right]$$

$$H=\left[\begin{array}{cccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$$

系统的可观测性矩阵为：

$$O=\left[\begin{array}{c}H\\ HF\\ ⋮\\ H{F}^{11}\end{array}\right]=\left[\begin{array}{cccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& g_z& -g_y& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ -g_z& 0& g_x& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ g_y& -g_x& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ -g_z{\omega }_z& 0& 0& 0& 0& 0& 0& -g_z& g_y& 0& 0& 0\\ 0& -g_z{\omega }_z& 0& 0& 0& 0& g_z& 0& -g_x& 0& 0& 0\\ g_x{\omega }_z& g_y{\omega }_z& 0& 0& 0& 0& -g_y& g_x& 0& 0& 0& 0\\ 0& -g_z{\omega }_z^2& 0& 0& 0& 0& g_z{\omega }_z& 0& 0& 0& 0& 0\\ g_z{\omega }_z^2& 0& 0& 0& 0& 0& 0& g_z{\omega }_z& 0& 0& 0& 0\\ -g_y{\omega }_z^2& g_x{\omega }_z^2& 0& 0& 0& 0& -g_x{\omega }_z& -g_y{\omega }_z& 0& 0& 0& 0\\ & & & & & & ⋮& & & & & \end{array}\right]$$

系统的状态为12维，通过可观测性判断矩阵O可知，其12行各行均不相关，即$rank(O)\ge 12$，所以状态向量X中各个状态均可观测。关于系统的可观测性判断可参考：069-线性系统的可控性和可观测性

建立12维随机系统模型如下：

$$\{\begin{array}{l}\dot{X}=FX+G{W}^b\\ Z=HX+V\end{array}$$

其中：

$$X=\left[\begin{array}{c}\phi \\ \delta v\\ d\\ b\end{array}\right],F=\left[\begin{array}{cccc}-{\Omega }_{ie}^e& O_{3\times 3}& -C_b^e& O_{3\times 3}\\ -g^e\times & O_{3\times 3}& O_{3\times 3}& C_b^e\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& O_{3\times 3}\end{array}\right],G=\left[\begin{array}{cc}-C_b^e& O_{3\times 3}\\ O_{3\times 3}& C_b^e\\ O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}\end{array}\right],W^b=\left[\begin{array}{c}{\epsilon }_g\\ {\epsilon }_a\end{array}\right]$$

$$Z=\left[\begin{array}{c}v\end{array}\right],H=\left[\begin{array}{cccc}{O}_{3\times 3}& I_{3\times 3}& O_{3\times 3}& O_{3\times 3}\end{array}\right],V=\left[\begin{array}{c}{\epsilon }_v\end{array}\right]$$

当然，一般也可以将F表示为：

$$F=\left[\begin{array}{cccc}-{\Omega }_{ie}^e& O_{3\times 3}& -C_b^e& O_{3\times 3}\\ f_{ib}^e\times & O_{3\times 3}& O_{3\times 3}& C_b^e\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& O_{3\times 3}\end{array}\right]$$

组合一下很容易发现，该对准模型就是前面065-松组合备忘之捷联惯导得出的松组合卡尔曼滤波算法的简化版。

---

测试结果

同068-地固系下的粗对准方案的结果对比，效果差不多。