四元数及error-state kalman

四元数相关的基本推导，主要是扰动相关。然后记下了一些error-state kalman相关的公式。

不做具体说明时，都是右手系

四元数乘法

$q \otimes p \neq p \otimes q$

其中$\otimes$为四元数乘法，$p$和$q$为四元数，四元数乘法满足结合律和分配律。四元数乘法为二元运算，可以写成两种等价的形式：

$q \otimes p = {[q]}_L p = {[p]}_Rq$

$[q]_L = q_wI + \begin{bmatrix} 0 & -q_v^T \\ q_v & [q_v]_{\times} \end{bmatrix}$

$[q]_R = q_wI + \begin{bmatrix} 0 & -q_v^T \\ q_v & -[q_v]_{\times} \end{bmatrix}$

其中$q = (q_w, q_v)$，$q_w$为实数，$q_v$为其中虚数向量。

左手系和右手系在四元数计算上的区别：

• 右手系：$ij = -ji = k$， $jk = -kj = i$，$ki = -ik = j$
• 左手系：$ij = -ji = -k$，$jk = -kj = -i$，$ki = -ik = -j$

Eigen中的四元数运算如果使用方法 coeffs()输出系数，输出的向量是按照$(q_v,q_w)$排列的。在Eigen中四元数的构造函数中，输入参数顺序是按照$(q_w,q_v)$排列的。所以想使用四元数乘法的时候可以构造四元数使用Eigen的四元数相乘函数来避免这些问题。但是想使用Eigen的四元数系数进行四元数乘法运算时（尤其对于论文里面经常提到的$\Omega$矩阵），其形式为
$[q]_L = q_wI +\begin{bmatrix}[q_v]_{\times} & q_v \\-q_v^T & 0\end{bmatrix}$
$[q]_R = q_wI +\begin{bmatrix}-[q_v]_{\times} & q_v \\-q_v^T & 0\end{bmatrix}$
当然计算结果自然是按照$(q_v,q_w)$排列的四元数

由于

$q \otimes r \otimes p = [p]_R[q]_Lr$

$q \otimes r \otimes p = [q]_L[p]_Rr$

因此有$[p]_R[q]_L = [q]_L[p]_R$

---

扰动及求导

先交代一些东西

$q_{AB} \otimes q_{BC} = q_{AC}$

$R_{AB} R_{BC} = R_{AC}$

$\Delta q = \begin{bmatrix} 1 \\ \frac{1}{2} \Delta \theta \end{bmatrix} + O \left( \| \Delta \theta \|^2 \right)$

$\Delta R = I + [\Delta \theta]_{\times} + O\left( \| \Delta \theta \|^2 \right)$

下文可能使用局部到全局坐标系的变换更容易理解

局部扰动

局部扰动也就是把扰动加在目前的坐标系数据下，然后再通过坐标系之间的关系变到其他坐标系下，具体形式为：

$\tilde{q} = q \otimes \Delta q$

$\tilde{R} = R \Delta R$

其扰动放在右边，其原因为$q_{AB'} = q_{AB} \otimes q_{BB'}$，可以满足先对局部数据进行扰动调整再通过坐标系关系变换到其他坐标系下。典型的应用为IMU预积分，具体形式为$q_{GI_{k+1}} = q_{GI_{k}} \otimes q_{I_{k}I_{k+1}}$。旋转矩阵形式类似。其对时间求导形式为：

$\dot{q} = \frac{1}{2} \Omega(\omega) q = \frac{1}{2} q \otimes \omega$

$\dot{R} = R[\omega]_{\times}$

其中$\omega$为局部角速度（当前坐标系的角速度），$\Omega(\omega)$为$[\omega]_R$（不要在意维数的问题）。具体推导如下：

$\begin{aligned} \dot{q} &= \lim_{\Delta t \rightarrow 0} \frac{q(t + \Delta t) - q(t)}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{q \otimes \Delta q - q}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{q \otimes \left(\begin{bmatrix}1 \\ \Delta \theta/2 \end{bmatrix} - \begin{bmatrix} 1 \\ 0 \end{bmatrix} \right)}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{q \otimes \left(\begin{bmatrix}0 \\ \Delta \theta/2 \end{bmatrix} \right)}{\Delta t} \\ &= \frac{1}{2} q \otimes \begin{bmatrix} 0 \\ \omega_L \end{bmatrix} \\ \\ \dot{R} &= \lim_{\Delta t \rightarrow 0} \frac{R(t + \Delta t) - R(t)}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{R \Delta R - R}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{R (\Delta R - I)}{\Delta t} \\ &= \lim_{\Delta t \rightarrow 0} \frac{R [\Delta \theta]_{\times}} {\Delta t} \\ &= R[\omega]_{\times} \end{aligned}$

全局扰动

和局部扰动相对，也就是先通过坐标系的关系变换到其他坐标系下再进行扰动。具体形式为：

$\tilde{q} = \Delta q \otimes q$

$\tilde{R} = \Delta R R$

由于$q_{A'B} = q_{A'A}q_{AB}$，也就是扰动没有加在局局部下，是加在了坐标系之间的变换这种全局关系中。其时间导数为：

$\dot{q} = \frac{1}{2} [\omega]_L q = \frac{1}{2} \omega \otimes q$

$\dot{R} = [\omega]_{\times} R$

其中$\omega$是全局角速度，也就是要目标坐标系下的角速度，不是当前坐标系下的角速度。

其他的求导

$\begin{aligned} \frac{\partial (q \otimes a \otimes q^*)}{\partial \theta} &= \frac{\partial (Ra)}{\partial \theta} \\ &= \lim_{\partial \theta \rightarrow 0} \frac{R\{ \theta + \partial \theta \}a - Ra}{\partial \theta} \\ &= \lim_{\partial \theta \rightarrow 0} \frac{(I + [\partial \theta]_{\times})Ra - Ra}{\partial \theta} \\ &= -\left[ Ra \right]_{\times} \\ \\ \frac{\partial (q \otimes a \otimes q^*)}{\partial q} &= \frac{\partial (Ra)}{\partial q} \\ &= 2 \left[ q_w a + q_v \times a | q_v^T a I + q_v a^T - a q_v^T - q_w [a]_{\times} \right] \end{aligned}$

---

旋转矩阵与四元数关系

（不要在意维数的细节问题）

$q \otimes r \otimes q^* = Rr$

$R_4 = [q^*]_R[q]_L = [q]_L[q^*]_R = \begin{bmatrix} 1 & 0 \\ 0 & R \end{bmatrix}$

---

系统运动（局部）

连续时间

论文里面多是给的连续时间的推导结果
true state：

$\dot{p}_t = v_t$

$\dot{v}_t = R_t(a_m - a_{bt} - a_n) + g_t$

$\dot{q}_t = \frac{1}{2} q_t \otimes (\omega_m - \omega_{bt} - \omega_n)$

$\dot{a}_{bt} = a_w$

$\dot{\omega}_{bt} = \omega _w$

$\dot{g}_t = 0$

其中$a_m$与$\omega_m$为测量值，$a_{bt}$与$\omega_{bt}$为true bias，$a_n$与$\omega_n$为噪声。
nominal state：

$\dot{p} = v$

$\dot{v} = R(a_m - a_b) + g$

$\dot{q} = \frac{1}{2} q \otimes (\omega_m - \omega_b)$

$\dot{a}_b = 0$

$\dot{\omega}_b = 0$

$\dot{g} = 0$

error state：

$\dot{\delta p} = \delta v$

$\dot{\delta v} = -R[a_m - a_b]_{\times} \delta \theta - R \delta a_b + \delta g - Ra_n$

$\dot{\delta \theta} = -[\omega_m - \omega_b]_{\times}\delta\theta - \delta \omega_b - \omega_n$

$\dot{\delta a_b} = a_w$

$\dot{\omega_b} = \omega_w$

$\dot{\delta g} = 0$

TODO：$\dot{\delta v}$和$\dot{\delta \theta}$的推导有时间补上

离散时间

其实和连续时间没差多少，就是把导数变成已经积分好的$x \leftarrow x + \dot{x}\Delta t$。数值积分方法还可以使用其他的，之后的具体形式都需要重新推导，这里是示例。
nominal state：

$p \leftarrow p + v\Delta t + \frac{1}{2} (R(a_m - a_b) + g)\Delta t^2$

$v \leftarrow v + (R(a_m - a_b) + g) \Delta t$

$q \leftarrow q \otimes q\{ (\omega_m - \omega_b)\Delta t \}$

$a_b \leftarrow a_b$

$\omega_b \leftarrow \omega_b$

$g \leftarrow g$

error state：

$\delta p \leftarrow \delta p + \delta v \Delta t$

$\delta v \leftarrow \delta v + (-R[a_m-a_b]_{\times}\delta \theta - R\delta a_b + \delta g) \Delta t + v_i$

$\delta \theta \leftarrow R^T\{ (\omega_m - \omega_b) \Delta t \}\delta \theta - \delta \omega_b \Delta t + \theta_i$

$\delta a_b \leftarrow \delta a_b + a_i$

$\delta \omega_b \leftarrow \delta \omega_b + \omega_i$

$\delta g \leftarrow \delta g$

$F$矩阵可以通过以上内容很容易推导出来，具体形式懒得写了。
error state的期望为$0$，在预测的过程中就是为了计算协方差矩阵，改变当前期望分布