038解析粗对准学习笔记

粗对准要求在一定的精度范围内尽快地确定一个满足要求的初始姿态矩阵。本文是关于严书解析粗对准所做的笔记。

书中所提的解析粗对准方法，是以双矢量定姿原理为基础的。首先假设r系和b系有：
$$V_1^r=C_b^r{V}_1^b$$

$$V_2^r=C_b^r{V}_2^b$$

那么可以得到两个坐标系的姿态阵：

$$C_b^r={\left[\begin{array}{c}(V_1^r{)}^T\\ \\ (V_2^r{)}^T\\ \\ (V_1^r\times V_2^r{)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}(V_1^b{)}^T\\ \\ (V_2^b{)}^T\\ \\ (V_1^b\times V_2^b{)}^T\end{array}\right]$$

以上是在假定测量没有误差的情况下给出的。但是测量值中会有误差，或者说给出的$V_1^r,V_2^r$和$V_1^b,V_2^b$是不精确的，所以姿态阵不能严格满足单位正交化，故假设对于b系中而言，测量得到的矢量为${\tilde{V}}_1^b$和${\tilde{V}}_2^b$，构造三个单位正交矢量：
$$\frac{{\tilde{V}}_1^b}{|{\tilde{V}}_1^b|},\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b|},\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b|}$$
同样对于r系中的矢量也如此构造，那么可得正交化的姿态阵：
$${\hat{C}}_b^r={\left[\begin{array}{c}(\frac{{\tilde{V}}_1^r}{|{\tilde{V}}_1^r|}{)}^T\\ \\ (\frac{{\tilde{V}}_1^r\times {\tilde{V}}_2^r}{|{\tilde{V}}_1^r\times {\tilde{V}}_2^r|}{)}^T\\ \\ (\frac{{\tilde{V}}_1^r\times {\tilde{V}}_2^r\times {\tilde{V}}_1^r}{|{\tilde{V}}_1^r\times {\tilde{V}}_2^r\times {\tilde{V}}_1^r|}{)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}(\frac{{\tilde{V}}_1^b}{|{\tilde{V}}_1^b|}{)}^T\\ \\ (\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b|}{)}^T\\ \\ (\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b|}{)}^T\end{array}\right]$$

$$=\left[\begin{array}{ccc}\frac{{\tilde{V}}_1^r}{|{\tilde{V}}_1^r|}& \frac{{\tilde{V}}_1^r\times {\tilde{V}}_2^r}{|{\tilde{V}}_1^r\times {\tilde{V}}_2^r|}& \frac{{\tilde{V}}_1^r\times {\tilde{V}}_2^r\times {\tilde{V}}_1^r}{|{\tilde{V}}_1^r\times {\tilde{V}}_2^r\times {\tilde{V}}_1^r|}\end{array}\right]\left[\begin{array}{c}(\frac{{\tilde{V}}_1^b}{|{\tilde{V}}_1^b|}{)}^T\\ \\ (\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b|}{)}^T\\ \\ (\frac{{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b}{|{\tilde{V}}_1^b\times {\tilde{V}}_2^b\times {\tilde{V}}_1^b|}{)}^T\end{array}\right]$$

解析粗对准就是为上述四个矢量赋值，得到估计的姿态阵。注意应用该方法进行估计的前提是静基座状态，并且线运动引起的${\omega }_{en}^n$和$(2{\omega }_{ie}^n+{\omega }_{en}^n)\times v^n$非常小，可以近似为0并忽略。

惯导中有很多解算都是有前提的，不在前提条件下很多公式是不成立的。而且就是由前提开始，百家争鸣。

经过一系列的忽略与递推，可以得到：
$${\omega }_{ie}^n={\tilde{C}}_b^n{\tilde{\omega }}_{ie}^b$$

$$-g^n={\tilde{C}}_b^n{\tilde{f}}_{sf}^b$$

其中:
$$g^n=\left[\begin{array}{c}0\\ \\ 0\\ \\ -g\end{array}\right],{\omega }_{ie}^n=\left[\begin{array}{c}0\\ \\ {\omega }_{ie}cosL\\ \\ {\omega }_{ie}sinL\end{array}\right]=\left[\begin{array}{c}0\\ \\ {\omega }_N\\ \\ {\omega }_U\end{array}\right]$$

带入姿态阵：
$${\hat{C}}_b^r=\left[\begin{array}{ccc}0& -1& 0\\ & & \\ 0& 0& 1\\ & & \\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}(\frac{{\tilde{f}}_{sf}^b}{|{\tilde{f}}_{sf}^b|}{)}^T\\ \\ (\frac{{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b}{|{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b|}{)}^T\\ \\ (\frac{{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b\times {\tilde{f}}_{sf}^b}{|{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b\times {\tilde{f}}_{sf}^b|}{)}^T\end{array}\right]=\left[\begin{array}{c}-(\frac{{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b}{|{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b|}{)}^T\\ \\ (\frac{{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b\times {\tilde{f}}_{sf}^b}{|{\tilde{f}}_{sf}^b\times {\tilde{\omega }}_{ib}^b\times {\tilde{f}}_{sf}^b|}{)}^T\\ \\ (\frac{{\tilde{f}}_{sf}^b}{|{\tilde{f}}_{sf}^b|}{)}^T\end{array}\right]$$

贴一下书上最后一段话：

为什么地理纬度隐含在两矢量？

假设该点的纬度为$L$：
$$\begin{array}{rl}\omega \cdot f& =|\omega |\cdot |f|cos\angle (\omega ,f)\\ & =|\omega |\cdot |f|cos(\frac{\pi }{2}-L)\\ & =|\omega |\cdot |f|sinL\end{array}$$

所以：

$$L=arcsin\frac{\omega f}{|\omega ||f|}$$