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

在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}
$$

如果我的文章对你有所帮助,那么不妨?