在SAD的书籍中已经有了ESKF的李群和李代数运动学方程的推导方法，本文主要是利用四元数来推导ESKF的运动学方程。
连续时间运动学方程
| Magnitude | True | Nominal | Error | Composition | Measured | Noise |
|---|---|---|---|---|---|---|
| Full state | $x_{t}$ | $x$ | $\delta x$ | $x_{t}=x\otimes\delta x$ | ||
| Position | $p_{t}$ | $p$ | $\delta p$ | $p_{t}=p+\delta p$ | ||
| Velocity | $v_{t}$ | $v$ | $\delta v$ | $v_{t}=v+\delta v$ | ||
| Quaternion | $q_{t}$ | $q$ | $\delta q$ | $q_{t}=q\delta q$ | ||
| Rotation matrix | $R_{t}$ | $R$ | $\delta R$ | $R_{t}=R\delta R$ | ||
| Angles vector | $\delta\theta$ | $\delta q=Exp(\frac{\delta\theta}{2})$$\delta R=Exp(\delta\theta)$ | ||||
| Accelerometer bias | $b_{at}$ | $b_{a}$ | $\delta b_{a}$ | $b_{at}=b_{a}+\delta b_{a}$ | $\eta_{ba}$ | |
| Gyrometer bias | $b_{gt}$ | $b_{g}$ | $\delta b_{g}$ | $b_{gt}=b_{g}+\delta b_{g}$ | $\eta_{bg}$ | |
| Gravity vector | $g_{t}$ | $g$ | $\delta g$ | $g_{t}=g+\delta g$ | ||
| Acceleration | $a_{t}$ | $a$ | $\tilde{a}$ | $\eta_{a}$ | ||
| Angular rate | $\omega_{t}$ | $\omega$ | $\tilde{\omega}$ | $\eta_{g}$ |
IMU测量方程中的噪声模型
记陀螺仪和加速度计的测量噪声分别为$\eta_{g},\eta_{a}$，同时记零偏为$b_{g},b_{a}$，下标$g$表示陀螺仪，下标$a$表示加速度计。那么这几个参数在测量方程中体现为
$$
\begin{eqnarray}
\tilde{a} & = & R^{T}(a_{t}-g)+b_{a}+\eta_{a}\tag{1.18}\
\tilde{\omega} & = & \omega_{t}+b_{g}+\eta_{g}\tag{1.19}
\end{eqnarray}
$$
一个普通的零偏$b$的随机游走过程可以建模为
$$
\begin{equation}
\dot{b}(t)=\eta_{b}(t)\tag{1.20}
\end{equation}
$$
其中$\eta_{b}(t)$也是一个高斯过程。于是，$b_{a}$和$b_{g}$的随机游走可以建模为
$$
\begin{eqnarray}
\dot{b}{a}(t) & = & \eta{ba}(t)\sim\mathcal{GP}(0,Cov(b_{a})\delta(t-t’))\tag{1.21}\
\dot{b}{g}(t) & = & \eta{bg}(t)\sim\mathcal{GP}(0,Cov(b_{g})\delta(t-t’))\tag{1.22}
\end{eqnarray}
$$
真值运动学方程
设ESKF的真值状态为$x_{t}=[p_{t},v_{t},q_{t},b_{at},b_{gt},g_{t}]^{T}$，下标$t$表示true，即真值状态。这个状态随时间改变，可以记作$x_{t}(t)$。在连续时间上，记IMU读数为$\tilde{\omega},\tilde{a}$，那么可以写出真值运动学方程式
$$$$
$$
\begin{eqnarray}
\dot{p}{t} & = & v{t}\tag{1.23}\
\dot{v}{t} & = & a{t}\tag{1.24}\
\dot{q}{t} & = & \frac{1}{2}q{t}\otimes\omega_{t}\tag{1.25}\
\dot{b}{gt} & = & \eta{bg}\tag{1.26}\
\dot{b}{at} & = & \eta{ba}\tag{1.27}\
\dot{g}{t} & = & 0\tag{1.28}
\end{eqnarray}
$$
根据公式$(1.18)$和$(1.19)$，真值可以表示为
$$
\begin{eqnarray}
a{t} & = & R_{t}(\tilde{a}-b_{at}-\eta_{a})+g_{t}\tag{1.29}\
\omega_{t} & = & \tilde{\omega}-b_{gt}-\eta_{g}\tag{1.30}
\end{eqnarray}
$$
其中，$R_{t}=R\delta R= RExp(\delta\theta)$，将上式带入真值运动学方程可以改写为
$$
\begin{eqnarray}
\dot{p}{t} & = & v{t}\tag{1.31}\
\dot{v}{t} & = & R{t}(\tilde{a}-b_{at}-\eta_{a})+g_{t}\tag{1.32}\
\dot{q}{t} & = & \frac{1}{2}q{t}\otimes(\tilde{\omega}-b_{gt}-\eta_{g})\tag{1.33}\
\dot{b}{gt} & = & \eta{bg}\tag{1.34}\
\dot{b}{at} & = & \eta{ba}\tag{1.35}\
\dot{g}_{t} & = & 0\tag{1.36}
\end{eqnarray}
$$
误差状态变量
根据上面的表格，定义误差状态变量：
$$
\begin{eqnarray}
p_{t} & = & p+\delta p\tag{1.37}\
v_{t} & = & v+\delta v\tag{1.38}\
q_{t} & = & q\delta q\tag{1.39}\
b_{gt} & = & b_{g}+\delta b_{g}\tag{1.40}\
b_{at} & = & b_{a}+\delta b_{a}\tag{1.41}\
g_{t} & = & g+\delta g\tag{1.42}
\end{eqnarray}
$$
不带下标的就是名义状态变量。名义状态变量的运动学方程与真值相同，只是不必考虑噪声（因为噪声在误差状态方程中考虑了）。名义状态变量的运动学方程可以写为
$$
\begin{eqnarray}
\dot{p} & = & v\tag{1.43}\
\dot{v} & = & R(\tilde{a}-b_{a})+g\tag{1.44}\
\dot{q} & = & \frac{1}{2}q\otimes(\tilde{\omega}-b_{g})\tag{1.45}\
\dot{b}{g} & = & 0\tag{1.46}\
\dot{b}{a} & = & 0\tag{1.47}\
\dot{g} & = & 0\tag{1.48}
\end{eqnarray}
$$
在误差状态的公式$(1.37)(1.40)(1.41)(1.42)$中，在等式两侧分别对时间求导，很容易得到对应的时间导数表达式：
$$
\begin{eqnarray}
\dot{\delta p} & = & \delta v\tag{1.49}\
\dot{\delta v} & = & -R(\tilde{a}-b_{a})^{\wedge}\delta\theta-R\delta b_{a}-\eta_{a}+\delta g\tag{1.50}\
\dot{\delta\theta} & = & -(\tilde{\omega}-b_{g})^{\wedge}\delta\theta-\delta b_{g}-\eta_{g}\tag{1.51}\
\dot{\delta b_{g}} & = & \eta_{g}\tag{1.52}\
\dot{\delta b_{a}} & = & \eta_{a}\tag{1.53}\
\dot{\delta g} & = & 0\tag{1.54}
\end{eqnarray}
$$
其中，公式$(1.50)$和$(1.51)$分别是速度和旋转误差，需要针对$(1.32)$和$(1.33)$这两个非线性公式做一些复杂处理，得到一个线性方程，下面给出单独的推导。
误差状态的速度项
我们希望得到$\dot{\delta v}$这个关于速度误差的方程，考虑公式$(1.38)$的误差形式。对两侧求时间导数，就可以得到$\dot{\delta v}$的表达式。
公式$(1.38)$的左侧为：
$$
\begin{align}
\dot{v}{t} & =R{t}(\tilde{a}-b_{at}-\eta_{a})+g_{t}\nonumber \
& =RExp(\delta\theta)(\tilde{a}-b_{a}-\delta b_{a}-\eta_{a})+g+\delta g\nonumber \
& \approx R(I+\delta\theta^{\wedge})(\tilde{a}-b_{a}-\delta b_{a}-\eta_{a})+g+\delta g\nonumber \
& =R(\tilde{a}-b_{a})+R(-\delta b_{a}-\eta_{a})+R\delta\theta^{\wedge}(\tilde{a}-b_{a})+R\delta\theta^{\wedge}(-\delta b_{a}-\eta_{a})+g+\delta g\nonumber
\end{align}\tag{1.55}
$$
其中上式分别将真值$b_{at}$和$g_{t}$展开为名义状态变量和误差状态变量，“$\approx$”来自于$Exp(\delta\theta)$的展开，省略了二阶小量。
公式$(1.38)$的右侧为：
$$
\begin{align}
\dot{v}+\dot{\delta v} & =R(\tilde{a}-b_{a})+g+\dot{\delta v}\tag{1.56}
\end{align}
$$
因为公式$(1.38)$的两侧相等，可以得到
$$
\begin{align}
R(\tilde{a}-b_{a})+g+\dot{\delta v} & =R(\tilde{a}-b_{a})+R(-\delta b_{a}-\eta_{a}) \nonumber\
& +R\delta\theta^{\wedge}(\tilde{a}-b_{a})+R\delta\theta^{\wedge}(-\delta b_{a}-\eta_{a})+g+\delta g\nonumber
\end{align}\tag{1.57}
$$
将公式两侧的$R(\tilde{a}-b_{a})+g$消去后，可以得到
$$
\begin{align}
\dot{\delta v} & =R(-\delta b_{a}-\eta_{a})+R\delta\theta^{\wedge}(\tilde{a}-b_{a})+R\delta\theta^{\wedge}(-\delta b_{a}-\eta_{a})+\delta g \nonumber\
& =-R\delta b_{a}-R\eta_{a}+R\delta\theta^{\wedge}\tilde{a}-R\delta\theta^{\wedge}b_{a}-R\delta\theta^{\wedge}\delta b_{a}-R\delta\theta^{\wedge}\eta_{a}+\delta g \nonumber\
& \approx-R\delta b_{a}-R\eta_{a}+R\delta\theta^{\wedge}\tilde{a}-R\delta\theta^{\wedge}b_{a}+\delta g\nonumber\
& =-R\delta b_{a}-R\eta_{a}-R\tilde{a}^{\wedge}\delta\theta+Rb_{a}^{\wedge}\delta\theta+\delta g \nonumber\
& =-R(\tilde{a}-b_{a})^{\wedge}\delta\theta-R\delta b_{a}-R\eta_{a}+\delta g\nonumber
\end{align}\tag{1.58}
$$
因此，
$$
\begin{equation}
\boxed{\dot{\delta v}=-R(\tilde{a}-b_{a})^{\wedge}\delta\theta-R\delta b_{a}-R\eta_{a}+\delta g}\tag{1.59}
\end{equation}
$$
这样就得到了$\delta v$的运动学模型。需要补充一句，式$(1.59)$中的$\eta_{a}$是一个零均值白噪声，也就是说
$$
\begin{equation}
E[\eta_{a}]=0\qquad E[\eta_{a}\eta_{a}^{T}]=\sigma_{a}^{2}I
\end{equation}\tag{1.60}
$$
它乘上任意旋转矩阵之后仍然是一个零均值白噪声，而且$R^{T}R=I$，容易证明其协方差矩阵也不变（Proof:$E[R\eta_{a}]=RE[\eta_{a}]=0$，$E[(R\eta_{a})(R\eta_{a})^{T}]=RE[\eta_{a}\eta_{a}^{T}]R^{T}=R\sigma_{a}^{2}IR^{T}=\sigma_{a}^{2}I$），因此，我们可以得到
$$
\begin{equation}
\eta_{a}\leftarrow R\eta_{a}
\end{equation}\tag{1.61}
$$
因此，公式$(1.59)$可以简化为公式
$$
\begin{equation}
\boxed{\dot{\delta v}=-R(\tilde{a}-b_{a})^{\wedge}\delta\theta-R\delta b_{a}-\eta_{a}+\delta g}
\end{equation}\tag{1.62}
$$
误差状态的旋转项
针对四元数表示旋转，回顾之前的内容，指定$\mathbb{\phi}=\theta u$为表示绕轴$u$旋转$\theta$角度的旋转向量，那么指数映射可以通过欧拉公式展开，
$$
\begin{equation}
q=Exp(\theta u)=e^{\frac{\theta u}{2}}=cos\frac{\theta}{2}+usin\frac{\theta}{2}=\left[\begin{array}{c}
cos\frac{\theta}{2}\
usin\frac{\theta}{2}
\end{array}\right]
\end{equation}\tag{1.63}
$$
如果$\theta$为小量，那么可以近似表示为
$$
\begin{equation}
q=Exp(\theta u)=\left[\begin{array}{c}
cos\frac{\theta}{2}\
usin\frac{\theta}{2}
\end{array}\right]\approx\left[\begin{array}{c}
1\
\frac{\theta}{2}
\end{array}\right]
\end{equation}\tag{1.64}
$$
根据之前定义的符号
$$
\begin{equation}
q^{+}=\left[\begin{array}{cc}
s & -v^{T}\
v & sI+v^{\wedge}
\end{array}\right],\qquad q^{\otimes}=\left[\begin{array}{cc}
s & -v^{T}\
v & sI-v^{\wedge}
\end{array}\right]
\end{equation}\tag{1.65}
$$
四元数的运算有以下性质：
$$
\begin{equation}
q_{1}q_{2}=q_{1}^{+}q_{2}=q_{2}^{\otimes}q_{1}
\end{equation}\tag{1.66}
$$
我们希望得到$\dot{\delta\theta}$这个关于角度误差的方程，我们根据下面的方程
$$
\begin{eqnarray}
\dot{q}{t} & =\frac{1}{2}q{t}\otimes\omega_{t}= & \frac{1}{2}q_{t}\otimes(\tilde{\omega}-b_{gt}-\eta_{g})\tag{1.67}
\end{eqnarray}
$$
$$
\begin{eqnarray}
\dot{q} & =\frac{1}{2}q\otimes\omega= & \frac{1}{2}q\otimes(\tilde{\omega}-b_{g})\nonumber
\end{eqnarray}\tag{1.68}
$$
分别是四元数真值和名义状态变量导数的定义，注意，这里面的$\omega_{t}$和$\omega$都是四元数。
针对公式$(1.67)$，我们针对左右两侧分别计算
$$
\begin{eqnarray}
(\dot{q\delta q})= & \boxed{\dot{q_{t}}} & =\frac{1}{2}q_{t}Exp(\omega_{t}) \nonumber\
\dot{q}\delta q+q\dot{\delta q}= & & =\frac{1}{2}q\delta qExp(\omega_{t})\nonumber\
\frac{1}{2}qExp(\omega)\delta q+q\dot{\delta q}= & & =\frac{1}{2}q\delta qExp(\omega_{t}) \nonumber
\end{eqnarray}\tag{1.69}
$$
针对公式$(1.69)$两侧的计算，约掉$q$，并且将$\dot{\delta q}$移到一侧可以得到
$$
\begin{align}
\left[\begin{array}{c}
0\
\dot{\delta\theta}
\end{array}\right]=\boxed{2\dot{\delta q}} & =\delta qExp(\omega_{t})-Exp(\omega)\delta q \nonumber\
& =q^{\otimes}(\omega_{t})\delta q-q^{+}(\omega)\delta q \nonumber\
& =\left[q^{\otimes}(\omega_{t})-q^{+}(\omega)\right]\delta q \nonumber\
& =\left[\left[\begin{array}{cc}
0 & -\omega_{t}^{T}\
\omega_{t} & -\omega_{t}^{\wedge}
\end{array}\right]-\left[\begin{array}{cc}
0 & -\omega^{T}\
\omega & \omega^{\wedge}
\end{array}\right]\right]\left[\begin{array}{c}
cos\frac{\delta\theta}{2}\
sin\frac{\delta\theta}{2}
\end{array}\right]\nonumber\
& =\left[\begin{array}{cc}
0 & -\omega_{t}^{T}+\omega^{T}\nonumber\
\omega_{t}-\omega & -\omega_{t}^{\wedge}-\omega^{\wedge}
\end{array}\right]\left[\begin{array}{c}
cos\frac{\delta\theta}{2}\
sin\frac{\delta\theta}{2}
\end{array}\right] \
& \approx\left[\begin{array}{cc}
0 & -\omega_{t}^{T}+\omega^{T}\nonumber\
\omega_{t}-\omega & -(\omega_{t}+\omega)^{\wedge}
\end{array}\right]\left[\begin{array}{c}
1\
\frac{\delta\theta}{2}
\end{array}\right]\nonumber
\end{align}\tag{1.70}
$$
这样就分别得到一个标量等式，
$$
\begin{align}
0 & =(-\omega_{t}+\omega)^{T}\delta\theta\nonumber
\end{align}\tag{1.71}
$$
还有一个向量等式，
$$
\begin{align}
\dot{\delta\theta} & =\omega_{t}-\omega-\frac{1}{2}(\omega_{t}+\omega)^{\wedge}\delta\theta \nonumber\
& =((\tilde{\omega}-b_{gt}-\eta_{g})-(\tilde{\omega}-b_{g}))-\frac{1}{2}((\tilde{\omega}-b_{gt}-\eta_{g})+(\tilde{\omega}-b_{g}))^{\wedge}\delta\theta \nonumber\
& =-\delta b_{g}-\eta_{g}-\frac{1}{2}(2\tilde{\omega}-2b_{g}-\delta b_{g}-\eta_{g})^{\wedge}\delta\theta\nonumber\
& =-(\tilde{\omega}-b_{g})^{\wedge}\delta\theta-\delta b_{g}-\eta_{g}+\frac{1}{2}(\delta b_{g}+\eta_{g})^{\wedge}\delta\theta \nonumber\
& \approx-(\tilde{\omega}-b_{g})^{\wedge}\delta\theta-\delta b_{g}-\eta_{g}\nonumber
\end{align}\tag{1.72}
$$
标量等式都是二阶小量，用处不大。向量等式中第5行同样是省略了二阶小量，得到了角度误差的线性方程：
$$
\begin{equation}
\boxed{\dot{\delta\theta}=-(\tilde{\omega}-b_{g})^{\wedge}\delta\theta-\delta b_{g}-\eta_{g}}\tag{1.73}
\end{equation}
$$
