ESKF的四元数运动学方程推导方法

在《自动驾驶与机器人中的SLAM技术：从理论到实践 》一书中给出了以李群为表示法推导的ESKF运动学方程，本文从四元数为表示法推导ESKF的状态转移方程。

首先是各个变量的名称和数值，如下表：

1. 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}
\)

2. 真值运动学方程

设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}
\)

3. 误差状态变量

根据上面的表格，定义误差状态变量：
\(
\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_{bg}\tag{1.52}\\
\dot{\delta b_{a}} & = & \eta_{ba}\tag{1.53}\\
\dot{\delta g} & = & 0\tag{1.54}
\end{eqnarray}
\)
其中，公式\((1.50)\)和\((1.51)\)分别是速度和旋转误差，需要针对\((1.32)\)和\((1.33)\)这两个非线性公式做一些复杂处理，得到一个线性方程，下面给出单独的推导。

3.1 误差状态的速度项

我们希望得到\(\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})\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.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} \nonumber\\
& =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\)，容易证明其协方差矩阵也不变
\(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}
\)

3.2 误差状态的旋转项

针对四元数表示旋转，回顾之前的内容，指定\(\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})) \nonumber\\
& -\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}
\)

参考文献：
[1] 高翔, 自动驾驶与机器人中的SLAM技术:从理论到实践[M], 北京:电子工业出版社, 2023.
[2] Solà, Joan.Quaternion kinematics for the error-state Kalman filter[J]. 2017.DOI:10.48550/arXiv.1711.02508.