1.姿态计算

方法： 等效旋转矢量为小量时才可以使用加减法；四元数和姿态阵采用乘法； 欧拉角只有物理意义不直接用于计算。
ESKF中状态量的建立使用等效旋转矢量；ESKF 的prediction和update过程使用等效旋转矢量做运算；

1.1 progation过程姿态变化量采用等效旋转矢量计算

因为短时间内姿态变化量是小量，可以与陀螺转动积分的结果，在delta_t小段时间内做代数运算。以初始时刻姿态为基准,求出姿态变化量等效旋转矢量，转化为姿态变化量四元数，求出新时刻姿态先验值。
$$\begin{gathered} {\varphi }_{k+1}={\theta }_{k+1}+\frac{1}{12}{\theta }_k\times {\theta }_{k+1} \\ \Delta q_{k+1}^k=rotateVector2quaternion({\varphi }_k) \\ {q}_{k+1}^{nav}\_prior=q_k\ast \Delta q_{k+1}^k \end{gathered}$$

1.2 observation过程姿态的误差量使用四元数或者姿态阵计算

因为观测的姿态和递推出来的姿态都是大量，不可以直接转为等效旋转矢量做加减法。采用四元数或者等效旋转阵相乘求出姿态误差量以后才能转化为姿态误差量等效旋转矢量。
$$\begin{gathered} \Delta q=q_{k+1\_obs}^{nav}\ast inverse(q_{k+1}^{nav}\_prior) \\ {\varphi }_{k+1}\_obs=quaterniond2rotationVector(\Delta q) \end{gathered}$$

1.3 ESKF prediction和updation全用等效旋转矢量运算

$${\varphi }_{k+1}\_post$$

1.4correction过程姿态的校正采用四元数或者姿态阵计算

$$\begin{gathered} \Delta q\_post=rotateVector2quaternion({\varphi }_k\_post) \\ {q}_{k+1}=\Delta q\_post\ast q_{k+1}^{nav}\_prior \end{gathered}$$

2.观测与校正配合

观测误差姿态量的构建与最后校正的形式密切相关，但是记住一个原则就不会错：令增益系数K=1是，按照公式得出的姿态量应该等于$$q_{k+1}^{nav}\_obs$$
当增益系数为1时，可以看做如下公式成立。
$$\Delta q\_post=\Delta q$$
以下四种情况，就可以凑出姿态的观测量。

2.1 观测与校正配合1

$$\begin{gathered} \Delta q=q_{k+1\_obs}^{nav}\ast inverse(q_{k+1}^{nav}\_prior) \\ {q}_{k+1}=\Delta q\_post\ast q_{k+1}\_prior \end{gathered}$$

2.2观测与校正配合2

$$\begin{gathered} \Delta q=inverse(q_{k+1\_obs}^{nav})\ast q_{k+1}^{nav}\_prior \\ {q}_{k+1}=q_{k+1}\_prior\ast inverse(\Delta q\_post) \end{gathered}$$

2.3 观测与校正配合3

$$\begin{gathered} \Delta q=q_{k+1}^{nav}\_prior\ast inverse(q_{k+1\_obs}^{nav}) \\ {q}_{k+1}=inverse(\Delta q\_post)\ast q_{k+1}\_prior \end{gathered}$$

2.4 观测与校正配合4

$$\begin{gathered} \Delta q=inverse(q_{k+1}^{nav}\_prior)\ast q_{k+1\_obs}^{nav} \\ {q}_{k+1}=q_{k+1}\_prior\ast \Delta q\_post \end{gathered}$$