一步一步推导S-MSCKF（一）姿态表示方法

坐标系约定与外积的关系

如果统一使用右手坐标系，那么外积的方向要根据右手法则来判断；如果使用左手坐标系，就要使用左手法则来判断。
但是无论哪一种方式，外积的计算都可以写成以下代数形式。给定三维向量$a,b$，其外积可以表示为：

$$a\times b=[a{]}_{\times }b=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$$
本篇文章默认使用右手坐标系。

1 三维向量的旋转变换

在某个坐标系中，将一个向量$x$沿着单位轴向量$u$旋转角度$\varphi$，得到的新向量记为$x^{{}^{\prime }}$，那么$x$与$x^{{}^{\prime }}$有什么关系呢？（这里称$\varphi u$为旋转向量）
考虑如下两种情况，右手法则旋转和左手法则旋转。

1.1 右手旋转

如下图所示：

将$x$分解为与$u$垂直的$x_{\perp }$，和平行的$x_{\parallel }$：
$$x=x_{\parallel }+x_{\perp }$$
$$x_{\parallel }=u(||x||cos\alpha )=u{u}^{T}x,x_{\perp }=x-x_{\parallel }=x-u{u}^{T}x$$
而与轴平行部分不会旋转，所以有：
$$x_{\parallel }^{{}^{\prime }}=x_{\parallel }$$

我们构建两个正交向量$e_1$和$e_2$，$||e_1||=||e_2||$：
$$e_1=x_{\perp },e_2=u\times x_{\perp }=u\times x$$
所以有：
$$x_{\perp }^{{}^{\prime }}=e_{1}cos\varphi +e_{2}sin\varphi$$
$$x_{\perp }^{{}^{\prime }}=x_{\perp }cos\varphi +(u\times x)sin\varphi$$
得到$x^{{}^{\prime }}$为：
$$x^{{}^{\prime }}=x_{\parallel }+x_{\perp }cos\varphi +(u\times x)sin\varphi$$
将$x_{\parallel }=u{u}^{T}x$和$x_{\perp }=x-u{u}^{T}x$带入上式得到：
$$x^{{}^{\prime }}=\left[I+sin\varphi [u{]}_{\times }+(1-cos\varphi )[u{]}_{\times }^2\right]x$$

1.2 左手旋转

将$x$沿着$u$左手法则旋转角度$\varphi$等价于右手法则旋转$-\varphi$，所以立即得到：
$$x^{{}^{\prime }}=\left[I-sin\varphi [u{]}_{\times }+(1-cos\varphi )[u{]}_{\times }^2\right]x$$

2 正交群

定义一个线性变换$r$:
$$r:R^3\to R^3;v\to r(v)$$
其满足如下条件：

• 保持向量的长度的不变：
  $$||r(v)||=\sqrt{⟨r(v),r(v)⟩}=\sqrt{⟨v,v⟩}=||v||$$
• 保持向量之间的夹角不变。（这里指余弦值不变）
  $$⟨r(v),r(w)⟩=⟨v,w⟩=||v||||w||cos\alpha$$
• 保持向量之间的相对方向不变。（满足右手准则关系）
  $$变换之前：u\times v=w；那么变换之后要满足：r(u)\times (v)=r(w)$$
  其中前两个条件是等价的，证明如下：
  因为$r(.)$是线性变换，不妨设$r(v)=Rv$，如果已知第一个条件，那么有：
  $$⟨r(v),r(v)⟩=(Rv{)}^{T}Rv=v^T{R}^{T}Rv=v^{T}v$$
  $$R^{T}R=I=R{R}^T$$
  $$⟨r(v),r(w)⟩=v^T{R}^{T}Rw=v^{T}w=⟨v,w⟩$$
  如果已知第二个条件，那么也可按照上述方法证出第一个条件。
  现在我们定义正交群$SO(3)$：
  $$SO(3):\left\{r:R^3\to R^3/\forall v,w\in R^3,||r(v)||=||v||,r(v)\times (w)=r(v\times w)\right\}$$

3 旋转矩阵

3.1 指数映射

令$u$是一个单位向量，$\varphi$是一个标量值，令$v=\varphi u$，定义指数映射Exp(.)，将旋转矩阵$R$定义如下：
$$R=Exp(v)=e^{[v{]}_{\times }}$$
可以验证该$R$代表的线性变换满足正交群的条件。
计算$R$：
$$R=e^{\varphi [u{]}_{\times }}=I+\varphi [u{]}_{\times }+\frac{1}{2}{\varphi }^2[u{]}_{\times }^2+\frac{1}{3}{\varphi }^3[u{]}_{\times }^3+...$$
因为：
$$[u{]}_{\times }^2=u{u}^T-I,[u{]}_{\times }^3=-[u{]}_{\times }$$
所以有:
$$[u{]}_{\times }^4=-[u{]}_{\times }^2,[u{]}_{\times }^5=[u{]}_{\times },[u{]}_{\times }^6=[u{]}_{\times }^2,[u{]}_{\times }^7=-[u{]}_{\times }$$
将其带入$R$的表达式，可以化简得到：
$$R=Icos\varphi +[u{]}_{\times }sin\varphi +u{u}^T(1-cos\varphi )$$
我们可以看到这里的$R$和本文开头推出的三维向量旋转变换的结果一样（右手法则）：
$$x^{{}^{\prime }}=Rx$$

3.2 左右手法则

所以在三维向量旋转时，如果我们知道旋转轴为$u$，那么沿右手方向旋转$\varphi$后，得到的新向量为：
$$x^{{}^{\prime }}=e^{\varphi [u{]}_{\times }}x$$
沿左手方向旋转$\varphi$后，得到的新向量为：
$$x^{{}^{\prime }}=e^{-\varphi [u{]}_{\times }}x$$

3.3 坐标变换

我们前面一直在讨论向量$x$绕着过原点的轴$u$旋转角度$\varphi$之后得到的$x^{{}^{\prime }}$坐标是多少，不过这里只涉及一个坐标系，记为M，下面我们换一种思路来解释这个过程。

我们把整个坐标系M都绕着$u$转过角度$\varphi$（当然也包括$x$），得到的新坐标系记为N；记$x_N$为$x$在N系的坐标，$x_M$为$x$在M系的坐标；显然$x_N$等于旋转之前x在M系的坐标，也就是说：
$$x_M=e^{\varphi [u{]}_{\times }}{x}_N=R{x}_N$$

显然，R可以表示某向量在两个坐标系之间的坐标变换关系，这在SLAM中十分有用。

因为$x$是过原点的直线，所以M与N系的原点重合。当N系原点在M系中的坐标为t时候，我们按照上式算出的结果是在与N系原点重合并与M系各轴平行的临时坐标系$M_1$中的坐标，我们需要再加上t：
$$x_M=R{x}_N+t$$

4 四元数

4.1 指数映射

假如$v=\varphi u$表示旋转向量，定义指数映射：
$$q=Exp(\varphi u)=e^{\frac{\varphi u}{2}}=cos\frac{\varphi }{2}+usin\varphi 2=\left[\begin{array}{c}cos\frac{\varphi }{2}\\ usin\frac{\varphi }{2}\end{array}\right]$$
对于上述单位四元数q，定义线性变换r(.)如下：
$$r(v)=q⨂v⨂q^{\ast }$$
其中$v$是向量$v$对应的纯虚四元数$v=v_{1}i+v_{2}j+v_{3}k$，可以证明r(.)满足正交群的定义。

而且可以验证在Hamilton四元数的定义下（$ij=k$，右手）：
$$x^{{}^{\prime }}=q⨂x⨂q^{\ast }=x_{\parallel }+x_{\perp }cos\varphi +(u\times x)sin\varphi$$
在JPL四元数定义下（$ij=-k$，左手）：
$$x^{{}^{\prime }}=q⨂x⨂q^{\ast }=x_{\parallel }+x_{\perp }cos\varphi -(u\times x)sin\varphi$$
这与前面轴角推出的结果一致，显然这种$r(.)$也可以表示三维向量的旋转。

4.2 Hamilton与JPL

他们的数值之间的关系为：
$$q_{JPL}=q_{LG,left}=q_{LG,right}^{\ast }=q_{GL,right}=q_{Hamilton}$$

对于小角度的扰动$\varphi u$，对于Hamiton:
$$q\approx \left[\begin{array}{c}1\\ \frac{1}{2}\varphi u\end{array}\right]R=I+\varphi [u{]}_{\times }$$
对于JPL：
$$q\approx \left[\begin{array}{c}1\\ \frac{1}{2}\varphi u\end{array}\right]R=I-\varphi [u{]}_{\times }$$

参考文献

（1）Quaternion kinematics for the error-state Kalman filter