MSCKF2讲：JPL四元数与Hamilton四元数

  本文总结了MSCKF中使用的JPL四元数，并且与Hamilton四元数进行了对比，主要参考了以下两篇论文。所有更加详细内容都可以在论文中找到。

论文1：Indirect Kalman Filter for 3D Attitude Estimation
论文2：Quaternion kinematics for the error-state Kalman filter

2 JPL四元数

参考Indirect Kalman Filter for 3D Attitude Estimation论文

2.1 定义与区别

  一个四元数可以表示为（不分种类）
$$\mathbf{q}=q_0+q_1\mathrm{i}+{\mathrm{q}}_2\mathrm{j}+{\mathrm{q}}_3\mathrm{k}$$
  十四讲上的四元数是Hamilton四元数，符合右手定则；这里是JPL四元数，符合左手定则。

Hamilton四元数（右手）	JPL四元数（左手）	异同
${\mathrm{i}}^2={\mathrm{j}}^2={\mathrm{k}}^2=-1$	${\mathrm{i}}^2={\mathrm{j}}^2={\mathrm{k}}^2=-1$	相同
$\mathrm{ij}=\mathrm{k},\mathrm{ji}=-\mathrm{k}$	$\mathrm{ij}=-\mathrm{k},\mathrm{ji}=\mathrm{k}$	不相同
$\mathrm{jk}=\mathrm{i},\mathrm{kj}=-\mathrm{i}$	$\mathrm{jk}=-\mathrm{i},\mathrm{kj}=\mathrm{i}$	不相同
$\mathrm{ki}=\mathrm{j},\mathrm{ik}=-\mathrm{j}$	$\mathrm{ki}=-\mathrm{j},\mathrm{ik}=\mathrm{j}$	不相同

在JPL四元数中，表示为$q={\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_0\end{array}\right]}^T$,虚部在前，实部在后！

在Hamilton四元数中，表示为$q={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$，实部在前，虚部在后！

  一个四元数可以通过角轴表示，$\theta$和$n$分别表示转轴和转角（注意两种四元素实虚部不一样，下面是Hamilton四元数，JPL四元数把虚部和实部反过来即可）。
$$\begin{gathered} \mathbf{q}=[\cos \frac{\theta }{2},n\sin \frac{\theta }{2}{]}^{\mathrm{T}} \\ \{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^{\mathrm{T}}={\left[q_1,q_2,q_3\right]}^{\mathrm{T}}/\sin \frac{\theta }{2}\end{array}. \end{gathered}$$
  我们称这种四元数为旋转四元数，也是单位四元数。

$$|\bar{q}|=\sqrt{{\bar{q}}^{\mathrm{T}}\bar{q}}=\sqrt{|\mathbf{q}{|}^2+q_0^2}=1$$

注意：

① 四元数q和四元数−q描述旋转到相同的最终坐标系位置！q0取负相当于π-θ/2，对于θ来说就是2π-θ，实部sin(π-θ/2)=sin(θ/2)，所以符号到了旋转轴上，旋转轴不管左手系还是右手系，取负相当于原来的逆方向！而角度刚好也是2π-θ，所以实际上旋转的还是原来的方向！唯⼀的区别是到达目标的旋转方向，具有正标量元素q0的四元数描述最短旋转！（同理，+R和-R描述的是相同的旋转）

公式cos(π-θ) = -cos(θ), sin(π-θ) = sin(θ)

② 不要把四元数$q$和四元数的逆$q^{-1}$搞混，它们描述相反的旋转

2.2 JPL四元数的乘法

Indirect Kalman Filter for 3D Attitude Estimation论文1.2节

  注意这里也是和Hamilton四元数一个不同之处！这也会使得两种四元数和旋转矩阵的转换公式不一致。

1 乘法

$$\begin{array}{rl}\bar{q}\otimes \bar{p}& =(q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k})\left(p_0+p_1\mathbf{i}+p_2\mathbf{j}+p_3\mathbf{k}\right)\\ & =q_0{p}_0-q_1{p}_1-q_2{p}_2-q_3{p}_3+(q_0{p}_1+q_1{p}_0-q_2{p}_3+q_3{p}_2)\mathbf{i}\\ & +\left(q_0{p}_2+q_2{p}_0-q_3{p}_1+q_1{p}_3\right)\mathbf{j}+\left(q_0{p}_3+q_3{p}_0-q_1{p}_2+q_2{p}_1\right)\mathbf{k}\\ & =[\begin{array}{c}{q}_0{p}_1+q_3{p}_2-q_2{p}_3+q_1{p}_0\\ -q_3{p}_1+q_0{p}_2+q_1{p}_3+q_2{p}_0\\ q_2{p}_1-q_1{p}_2+q_0{p}_3+q_3{p}_0\\ -q_1{p}_1-q_2{p}_2-q_3{p}_3+q_0{p}_0\end{array}]\end{array}$$

$$=[\begin{array}{cccc}{q}_0& q_3& -q_2& q_1\\ -q_3& q_0& q_1& q_2\\ q_2& -q_1& q_0& q_3\\ -q_1& -q_2& -q_3& q_0\end{array}]\left[\begin{array}{c}{p}_1\\ p_2\\ p_3\\ p_0\end{array}\right]$$

/* @brief Perform q1 * q2 */		// 注意代码中q=(q0,q1,q2,q3)  q3是实部，q0,q1,q2对应虚部
inline Eigen::Vector4d quaternionMultiplication(const Eigen::Vector4d& q1,const Eigen::Vector4d& q2) {Eigen::Matrix4d L;L(0, 0) =  q1(3); L(0, 1) =  q1(2); L(0, 2) = -q1(1); L(0, 3) =  q1(0);L(1, 0) = -q1(2); L(1, 1) =  q1(3); L(1, 2) =  q1(0); L(1, 3) =  q1(1);L(2, 0) =  q1(1); L(2, 1) = -q1(0); L(2, 2) =  q1(3); L(2, 3) =  q1(2);L(3, 0) = -q1(0); L(3, 1) = -q1(1); L(3, 2) = -q1(2); L(3, 3) =  q1(3);Eigen::Vector4d q = L * q2;		// JPL四元数乘法quaternionNormalize(q);			// 每次归一化四元数return q;
}

2 对某一个四元数虚部定义反对称矩阵简化运算。重新定义JPL四元数$q=[v,s{]}^T$，其中$v=[q_1,q_2,q_3{]}^T$是虚部，$s=q_0$是实部！

$$\lfloor \mathbf{v}\times \rfloor =[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}]$$

  所以，四元数乘法可以简化为下面的式子，通过下表s和v来区分四元数pq的实部与虚部
$$\begin{array}{c}\bar{q}\otimes \bar{p}=\mathcal{L}(\bar{q})\bar{p}=[\begin{array}{cccc}{q}_0& q_3& -q_2& q_1\\ -q_3& q_0& q_1& q_2\\ q_2& -q_1& q_0& q_3\\ -q_1& -q_2& -q_3& q_0\end{array}]\left[\begin{array}{c}{p}_1\\ p_2\\ p_3\\ p_0\end{array}\right]\\ \bar{q}\otimes \bar{p}=\mathcal{L}(\bar{q})\bar{p}=[\begin{array}{cc}{q}_s{\mathbf{I}}_{3\times 3}-\lfloor {\mathbf{q}}_v\times \rfloor & {\mathbf{q}}_v\\ -{\mathbf{q}}_v^{\mathrm{T}}& q_s\end{array}]\left[\begin{array}{c}{\mathbf{p}}_v\\ p_s\end{array}\right]\end{array}$$
  同理，我们可以互换四元数pq
$$\bar{q}\otimes \bar{p}=\mathcal{R}(\bar{p})\bar{q}=[\begin{array}{cccc}{p}_0& -p_3& p_2& p_1\\ p_3& p_0& -p_1& p_2\\ -p_2& p_1& p_0& p_3\\ -p_1& -p_2& -p_3& p_0\end{array}]\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_0\end{array}\right]$$

$$\bar{q}\otimes \bar{p}=\mathcal{R}(\bar{p})\bar{q}==[\begin{array}{cc}{p}_s{\mathbf{I}}_{3\times 3}+\lfloor {\mathbf{p}}_v\times \rfloor & {\mathbf{p}}_v\\ -{\mathbf{p}}_v^{\mathrm{T}}& p_s\end{array}]\left[\begin{array}{c}{\mathbf{q}}_v\\ q_s\end{array}\right]$$

  结合上面两个式子，可以得到
$$\begin{array}{rl}\mathcal{L}({\bar{q}}^{-1})& ={\mathcal{L}}^{\mathrm{T}}(\bar{q})\\ \mathcal{R}({\bar{p}}^{-1})& ={\mathcal{R}}^{\mathrm{T}}(\bar{p})\end{array}$$

4 对于单位旋转矩阵——对应四元数

$$\begin{gathered} {{\bar{q}}_0=[\begin{array}{cccc}0& 0& 0& 1\end{array}]}^{\mathrm{T}} \\ \bar{q}\otimes {\bar{q}}_0={\bar{q}}_0\otimes \bar{q}=\bar{q} \end{gathered}$$

/** @brief 输入一个四元数，将其转换维单位四元数.*/
inline void quaternionNormalize(Eigen::Vector4d& q) {double norm = q.norm();q = q / norm;return;
}

5 四元数的逆$q^{-1}$：转轴相同，转角相反。

$${\bar{q}}^{-1}=[\begin{array}{c}-{\mathbf{q}}_v\\ q_s\end{array}]=\left[\begin{array}{c}-\hat{\mathbf{k}}\sin (\theta /2)\\ \cos (\theta /2)\end{array}\right]=\left[\begin{array}{c}\hat{\mathbf{k}}\sin (-\theta /2)\\ \cos (-\theta /2)\end{array}\right]$$

  也就是说，一个四元数乘以其逆，相当于没有旋转，即对应单位旋转。

2.3 反对称矩阵

  在线性代数中，反对称矩阵（或称斜对称矩阵）指转置矩阵和自身的加法逆元相等的方形矩阵。其满足：$A^{\mathsf{T}}=-A$或写作$A=(a_{ij})$，各元素关系$a_{ij}=-a_{ji}$

$$\lfloor \mathbf{v}\times \rfloor =[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}]$$

/*	math_utils.hpp */
inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) {Eigen::Matrix3d w_hat;w_hat(0, 0) = 0;w_hat(0, 1) = -w(2);w_hat(0, 2) = w(1);w_hat(1, 0) = w(2);w_hat(1, 1) = 0;w_hat(1, 2) = -w(0);w_hat(2, 0) = -w(1);w_hat(2, 1) = w(0);w_hat(2, 2) = 0;return w_hat;
}

1 反交换性

$$\begin{array}{rl}\lfloor \mathbf{\omega }\times \rfloor & =-\lfloor \mathbf{\omega }\times {\rfloor }^{\mathrm{T}}\\ \lfloor \mathbf{a}\times \rfloor \mathbf{b}& =-\lfloor \mathbf{b}\times \rfloor \mathbf{a}\\ \iff {\mathbf{a}}^{\mathrm{T}}\lfloor \mathbf{b}\times \rfloor & =-{\mathbf{b}}^{\mathrm{T}}\lfloor \mathbf{a}\times \rfloor \end{array}$$

2 分配律

$$\lfloor \mathbf{a}\times \rfloor +\lfloor \mathbf{b}\times \rfloor =\lfloor \mathbf{a}+\mathbf{b}\rfloor \times$$

3 同一向量叉积

$$\mathbf{\omega }\times (c\cdot \mathbf{\omega })=c\cdot \lfloor \mathbf{\omega }\times \rfloor \mathbf{\omega }=-c\cdot {\left({\mathbf{\omega }}^{\mathrm{T}}\lfloor \mathbf{\omega }\times \rfloor \right)}^{\mathrm{T}}=0_{3\times 1}$$

4 拉格朗日公式

$$\begin{array}{c}\lfloor \mathbf{a}\times \rfloor \lfloor \mathbf{b}\times \rfloor =\mathbf{b}{\mathbf{a}}^{\mathrm{T}}-\left({\mathbf{a}}^{\mathrm{T}}\mathbf{b}\right){\mathbf{I}}_{3\times 3}\\ \iff \mathbf{a}\times (\mathbf{b}\times \mathbf{c})=\mathbf{b}\left({\mathbf{a}}^{\mathrm{T}}\mathbf{c}\right)-\mathbf{c}\left({\mathbf{a}}^{\mathrm{T}}\mathbf{b}\right)\\ \lfloor \mathbf{a}\times \rfloor \lfloor \mathbf{b}\times \rfloor +\mathbf{a}{\mathbf{b}}^{\mathrm{T}}=\lfloor \mathbf{b}\times \rfloor \lfloor \mathbf{a}\times \rfloor +\mathbf{b}{\mathbf{a}}^{\mathrm{T}}\\ \lfloor (\mathbf{a}\times \mathbf{b})\times \rfloor =\mathbf{b}{\mathbf{a}}^{\mathrm{T}}-\mathbf{a}{\mathbf{b}}^{\mathrm{T}}(=(\mathbf{a}\times \mathbf{b})\times \mathbf{c})\end{array}$$

5 雅可比恒等式

$$\lfloor \mathbf{a}\times \rfloor \lfloor \mathbf{b}\times \rfloor \mathbf{c}+\lfloor \mathbf{b}\times \rfloor \lfloor \mathbf{c}\times \rfloor \mathbf{a}+\lfloor \mathbf{c}\times \rfloor \lfloor \mathbf{a}\times \rfloor \mathbf{b}=0$$

6 旋转（重要！！！） 下面R是旋转矩阵

$$\begin{array}{c}\lfloor (\mathbf{Ra})\times \rfloor =\mathbf{R}\lfloor \mathbf{a}\times \rfloor {\mathbf{R}}^{\mathrm{T}}\\ \mathbf{R}(\mathbf{a}\times \mathbf{b})=(\mathbf{Ra})\times (\mathbf{Rb})\end{array}$$

2.4 $\Omega (\omega )$矩阵

参考：论文Indirect Kalman Filter for 3D Attitude Estimation中1.3.2 Properties of the matrix Ω

  记向量$\omega =({\omega }_x,{\omega }_y,{\omega }_z{)}^T$。

  这个矩阵用在向量和四元数的乘积中，例如用于四元数导数中
$$\mathbf{\Omega }(\mathbf{\omega })=[\begin{array}{cccc}0& {\omega }_z& -{\omega }_y& {\omega }_x\\ -{\omega }_z& 0& {\omega }_x& {\omega }_y\\ {\omega }_y& -{\omega }_x& 0& {\omega }_z\\ -{\omega }_x& -{\omega }_y& -{\omega }_z& 0\end{array}]=[\begin{array}{cc}-\lfloor \mathbf{\omega }\times \rfloor & \mathbf{\omega }\\ -{\mathbf{\omega }}^{\mathrm{T}}& 0\end{array}]$$

平方

$$\begin{array}{rl}\mathbf{\Omega }(\omega {)}^2& =\left[\begin{array}{cc}\lfloor \mathbf{\omega }\times {\rfloor }^2-\mathbf{\omega }{\mathbf{\omega }}^{\mathrm{T}}& -\lfloor \mathbf{\omega }\times \rfloor \mathbf{\omega }\\ {\mathbf{\omega }}^{\mathrm{T}}\lfloor \mathbf{\omega }\times \rfloor & -{\mathbf{\omega }}^{\mathrm{T}}\mathbf{\omega }\end{array}\right]\\ & =\left[\begin{array}{cc}-|\mathbf{\omega }{|}^2\cdot {\mathbf{I}}_{3\times 3}& 0_{3\times 1}\\ 0_{1\times 3}& -|\mathbf{\omega }{|}^2\end{array}\right]\\ & =-|\omega {|}^2\cdot {\mathbf{I}}_{4\times 4}\end{array}$$

高阶幂

$$\begin{array}{rlrl}& \mathbf{\Omega }(\omega {)}^3& & =-|\mathbf{\omega }{|}^2\cdot \mathbf{\Omega }(\mathbf{\omega })\\ & \mathbf{\Omega }(\omega {)}^4& & =|\omega {|}^4\cdot {\mathbf{I}}_{4\times 4}\\ & \mathbf{\Omega }(\omega {)}^5& & =|\omega {|}^4\cdot \mathbf{\Omega }(\mathbf{\omega })\\ & \Omega (\omega {)}^6& & =-|\omega {|}^6\cdot {\mathbf{I}}_{4\times 4}\end{array}$$

补充：JPL四元数对应的$\Omega (\omega )$矩阵和Hamilton不同，下面是Hamilton对应的$\Omega (\omega )$矩阵

$$\Omega (\omega )\triangleq [\omega {]}_R=\left[\begin{array}{cc}0& -{\mathbf{\omega }}^{\top }\\ \mathbf{\omega }& -[\mathbf{\omega }{]}_{\times }\end{array}\right]=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$

2.5 JPL四元数与旋转矩阵的转换

参考论文Indirect Kalman Filter for 3D Attitude Estimation中1.4 Relationship between Quaternion and Rotational Matrix

  给定向量p，是⼀个三维点。我们将相应的四元数定义为
$$\bar{p}=[\begin{array}{c}{\mathbf{p}}_v\\ 0\end{array}]$$
  使用以下两种不同坐标系中表示的向量之间的关系

① 旋转矩阵
$${}^L\mathbf{p}={}_G^L{\mathbf{C}}^G\mathbf{p}$$
② 四元数（不同类型的四元数对旋转的表示都一样，即$p^{\prime }=qp{q}^{-1}.$）

$$\begin{array}{rl}{}^L\bar{p}& ={}_G^L\bar{q}\otimes {}^G\bar{p}\otimes {}_G^L{\bar{q}}^{-1}\\ & =[\begin{array}{cc}{q}_s{\mathbf{I}}_{3\times 3}-\lfloor {\mathbf{q}}_v\times \rfloor & {\mathbf{q}}_v\\ -{\mathbf{q}}_v^{\mathrm{T}}& q_s\end{array}]\left[\begin{array}{c}{\mathbf{p}}_v\\ 0\end{array}\right]{\otimes }_L^G{\bar{q}}^{-1}\\ & =[\begin{array}{c}{q}_s{\mathbf{p}}_v-{\mathbf{q}}_v\times {\mathbf{p}}_v\\ -{\mathbf{q}}_v^{\mathrm{T}}{\mathbf{p}}_v\end{array}]\otimes \left[\begin{array}{c}-{\mathbf{q}}_v\\ q_s\end{array}\right]\\ & =[\begin{array}{c}{q}_s^2\mathbf{p}-q_s\mathbf{q}\times \mathbf{p}+\mathbf{q}{\mathbf{q}}^{\mathrm{T}}\mathbf{p}+q_s\mathbf{p}\times \mathbf{q}-(\mathbf{q}\times \mathbf{p})\times \mathbf{q}\\ +q_s{\mathbf{q}}^{\mathrm{T}}\mathbf{p}-q_s{\mathbf{q}}^{\mathrm{T}}\mathbf{p}-{\mathbf{q}}^{\mathrm{T}}(\mathbf{q}\times \mathbf{p})\end{array}]\\ & =[\begin{array}{c}{q}_s^2\mathbf{p}-2q_s\mathbf{q}\times \mathbf{p}+\mathbf{q}{\mathbf{q}}^{\mathrm{T}}\mathbf{p}-\left(\left(1-q_s^2{\mathbf{I}}_{3\times 3}\right)-\mathbf{q}{\mathbf{q}}^{\mathrm{T}}\right)\mathbf{p}\\ -{\mathbf{q}}^{\mathrm{T}}\lfloor \mathbf{q}\times \rfloor \mathbf{p}\end{array}]\\ & =[\begin{array}{c}\left(2q_s^2-1\right){\mathbf{I}}_{3\times 3}-2q_s\lfloor {\mathbf{q}}_v\times \rfloor +2{\mathbf{q}}_v{\mathbf{q}}_v^{\mathrm{T}}\\ 0\end{array}]\left[\begin{array}{c}{}^G{\mathbf{p}}_v\\ 0\end{array}\right]\end{array}$$
  得到了旋转矩阵和四元数转换，即矩阵左上角
$${}_G^L\mathbf{C}(\bar{q})=\left(2q_s^2-1\right){\mathbf{I}}_{3\times 3}-2q_s\lfloor {\mathbf{q}}_v\times \rfloor +2{\mathbf{q}}_v{\mathbf{q}}_v^{\mathrm{T}}$$

/*	把JPL四元数转换为旋转矩阵* @brief Convert a quaternion to the corresponding rotation matrix* @note 公式参见论文"Indirect Kalman Filter for 3D Attitude Estimation: Equation (62).*	实际上论文中还有一种形式这里没给出，参考公式(77)*/
inline Eigen::Matrix3d quaternionToRotation(const Eigen::Vector4d& q) {const Eigen::Vector3d& q_vec = q.block(0, 0, 3, 1);		// qvconst double& q4 = q(3);		// qsEigen::Matrix3d R =(2*q4*q4-1)*Eigen::Matrix3d::Identity() -2*q4*skewSymmetric(q_vec) + 2*q_vec*q_vec.transpose();return R;
}

  当JPL四元数对应转角很小时，可以近似为：
$$\begin{array}{rl}\delta \bar{q}& =[\begin{array}{c}\delta {\mathbf{q}}_v\\ \delta q_s\end{array}]\\ & =[\begin{array}{c}\hat{\mathbf{n}}\sin (\delta \theta /2)\\ \cos (\delta \theta /2)\end{array}]\\ & \approx [\begin{array}{c}\frac{1}{2}\delta \mathbf{\theta }\\ 1\end{array}]\end{array}$$
​  对应的旋转矩阵可以近似为
$${}_G^L\mathbf{C}(\delta \bar{q})\approx {\mathbf{I}}_{3\times 3}-\lfloor \delta \mathbf{\theta }\times \rfloor$$

论文Indirect Kalman Filter for 3D Attitude Estimation之3.2 Kalman Filter Update，下面公式是在卡尔曼滤波更新使用的！本质上是把一个旋转小量θ转为相应的四元数，注意θ=2q

/* 将四元数的向量部分（虚部）转换为完整的四元数* @brief Convert the vector part of a quaternion to a full quaternion.* @note 这个函数对于delta quaternion*    which is usually a 3x1 vector to a full quaternion.*/
inline Eigen::Vector4d smallAngleQuaternion(const Eigen::Vector3d& dtheta) 
{Eigen::Vector3d dq = dtheta / 2.0;		// 0.5*δθEigen::Vector4d q;						// 四元数qdouble dq_square_norm = dq.squaredNorm();	// 计算向量模的平方if (dq_square_norm <= 1) {		// 如果这个向量的模的平方小于1   小角度旋转q.head<3>() = dq;q(3) = std::sqrt(1-dq_square_norm);} else {q.head<3>() = dq;q(3) = 1;q = q / std::sqrt(1+dq_square_norm);	// 归一化，都是用单位四元数来表示旋转的   大角度旋转}return q;
}// 说下这里的原理:对于一个单位四元数q=cos(θ/2)+nsin(θ/2),如果说虚部模长很大，那么实部就会很小，意味着角度θ很大！
// 但是这里是判断一个旋转向量的模长，旋转向量的模长就是角度，模长越大，说明旋转越大；模长越小，旋转越小。

③ 欧拉角：利用②中推导结果，代入角轴，即可得到下面公式
$$\begin{array}{rcl}{}_G^L\mathbf{C}& =& \cos (\theta )\cdot {\mathbf{I}}_{3\times 3}-\sin (\theta )\lfloor \hat{\mathbf{k}}\times \rfloor +(1-\cos (\theta ))\hat{\mathbf{k}}{\hat{\mathbf{k}}}^{\mathrm{T}}\\ & =& \left(2{\cos }^2(\theta /2)-1\right)\cdot {\mathbf{I}}_{3\times 3}-2\cos (\theta /2)\sin (\theta /2)\lfloor \hat{\mathbf{k}}\times \rfloor +2{\sin }^2(\theta /2)\hat{\mathbf{k}}{\hat{\mathbf{k}}}^{\mathrm{T}}\end{array}$$
  这个公式和罗德里格斯公式非常相似，只是角度取了负号！这是因为JPL四元数对应的是左手系，而罗德里格斯公式对应右手系。所以，不同于常见的指数映射，这里多了负号！

$${}_G^L\mathbf{C}(\bar{q})=\exp \left(-\lfloor \hat{\mathbf{k}}\times \rfloor \theta \right)$$

④ 旋转矩阵转四元数

/** The function follows the*    conversion in "Indirect Kalman Filter for 3D Attitude Estimation, Equation (78).*/
inline Eigen::Vector4d rotationToQuaternion(const Eigen::Matrix3d& R) 
{Eigen::Vector4d score;score(0) = R(0, 0);score(1) = R(1, 1);score(2) = R(2, 2);score(3) = R.trace();int max_row = 0, max_col = 0;score.maxCoeff(&max_row, &max_col);Eigen::Vector4d q = Eigen::Vector4d::Zero();if (max_row == 0) {q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));q(3) = (R(1, 2)-R(2, 1)) / (4*q(0));} else if (max_row == 1) {q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));q(3) = (R(2, 0)-R(0, 2)) / (4*q(1));} else if (max_row == 2) {q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));q(3) = (R(0, 1)-R(1, 0)) / (4*q(2));} else {q(3) = std::sqrt(1+R.trace()) / 2.0;q(0) = (R(1, 2)-R(2, 1)) / (4*q(3));q(1) = (R(2, 0)-R(0, 2)) / (4*q(3));q(2) = (R(0, 1)-R(1, 0)) / (4*q(3));}if (q(3) < 0) q = -q;quaternionNormalize(q);return q;
}

2.6 JPL四元数导数

  论文中写道，When the local coordinate frame {L} is moving with respect to the global reference frame {G}, we can compute the rate of change or the derivative of the corresponding quaternion describing their relationship. 当局部坐标系相较于世界系有移动，我们可以计算q关于时间的导数。----这种扰动只能是左扰动！

第一步：微分

第二步：引入中间极小量

第三步：推导

2.7 JPL四元数积分

  积分主要有0阶和一阶积分两种，MSCKF中采用了0阶积分

2.7.1 0阶积分

  欧拉法，认为Δt时间内角速度保持不变
$$\bar{\mathbf{\omega }}=\omega (t_k)$$

$$\begin{array}{rl}& |\hat{\omega }|>10^{-5}{\text{ 时:}}_G\hat{q}(t+\Delta t)={\left(\cos \left(\frac{|\hat{\omega }|}{2}\Delta t\right)\cdot I_{4\times 4}+\frac{1}{|\hat{\omega }|}\sin \left(\frac{|\hat{\omega }|}{2}\Delta t\right)\cdot \Omega (\hat{\omega })\right)}_G\hat{q}(t)\\ & |\hat{\omega }|\le 10^{-5}{\text{ 时:}}_G\hat{q}(t+\Delta t)={\left(I_{4\times 4}-\frac{\Delta t}{2}\Omega (\hat{\omega })\right)}_G\hat{q}(t)\end{array}$$

2.7.2 1阶积分

  角速度取两个时刻的均值，具体见论文
$$\bar{\mathbf{\omega }}=\frac{\mathbf{\omega }(t_{k+1})+\mathbf{\omega }(t_k)}{2}$$

3 JPL与Hamilton的区别

① 论文Quaternion kinematics for the error-state Kalman filter是Hamilton四元数

② 论文Indirect Kalman Filter for 3D Attitude Estimation是JPL四元数

  现在定义一个四元数$q=q_{1}i+q_{2}j+q_{3}k+q_0=q_v+q_S$，下面对比其作为不同四元数的一个区别。

  定义local表示局部坐标系，global表示世界坐标系

3.1 转换方向

  转换方向的不同，使其扰动更新分别对应左扰动和右扰动。

• JPL：global-to-local 世界坐标系 --> 局部坐标系

$$X_l=q\otimes X_g\otimes q^{-1}\text{,}{X}_l=R_g^l{X}_g$$

• Hamilton：local-to-global 局部坐标系 --> 世界坐标系

$$X_g=q\otimes X_l\otimes q^{-1},X_g=R_l^g{X}_l$$

  所以JPL形式四元数得到的旋转矩阵 和 Hamilton形式四元数得到的旋转矩阵互为转置！表示了相反的旋转！
  还有一点，上面也提到了q和-q表示了相同的旋转，只是方向相反

3.2 四元数与旋转向量的转换

• JPL：

$$q=\left[\begin{array}{c}\hat{k}\sin \frac{\theta }{2}\\ \cos \frac{\theta }{2}\end{array}\right]\Rightarrow R_g^l=\exp \left(-[\hat{k}{]}_{\times }\theta \right)$$

• Hamilton:

$$q=\left[\begin{array}{c}\cos \frac{\theta }{2}\\ \hat{k}\sin \frac{\theta }{2}\end{array}\right]\Rightarrow R_l^g=\exp ([\hat{k}{]}_{\times }\theta )$$

$$\begin{array}{rl}{R}_g^l& ={R_l^g}^T\\ & =exp{\left([\hat{k}{]}_{\times }\theta \right)}^T\\ & =exp{([\hat{k}{]}_{\times }\theta )}^{-1}\\ & =exp\left(-[\hat{k}{]}_{\times }\theta \right)\end{array}$$

  一样的旋转角度，但轴是相反的，本文一开始的两个坐标系图也能看出来

3.3 四元数与旋转矩阵的转换

• JPL：

$$R_g^l=\left(2q_s^2-1\right){\mathbf{I}}_{3\times 3}-2q_s\lfloor {\mathbf{q}}_{\mathbf{v}}\times \rfloor +2{\mathbf{q}}_{\mathbf{v}}{\mathbf{q}}_{\mathbf{v}}^{\mathrm{T}}$$

• Hamilton:

$$R_l^g=\left(q_s^2-{\mathbf{q}}_v^{\top }{\mathbf{q}}_v\right)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\top }+2q_s[{\mathbf{q}}_v{]}_{\mathbf{x}}$$

3.4 四元数的乘法

• JPL：

$$\begin{array}{rl}{q}_1\otimes q_2& =L\left(q_1\right)q_2=q_{0}I+\left[\begin{array}{cc}-q_{\times }& q\\ -q^{\top }& 0\end{array}\right]\\ & =R\left(q_2\right)q_1=q_{0}I+\left[\begin{array}{cc}{q}_{\times }& q\\ -q^{\top }& 0\end{array}\right]\end{array}$$

• Hamilton:

3.5 四元数的导数

• JPL：$\Omega (w)$见2.4

$${\dot{q}}_g^l=\frac{1}{2}\Omega (w)\cdot q=\frac{1}{2}\left[\begin{array}{c}w\\ 0\end{array}\right]\otimes q=\frac{1}{2}\left[\begin{array}{cc}-w\times & w\\ -w^{\top }& 0\end{array}\right]\cdot q$$

• Hamilton:

$${\dot{q}}_l^g=\frac{1}{2}\Omega (w)\cdot q=\frac{1}{2}q\otimes [\begin{array}{c}0\\ w\end{array}]=\frac{1}{2}\left[\begin{array}{cc}0& -w^{\top }\\ w& -w_{\times }\end{array}\right]\cdot q$$

注意：JPL四元数对应的$\Omega (\omega )$矩阵和Hamilton不同，上面2.4提到。上面两个导数的式子前半部分只是看起来相同！因为它们一个对应局部系，一个对应全局系，扰动分别对应左扰动和右扰动。

3.6 四元数的扰动

• JPL：

$$\delta R=I-[\theta {]}_{\times }$$

• Hamilton:

$$\delta R=I+[\theta {]}_{\times }$$