基于EKF的IMU姿态结算

EKF IMU

标签： IMU

---

说明

1. 具体代码实现：https://github.com/Xiarain/Rain_IMU
2. 代码实现部分分为三个部分，基于A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU中论文的实现，基于EKF-IMU算法的实现，基于ESKF-IMU算法的实现。

RAIN IMU姿态解算规则说明

四元数旋转矩阵(从body坐标系到world坐标系)：

Rwb=⎡⎣⎢⎢q2w+q2x−q2y−q2z2(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)q2w−q2x+q2y−q2z2(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)q2w−q2x−q2y+q2z⎤⎦⎥⎥

欧拉角符号说明：

• 偏航角，绕 Z 轴旋转，Yaw角， ψ
• 俯仰角，绕 Y 轴旋转，Pitch角， θ
• 滚转角，绕 X 轴旋转，Roll角， ϕ
• 定义欧拉角旋转顺序：先偏航角、后俯仰角、最后滚转角。

数值积分方法选择欧拉方法:

xn+1=xn+Δt⋅f(tn,xn)

状态量选择：

x=[qwb]

实现步骤：

step1:

初始化四元数:

θ=atan2(Acc.xAcc.y2+Acc.z2)ϕ=atan2(−Acc.y−Acc.z)ψ=atan2(−Mag.y∗cosφ+Mag.z∗sinφMag.x∗cosθ+Mag.y∗sinθsinφ+Mag.z∗sinθcosφ)

初始化 Q 过程激励噪声协方差矩阵和 R 观测噪声协方差矩阵：

Q=[wn00wbn]

R=[an00mn]

step2:

状态观测量：
需要注意的是这里的状态观测量需要对加速度和磁力计进行归一化，这个非常重要。

zk=[ammm]

step3:

状态转移方程：

x^k|k−1=f(x^k−1|k−1,uk−1)

qk+1←qk+12qk⊗q{(wm−wb)Δt}wbk+1←wbk

将四元数状态转移方程展开则为如下公式：

12q⊗q{(wm−wb)T}=T2⎡⎣⎢⎢⎢⎢qwqxqyqz⎤⎦⎥⎥⎥⎥⊗⎡⎣⎢⎢⎢⎢0wx−wbxwy−wbywz−wbz⎤⎦⎥⎥⎥⎥=T2[q]Lq{(wm−wb)T}=⎡⎣⎢⎢⎢⎢⎢−qx(wx−wbx)−qy(wy−wby)−qz(wz−wbz)qw(wx−wbx)+qy(wz−wbz)−qz(wy−wby)qw(wy−wby)−qx(wz−wbz)+qz(wx+wbx)qw(wz−wbz)+qx(wy−wby)−qy(wx−wbx)⎤⎦⎥⎥⎥⎥⎥∗T2

需要进行对状态量中的四元数中归一化。

step4:

状态转移方程矩阵表示：

[qk+1wbk+1]=⎡⎣I+12ΩT00I⎤⎦[qkwbk]

求状态转移方程的雅克比矩阵：

Fk−1=∂f∂x|x^k|k−1|uk−1

Fk=⎡⎣⎢I+T2Ω0−T2[qk]L[0I3×3]I⎤⎦⎥

计算协方差矩阵:

Pk|k−1=Fk−1⋅Pk−1|k−1⋅FTk−1+Qk−1

step5：

测量方程：
重力在世界坐标系下归一化可以得到重力参考值

Aref=[00|g|]T

加速度计可以测量重力在机体坐标系下各轴的分量。

加速度测量模型：

h1()=g^=(Rwb)T⎡⎣⎢00|g|⎤⎦⎥=|g|⎡⎣⎢⎢2qxqz−2qwqy2qwqx+2qyqzq2w−q2x−q2y+q2z⎤⎦⎥⎥

由于加速度计测量原理的构造，加速度测量模型测量的加速度值是以单位重力 g 为单位的，所以这里 |g|=1 。

Hk1=∂h1[i]∂q[j]=⎡⎣⎢⎢−2qy2qx2qw2qz2qw−2qx−2qw2qz−2qy2qx2qy2qz⎤⎦⎥⎥

为了消除环境因素的干扰，需要进行坐标变换；因为磁倾角的存在，地球的磁场强度可以认为是由水平和垂直两部分组成的。一个加速度计可以提供一个姿态的参考，而且可以用来补偿测量地球磁场强度中倾斜错误。将传感器数值从传感器参考系转换到地球参考系。

hmk=q⊗mk⊗q∗bk=[0h2x+h2y−−−−−−√20hz]

h2()=m^=(Rbn)Tbk=⎡⎣⎢⎢q2w+q2x−q2y−q2z2(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)q2w−q2x+q2y−q2z2(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)q2w−q2x−q2y+q2z⎤⎦⎥⎥T∗⎡⎣⎢bx0bz⎤⎦⎥=⎡⎣⎢⎢bx⋅(q2w+q2x−q2y−q2z)+2bz⋅(qxqz−qwqy)2bx⋅(qxqy−qwqz)+2bz⋅(qyqz+qwqx)2bx⋅(qwqy+qxqz)+bz⋅(q2w−q2x−q2y+q2z)⎤⎦⎥⎥

对 h2 求解雅克比矩阵：

Hk2=⎡⎣⎢⎢2bxqw−2bzqy−2bxqz+2bzqx2bxqy+2bzqw2bxqx+2bzqz2bxqy+2bzqw2bxqz−2bzqx−2bxqy−2bzqw2bxqx+2bzqz2bxqw−2bzqy−2bxqz+2bzqx−2bxqw+2bzqy2bxqx+2bzqz⎤⎦⎥⎥

step6：

计算卡尔曼增益：

Kk=Pk|k−1HTk(HkPk|k−1HTk+Rk)−1

Rk 噪声协方差矩阵

step7:

更新后验状态：

x^k|k=x^k|k−1+Kk(zk−h(x^k|k−1))

step7:

更新后验误差协方差矩阵：

Pk|k=(I−KkHk)Pk|k−1

注意事项：

1.大型矩阵的构造应该是尝试使用基础矢量或者矩阵的数学构造的方式，而不是通过人为的方式来填充，这样太容易出现错误，而且很不容易查找错误。
2.不知道是不是因为数据集的关系，在加速度计和磁力计的观测矩阵中，加速度计的观测矩阵与理论分析中相差一个负号，而磁力计是与理论分析是一样的。