Quaternion kinematics for the error-state KF (ESKF)－更新
ESKF的预测过程分析在上一篇博客，此篇分析更新过程，并配合实际示例验证结果．
融合IMU与互补的传感器数据
包含以下两个过程:
- 更新（通过观测模型更新ESKF状态 δ x , P \delta\mathbf{x},\mathbf{P} δx,P）
假设观测方程为: y = h ( X t ) + v \mathbf{y}=h(\mathbf{X_{t}})+v y=h(Xt)+v，其中 v v v是观测传感器的测量噪声，满足 v ∼ N ( 0 , V ) v\sim N(0, \mathbf{V}) v∼N(0,V).
由于 h ( X t ) h(\mathbf{X_{t}}) h(Xt)是非线性方程，在进行更新过程之前需要对其进行泰勒级数展开:
y ≈ h ( x t ^ ) + H δ x + v y\approx h(\hat{\mathbf{x_t}})+\mathbf{H}\delta \mathbf{x}+v y≈h(xt^)+Hδx+v
式中 H \mathbf{H} H为 h ( ∗ ) h(*) h(∗)对误差状态 δ x \delta {\mathbf{x}} δx的偏导数，所以根据链式法则有如下关系:
H = ∂ h ∂ δ x t ∣ x = ∂ h ∂ x t ∣ x ∂ x t ∂ δ x t ∣ x = H x X δ x \mathbf{H}=\frac{\partial h}{\partial \delta\mathbf{x}_{t}}|_{\mathbf{x}}=\frac{\partial h}{\partial \mathbf{x}_{t}}|_{\mathbf{x}}\frac{\partial \mathbf{x}_{t}}{\partial \delta\mathbf{x}_{t}}|_{\mathbf{x}}=\mathbf{H}_{\mathbf{x}}\mathbf{X}_{\delta{\mathbf{x}}} H=∂δxt∂h∣x=∂xt∂h∣x∂δxt∂xt∣x=HxXδx
其中 H x \mathbf{H}_{\mathbf{x}} Hx是 h ( ∗ ) h(*) h(∗)关于状态向量 X \mathbf{X} X的偏导数，可根据实际的观测模型来确定．而 X δ x \mathbf{X}_{\delta{\mathbf{x}}} Xδx是状态向量 X \mathbf{X} X关于误差状态向量 δ x \delta{\mathbf{x}} δx的偏导数，可直接通过下式获得:
X δ x = ∂ x t ∂ δ x ∣ x = [ ∂ ( p + δ p ) ∂ δ p ∂ ( v + δ v ) ∂ δ v 0 ∂ ( q ⊗ δ q ) ∂ δ θ ∂ ( a b + δ a b ) ∂ δ a b 0 ∂ ( ω b + δ ω b ) ∂ δ ω b ∂ ( g + δ g ) ∂ δ g ] = [ I 6 0 0 0 Q δ θ 0 0 0 I 9 ] \mathbf{X_{\delta{x}}} =\frac{\partial \mathbf{x}_{t}}{\partial \delta\mathbf{x}}|_{\mathbf{x}} =\begin{bmatrix} \frac{\partial (\mathbf{p+\delta p})}{\partial \delta\mathbf{p}} & & & & &\\ & \frac{\partial (\mathbf{v+\delta v})}{\partial \delta\mathbf{v}} & & & 0 &\\ & & \frac{\partial (\mathbf{q\otimes \delta q})}{\partial \delta\mathbf{\boldsymbol{\theta}}} & & &\\ & & & \frac{\partial (\mathbf{a}_b+\delta\mathbf{a}_{b})}{\partial \delta\mathbf{a}_{b}} & &\\ & 0 & & & \frac{\partial (\boldsymbol{\omega}_b+\delta\boldsymbol{\omega}_{b})}{\partial \delta\boldsymbol{\omega}_{b}} &\\ & & & & & \frac{\partial (\mathbf{g+\delta g})}{\partial \delta\mathbf{g}} \end{bmatrix} =\begin{bmatrix} \mathbf{I}_{6} & 0 & 0\\ 0 & \mathbf{Q}_{\delta_{\boldsymbol{\theta}}} & 0\\ 0 & 0 & \mathbf{I}_{9} \end{bmatrix} Xδx=∂δx∂xt∣x=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡∂δp∂(p+δp)∂δv∂(v+δv)0∂δθ∂(q⊗δq)∂δab∂(ab+δab)0∂δωb∂(ωb+δωb)∂δg∂(g+δg)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎡I6000Qδθ000I9⎦⎤
由 δ q → [ 1 1 2 δ θ ] \delta{\mathbf{q}\rightarrow \begin{bmatrix}1\\\frac{1}{2}\delta\boldsymbol{\theta}\end{bmatrix}} δq→[121δθ]可得:
Q δ θ = ∂ ( q ⊗ δ q ) ∂ δ q ∣ q ∂ δ q ∂ δ θ ∣ δ θ ^ = 0 = Q + ( q ) 1 2 [ 0 0 0 1 0 0 0 1 0 0 0 1 ] = 1 2 [ − q x − q y − q z q ω − q z q y q z q ω − q x − q y q x q ω ] \begin{matrix} \mathbf{Q}_{\delta_{\boldsymbol{\theta}}}=\frac{\partial (\mathbf{q}\otimes\mathbf{\delta q})}{\partial \delta\mathbf{q}}|_{\mathbf{q}}\frac{\partial \delta\mathbf{q}}{\partial \delta\boldsymbol{\theta}}|_{\hat{\delta\boldsymbol{\theta}}=0}\\ =\mathbf{Q}^{+}(\mathbf{q})\frac{1}{2}\begin{bmatrix}0&0&0\\1&0&0\\0&1&0\\0&0&1 \end{bmatrix}\\ =\frac{1}{2}\begin{bmatrix}-q_{x}&-q_{y}&-q_{z}\\q_{\omega}&-q_{z}&q_{y}\\q_{z}&q_{\omega}&-q_{x}\\ -q_{y}&q_{x}&q_{\omega} \end{bmatrix} \end{matrix} Qδθ=∂δq∂(q⊗δq)∣q∂δθ∂δq∣δθ^=0=Q+(q)21⎣⎢⎢⎡010000100001⎦⎥⎥⎤=21⎣⎢⎢⎡−qxqωqz−qy−qy−qzqωqx−qzqy−qxqω⎦⎥⎥⎤
至此实现了 H \mathbf{H} H的求解，再根据以下三个熟悉的卡尔曼滤波方程更新误差状态:
K = P H T ( H P H T + V ) − 1 δ x ^ ← K ( y − h ( x t ^ ) ) P ← ( I − K H ) P \begin{matrix} \mathbf{K}=\mathbf{PH}^{T}(\mathbf{HPH}^{T}+\mathbf{V})^{-1}\\ \hat{\delta\mathbf{x}}\leftarrow \mathbf{K}(\mathbf{y}-h(\hat{\mathbf{x}_{t}}))\\ \mathbf{P}\leftarrow(\mathbf{I-KH})\mathbf{P} \end{matrix} K=PHT(HPHT+V)−1δx^←K(y−h(xt^))P←(I−KH)P - 融合(将更新的误差状态融入到名义状态中)
p ← p + δ p ^ v ← v + δ v ^ q ← q ⊗ q { δ θ ^ } a b ← a b + δ a b ^ ω b ← ω b + δ ω b ^ g ← g + δ g ^ \begin{matrix} \mathbf{p}\leftarrow\mathbf{p}+\hat{\delta\mathbf{p}}\\ \mathbf{v}\leftarrow\mathbf{v}+\hat{\delta\mathbf{v}}\\ \mathbf{q}\leftarrow\mathbf{q}\otimes\mathbf{q}\left \{\hat{\delta\boldsymbol{\theta}} \right \}\\ \mathbf{a}_{b}\leftarrow\mathbf{a}_{b}+\hat{\delta\mathbf{a}_{b}}\\ \boldsymbol{\omega}_{b}\leftarrow\boldsymbol{\omega}_{b}+\hat{\delta\boldsymbol{\omega}_{b}}\\ \mathbf{g}\leftarrow\mathbf{g}+\hat{\delta{\mathbf{g}}} \end{matrix} p←p+δp^v←v+δv^q←q⊗q{δθ^}ab←ab+δab^ωb←ωb+δωb^g←g+δg^
测试样例(利用ESKF算法融合IMU和GPS)
上一篇博客和这篇博客分别讲了eskf算法的预测和更新过程，看完应该感叹一下这篇文章(Quaternion kinematics for the error-state KF)的魅力，它基本上把基于imu的eskf算法框架完整的呈现在你面前，并且公式写的非常详细，除了需要定义一下观测方程之外，其它的过程完全可以照着结论敲代码．
- 观测方程
z = [ I 3 × 3 0 ] 3 × 15 X + v \mathbf{z}=\begin{bmatrix}\mathbf{I}_{3\times3}&\mathbf{0}\end{bmatrix}_{3\times15}\mathbf{X}+v z=[I3×30]3×15X+v
式中 z \mathbf{z} z为gps观测值，为三维位置信息． v v v为测量噪声，满足 v ∼ N ( 0 , V ) v\sim N(0,\mathbf{V}) v∼N(0,V), V \mathbf{V} V的值一般是包含在传感器的数据帧里面. - H x \mathbf{H_{x}} Hx求解
由于观测方程是线性的，所以 H x ＝ [ I 3 × 3 0 ] 3 × 15 \mathbf{H_{x}}＝\begin{bmatrix}\mathbf{I}_{3\times3}&\mathbf{0}\end{bmatrix}_{3\times15} Hx＝[I3×30]3×15 - 代码
除了上述两个过程需要根据实际融合的观测传感器定义以外，其它的预测更新过程与博客中的一致，根据这些公式我写了一个测试demo，放在了https://github.com/chennuo0125-HIT/imu_gps_fusion仓库里面． - 结果
按照仓库里面的说明执行代码，可获得如下结果:
- 结论 从上述结果图可以看出，整条运动轨迹比较顺滑，因此可以初步证明该算法的有效性．