ESKF的预测过程分析在上一篇博客，此篇分析更新过程，并配合实际示例验证结果．

融合IMU与互补的传感器数据

包含以下两个过程:

更新（通过观测模型更新ESKF状态δx,P\delta\mathbf{x},\mathbf{P}δx,P）
假设观测方程为: y=h(Xt)+v\mathbf{y}=h(\mathbf{X_{t}})+vy=h(Xt)+v，其中vvv是观测传感器的测量噪声，满足v∼N(0,V)v\sim N(0, \mathbf{V})v∼N(0,V).
由于h(Xt)h(\mathbf{X_{t}})h(Xt)是非线性方程，在进行更新过程之前需要对其进行泰勒级数展开:
y≈h(xt^)+Hδx+vy\approx h(\hat{\mathbf{x_t}})+\mathbf{H}\delta \mathbf{x}+vy≈h(xt^)+Hδx+v
式中H\mathbf{H}H为h(∗)h(*)h(∗)对误差状态δx\delta {\mathbf{x}}δx的偏导数，所以根据链式法则有如下关系:
H=∂h∂δxt∣x=∂h∂xt∣x∂xt∂δxt∣x=HxXδ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
其中Hx\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=∂xt∂δx∣x=[∂(p+δp)∂δp∂(v+δv)∂δv0∂(q⊗δq)∂δθ∂(ab+δab)∂δab0∂(ωb+δωb)∂δωb∂(g+δg)∂δg]=[I6000Qδθ000I9]\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→[112δθ]\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)12[000100010001]=12[−qx−qy−qzqω−qzqyqzqω−qx−qyqxqω]\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=PHT(HPHT+V)−1δx^←K(y−h(xt^))P←(I−KH)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{δθ^}ab←ab+δab^ω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=[I3×30]3×15X+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观测值，为三维位置信息．vvv为测量噪声，满足v∼N(0,V)v\sim N(0,\mathbf{V})v∼N(0,V), V\mathbf{V}V的值一般是包含在传感器的数据帧里面.
Hx\mathbf{H_{x}}Hx求解
由于观测方程是线性的，所以Hx＝[I3×30]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仓库里面．
结果
按照仓库里面的说明执行代码，可获得如下结果:
结论
从上述结果图可以看出，整条运动轨迹比较顺滑，因此可以初步证明该算法的有效性．

Quaternion kinematics for ESKF－更新相关推荐

Quaternion kinematics for ESKF－预测
前言在看vins-mono, okvis, msckf等基于imu的vslam论文时,发现这些主流的slam前端vio都需要计算imu的预积分及其协方差,而计算方法类似,都是参考Quaternion ...
Quaternion kinematics for the error-state Kalman filter 资料整理
Quaternion kinematics for ESKF-预测 Quaternion kinematics for ESKF-更新文章翻译-基于误差状态卡尔曼滤波器的四元数运动学-前言 ESKF ...
Quaternion kinematics for error state kalman filter实现GPS+IMU融合，（附源码）
最近在学习kalman滤波相关的知识,恰好工作可能需要使用ESKF算法,因此将Joan Sola大神的书看了一遍,同时推导了相关的公式.俗话说得好:"Talk is cheap, show ...
Quaternion kinematics for the error-state Kalman filter论文阅读
文章目录 0. 参考文献 1. 四元数的定义和属性 1.1 四元数的定义 1.1.1 四元数的表示方式 1.2 四元数的性质 1.2.1 加和 1.2.2 积 1.2.3 单位1 1.2.4 共轭 1 ...
重读经典《Quaternion kinematics for the error-state Kalman filter》
本文将介绍一篇关于四元数运动学的误差卡尔曼滤波经典论文.本文结构如下: 第1章四元数定义和性质介绍,包括:加法.减法.乘法(矩阵表示).模.幂数.指数运算等. 第2章旋转群定义和性质介绍,包括:旋 ...
Quaternion kinematics for the error-state Kalman filter 文章理解
1. 四元数的定义和性质 1.1 四元数的定义 Cayley-Dickson construction 给出了一种非常吸引人的四元数定义:如果有两个复数A=a+biA=a+biA=a+bi 和 C=c ...
Quaternion kinematics for the error-state Kalman filter
一.四元数的属性与定义 1.1 四元数的定义一种比较有吸引力的克莱迪克森定义四元数的方式:用例两个复数,去定义一个新的四元数. 定义了纯四元数的概念单位长度的复数可以表示2D的旋转,同样的,单位长 ...
Quaternion kinematics for the error-state Kalman filter 学习笔记
1.卡尔曼滤波器的核心是追踪状态变量的均值和方差在一个动力系统里如何变化,以及计算关于测量值的条件期望和条件方差.在 IMU 的姿态估计中,姿态的均值和方差随时间的变化是由陀螺仪的误差模型所决定的. ...
VINS-Mono-IMU预积分原理及源码解析
IMU预积分原理 PVQ增量预积分(可理解为EKF过程中的状态估计) PVQ增量的连续形式参照VINS-Mono论文知识点精炼(翻译)中IMU预积分章节,可知PVQ增量连续时间积分公式如下: α b ...

Quaternion kinematics for ESKF－更新

融合IMU与互补的传感器数据

测试样例(利用ESKF算法融合IMU和GPS)

Quaternion kinematics for ESKF－更新相关推荐

最新文章

热门文章