平台失准角定义
记从导航坐标系$n$至计算坐标系$n'$的等效旋转矢量为$\boldsymbol{\phi}$,常称其为失准角误差。
假设$\boldsymbol{\phi}$为小量,根据等效旋转矢量与方向余弦阵关系式,近似有
$$ {\boldsymbol{C}}_{n'}^n \approx {\boldsymbol{I}} + \left( {{\phi } \times } \right)\\ {\boldsymbol{C}}_n^{n'} = {\left( {{\boldsymbol{C}}_{n'}^n} \right)^{\rm{T}}} \approx {\boldsymbol{I}} - \left( {{\phi } \times } \right)\\ {\boldsymbol{C}}_b^{n'} = {\boldsymbol{C}}_n^{n'}{\boldsymbol{C}}_b^n \approx \left[ {{\boldsymbol{I}} - \left( {{\phi } \times } \right)} \right]{\boldsymbol{C}}_b^n\\ {\boldsymbol{C}}_{n'}^b = {\boldsymbol{C}}_n^b{\boldsymbol{C}}_{n'}^n \approx {\boldsymbol{C}}_n^b\left[ {{\boldsymbol{I}} + \left( {{\phi } \times } \right)} \right] $$
平台失准角与四元数
姿态修正时可将估计出来平台失准角转换为修正四元数,令修正四元数为$${\boldsymbol{q}}_{n'}^n$$,修正四元数与平台失准角的关系为
$$ {\boldsymbol{q}}_{n'}^n{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {\cos \frac{{\left| \phi \right|}}{2}}\\ {\frac{\pmb{\phi}}{{\left| \phi \right|}}\sin \frac{{\left| \phi \right|}}{2}} \end{array}} \right] $$
那么其修正公式为($\circ$为四元数乘法)
$$ {\boldsymbol{\hat q}}_b^n{\rm{ = }}{\boldsymbol{q}}_{n'}^n \circ {\boldsymbol{q}}_b^{n'} $$
注意:四元数上下角标与姿态转换矩阵相反,例如$\boldsymbol{q}_b^n$表示为$n$系到$b$系的四元数。
平台失准角与姿态误差角的关系
平台误差角定义为$delta {boldsymbol{A}} = {left[ {begin{array}{*{20}{c}}
{delta theta }&{delta gamma }&{delta psi }
end{array}} right]^T} = {boldsymbol{tilde A}} - {boldsymbol{A}}$,那么平台失准角与平台误差角之间的关系为
$$ \pmb{\phi} {\rm{ = }} - \left[ {\begin{array}{*{20}{c}} {\cos \psi }&{ - \cos \theta \sin \psi }&0\\ {\sin \psi }&{\cos \theta \cos \psi }&0\\ 0&{\sin \theta }&1 \end{array}} \right]\cdot\delta {\boldsymbol{A}} $$
平台误差角与平台失准角之间的关系为
$$ \delta {\boldsymbol{A}}{\rm{ = }}\left[ {\begin{array}{*{20}{c}} { - \cos \psi }&{ - \sin \psi }&0\\ {{\sin \psi }}/{{\cos \theta }}&{ - {\cos \psi }/{\cos \theta }}&0\\ { - \tan \theta \sin \psi }&{\tan \theta \cos \psi }&{ - 1} \end{array}} \right]\cdot\pmb{\phi} $$
航向误差角与平台失准角之间的关系为
$$ \delta \psi {\rm{ = }} - \tan \theta \sin \psi \cdot {\phi _E} + \tan \theta \cos \psi \cdot {\phi _N} - {\phi _U} $$