第十三篇 惯性定位计算基础公式

1 反对称矩阵

$$V=\left[\begin{array}{ccc}0& -v3& v2\\ v3& 0& -v1\\ -v2& v1& 0\end{array}\right]$$
反对称矩阵乘以向量等于向量叉乘
$$V\ast \vec{b}=\vec{V}\times \vec{b}$$

2 罗德里格斯公式

$$\begin{gathered} D=I+sin(\varphi )\ast (\vec{u}\times )+(1-cos(\varphi ))\ast (\vec{u}\times {)}^2 \\ D=cos(\varphi )\ast I+(1-cos(\varphi ))\ast \vec{u}\ast {\vec{u}}^T+sin(\varphi )\ast (\vec{u}\times ) \end{gathered}$$

3 姿态微分方程以及求解

3.1 方向余弦阵微分方程以及求解

微分方程
$${\dot{C}}_b^i=C_b^i\ast ({\omega }_{ib}^b\times )$$
求解结果
$$\begin{gathered} C_{b(m)}^i=C_{b(m-1)}^i\ast C_{b(m)}^{b(m-1)} \\ {C}_{b(m)}^{b(m-1)}=I+\frac{sin(\Delta {\theta }_m)}{\Delta {\theta }_m}(\Delta {\vec{\theta }}_m\times {)}^{+}\frac{1-cos(\Delta {\theta }_m)}{\Delta {\theta }_m^2}(\Delta {\vec{\theta }}_m\times {)}^2 \end{gathered}$$

3.2 四元数微分方程以及求解

四元数微分方程，后一项是角速度零标量四元数。
$${\dot{Q}}_b^i=\frac{1}{2}\ast Q_b^i\ast {\omega }_{ib}^b$$
四元数微分方程求解
$$\begin{gathered} Q_{b(m)}^i=Q_{b(m-1)}^i\ast Q_{b(m)}^{b(m-1)} \\ {Q}_{b(m)}^{b(m-1)}=\left[\begin{array}{c}cos\frac{\Delta {\theta }_m}{2}\\ \frac{\Delta {\vec{\theta }}_m}{\Delta {\theta }_m}sin\frac{\Delta {\theta }_m}{2}\end{array}\right] \end{gathered}$$

3.3 等效旋转矢量微分方程以及求解

等效旋转矢量微分方程，只在等效旋转矢量为小量时成立。
$$\dot{\varphi }=\omega +\frac{1}{2}\varphi \times \omega$$
等效旋转矢量求解,实际应用时一总是以t(m-1)为新的时间起点，phi(m-1)=0,计算等效旋转矢量，再利用四元数或者方向余弦递推方程。
$$\begin{gathered} Q_{b(m)}^i=Q_{b(m-1)}^i\ast Q_{b(m)}^{b(m-1)} \\ {Q}_{b(m)}^{b(m-1)}=\left[\begin{array}{c}cos\frac{\Delta {\varphi }_m}{2}\\ \frac{\Delta {\vec{\varphi }}_m}{\Delta {\varphi }_m}sin\frac{\Delta {\varphi }_m}{2}\end{array}\right] \end{gathered}$$
等效旋转矢量，二子样算法，如下：
$$\varphi (T)=\Delta {\theta }_1+\Delta {\theta }_2+\frac{2}{3}\Delta {\theta }_1\times \Delta {\theta }_2$$
等效旋转矢量，单子样+前一周期算法，如下：
$$\varphi (T)=\Delta {\theta }_1+\frac{1}{12}\Delta {\theta }_0\times \Delta {\theta }_1$$

3.4 推导过程重要公式

角速度反对称矩阵转换坐标系(反对称矩阵相似变换公式，推导过程引入一个右向量实现，非常重要)
$$({\omega }_{ib}^i\times )=C_b^i\ast ({\omega }_{ib}^b\times )\ast C_i^b$$
四元数转换坐标系
$$r^i=Q_b^i\ast r^b\ast Q_i^b$$
四元数左乘转矩阵乘法
$$P\ast Q=M_p\ast Q=P_0\ast I+\left[\begin{array}{cc}0& -P_v^T\\ P_v& P_v\times \end{array}\right]$$
四元数右乘转矩阵乘法
$$P\ast Q=M_q\ast P=Q_0\ast I+\left[\begin{array}{cc}0& -Q_v^T\\ Q_v& -Q_v\times \end{array}\right]$$

4 SINS公式

以下考虑了地球自转和地球曲率，不考虑的地方可以去掉相关项。
速度微分方程(这是所有推导的基础方程)
$${\dot{v}}_{en}^n=C_b^n\ast f^b-(2\ast {\omega }_{ie}^n+{\omega }_{en}^n)\times v_{en}^n+g^n$$
姿态误差方程和速度误差方程
$$\begin{gathered} {\dot{\varphi }}^n=\delta {\omega }_{ie}^t+\delta {\omega }_{et}^t-({\omega }_{ie}^t+{\omega }_{et}^t)\times {\varphi }^t+C_b^t{\epsilon }^b \\ \delta {\dot{v}}^n=C_b^n\ast f^b\times {\varphi }^n-(2\ast \delta {\omega }_{ie}^n+\delta {\omega }_{en}^n)\times v^n-(2\ast {\omega }_{ie}^n+{\omega }_{en}^n)\times \delta v^n+C_b^t{\nabla }^b \end{gathered}$$