基于ESKF的IMU姿态融合【附MATLAB代码】

0 前言

在很多工程应用里都需要获得物体的姿态信息，而通过imu估计姿态应该最广。
仅使用imu获取姿态信息有很多算法，本文讨论基于ESKF（Error-State Kalman Filter）的算法。
本文偏向于算法的工程应用，因此省去了公式推导，但是会列出必不可少的核心公式。

1 什么是ESKF

ESKF（Error-State Kalman Filter）看起来很吓唬人，其实它和一般的KF、EKF没有什么本质的区别，只是状态量的选取不同罢了。
以位姿估计为例，普通的kalman filter的状态量（名义状态量）一般都是位置p、速度v、四元数q，而ESKF的状态量（误差状态量）是上述状态量的误差，如位置的误差δp、速度误差δv、姿态误差δq，而kalman得5个核心公式是没有区别的。
为了方便阅读，kalman的核心公式扔在这里。

​

优点（不翻译了）

2 系统方程

2.1 状态变量

1）真实状态量：
$x_t=\left[\begin{array}{c}{q}_t\\ {\omega }_{bt}\end{array}\right]$，$q_t$为真实四元数，${\omega }_{bt}$为陀螺仪的真实bias。
​

2）状态量（名义状态量）：
$x=\left[\begin{array}{c}q\\ {\omega }_b\end{array}\right]$，$q$为四元数，${\omega }_b$为陀螺仪的bias。
​

3）误差状态量：
$\delta x=\left[\begin{array}{c}\delta q\\ \delta {\omega }_b\end{array}\right]$，$\delta q$、$\delta {\omega }_b$分别真实状态量和名义状态量的误差。
​

那么他们之间有这样的关系：
$x_t=x\oplus \delta x=\left[\begin{array}{c}q\times \delta q\\ {\omega }_b+\delta {\omega }_b\end{array}\right]$​
其中，$\delta q$和 $\delta \theta$的关系为：$\delta q=e^{\delta \theta /2}$，因为有

$q\triangleq Exp(\varphi \mathbf{u})=e^{\varphi \mathbf{u}/2}=cos(\varphi /2)+\mathbf{u}sin(\varphi /2)=\left[\begin{array}{c}cos(\varphi /2)\\ \mathbf{u}sin(\varphi /2)\end{array}\right]$

也就是常用的轴角公式，由于$\delta \theta$只需要3个变量表示，所以误差状态量又可以表示为更简洁的形式：
$\delta x=\left[\begin{array}{c}\delta \theta \\ \delta {\omega }_b\end{array}\right]$
​

2.2 imu的测量值

1）加速度计
$a_t=a_m+a_n$
其中 $a_t$为真值，$a_m$为测量值， $a_n$为测量噪声。
​
2）陀螺仪
${\omega }_t={\omega }_m+{\omega }_n$
其中 ${\omega }_t$为真值，${\omega }_m$为测量值， ${\omega }_n$为测量噪声。

2.3 预测方程及雅克比矩阵

这里我们是用陀螺仪的数据对姿态进行预测。
对于名义状态量，有：
$\{\begin{array}{l}\dot{q}=\frac{1}{2}q\otimes ({\omega }_m-{\omega }_b)\\ {\dot{\omega }}_b=0\end{array}$

对于误差状态量，有：
$\{\begin{array}{l}\dot{\delta \theta }=-[{\omega }_m-{\omega }_b{]}_{\times }\delta \theta -\delta {\omega }_b-{\omega }_n\\ \dot{\delta {\omega }_b}={\omega }_{\omega }\end{array}$
上式中，
${\omega }_n$为陀螺仪数据噪声，${\omega }_{\omega }$为陀螺仪零偏的噪声（仔细品味一下，象征陀螺仪零偏的稳定性）；$[\bullet {]}_{\times }$为反对称矩阵。

这里，我们把误差状态的系统方程的一般形式表示为：
$\delta x\leftarrow f(x,\delta x,u_m,i)=F_x(x,u_m)\cdot \delta x+F_i\cdot i$
上式中，$u_m$为系统的输入，$i$为噪声。
​
那么其预测方程可以写为：
$\widehat{\delta x}\leftarrow F_x(x,u_m)\cdot \widehat{\delta x}$
$P\leftarrow F_{x}P{F}_x^T+F_i{Q}_i{F}_i^T$
其中:
F x = ∂ f ∂ δ x ∣ x , u m = [ R T { ( ω m − ω b ) Δ t } − I Δ t 0 I ] F_x = \frac{\partial f}{\partial \delta x} \lvert_{x,u_m} = \begin{bmatrix} R^T\{{(\omega_m-\omega_b)\Delta t}\} & -I\Delta t \\ 0 & I \end{bmatrix} Fx​=∂δx∂f​∣x,um​​=[RT{(ωm​−ωb​)Δt}0​−IΔtI​]

上式中， R { u } = I + s i n ϕ [ u ] × + ( 1 − c o s ϕ ) [ u ] × 2 R\{\boldsymbol{u}\} = I + sin \phi[\boldsymbol{u}]_{\times} + (1-cos \phi)[\boldsymbol{u}]^2_{\times} R{u}=I+sinϕ[u]×​+(1−cosϕ)[u]×2​，有$[\mathbf{u}{]}_{\times }^2=\mathbf{u}{\mathbf{u}}^T-I$
$F_i=\frac{\partial f}{\partial i}{|}_{x,u_m}=\left[\begin{array}{cc}I& 0\\ 0& I\end{array}\right]$

$Q_i=\left[\begin{array}{cc}{\sigma }_{{\omega }_n}^2\Delta t^{2}I& 0\\ 0& {\sigma }_{{\omega }_{\omega }}^2\Delta tI\end{array}\right]$​

需要注意的是，$\delta x$不需要做预测，因为每次初始化$\delta x$都为0，所以预测值肯定也是0。
有人会问，它会一直是0吗，经过几次运算迭代一直是0？
如果有这样的疑问，那就说明还没有从常规kalman filter的模式中走出来。ESKF每次迭代后，都会将最优估计得到的$\delta x$叠加到$x$中，所以对于下一时刻的开始，认为$x$是没有误差的，也就是认为$\delta x$为0了。
​

总结，在kalman预测这一步，每一轮新的kalman迭代，需要做的事情是：
1）对名义状态量进行预测；
3）计算雅克比矩阵
2）对误差状态量进行预测；（$\delta x$的值是0，可以不计算，但是不能认为它没有意义）

2.4 测量方程及雅克比矩阵

观测方程可以表示为下面的形式
$y=h(x_t)+v$
这里使用imu的加速度进行姿态的校正，假设的前提是没有机动加速度，只有重力加速度，所以有
$y=a_m=-R_t^T\mathbf{g}+a_n$，其中$R_t^T$是通过$q_t$得到的旋转矩阵的转置。
​

带入$R^T$，可以把上式化简为：
$\begin{array}{rl}y& =h(x_t)+v\\ & =-R_t^T\mathbf{g}+a_n\\ & =-R_t^T\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+a_n=\left[\begin{array}{c}2(q_x{q}_z-q_w{q}_y)\\ 2(q_y{q}_z+q_{w}qx)\\ q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right](-g)+a_n\end{array}$

然后将$h(x_t)$对$\delta x$求偏导，即
$H\triangleq \frac{\partial h}{\partial \delta x}{|}_x=\frac{\partial h}{\partial x_t}{|}_x\frac{\partial x_t}{\partial \delta x}{|}_x=H_x{X}_{\delta x}$

​$H_x=\frac{\partial h}{\partial \delta x}{|}_x=\left[\begin{array}{cccc}-2q_y& 2q_z& -2q_w& 2q_x\\ 2q_x& 2q_w& 2q_z& 2q_y\\ 2q_w& -2q_x& -2q_y& 2q_z\end{array}\right](-g)$

$X_{\delta x}=\left[\begin{array}{cc}\frac{\partial (q\otimes \delta q)}{\partial \delta \theta }& 0\\ 0& \frac{\partial ({\omega }_b+\delta {\omega }_b)}{\partial \delta {\omega }_b}\end{array}\right]=\left[\begin{array}{cc}{Q}_{{\delta }_{\theta }}& 0\\ 0& I\end{array}\right]$

$Q_{\delta \theta }=\frac{1}{2}\left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& -q_z& q_y\\ q_z& q_w& -q_x\\ -q_y& q_x& q_w\end{array}\right]$

3 kalman filter loop计算

上面的公式太多了，看得人头皮发麻，那就赶紧进入算法实践环节，接下来就可以按照Kalman Filter的“套路”计算了，大致的流程如下。

上图中的每一个公式或者变量都可以在前文中找到定义和说明，你要做的就是按照上述的流程进行计算就行。

4 Show me the code

有人说，没有代码代码，还是太抽象，好吧，这里奉上MATLAB脚本和仿真数据。

代码文件的结构如下。

​仿真结果如下。

%% 使用IMU的数据，基于ESKF的方法估计姿态
clc;clear;close;

%% 添加路径，加载数据
addpath('ESKF')
load('imu_log_data.mat');
mean_dt = mean(diff(imu.t));

%% 调用算法，记录数据
eskf.t = imu.t;
[eskf.quat, eskf.gyb, eskf.cov] = eskf_imu(imu.t, imu.gyr, imu.acc);
[eskf.yaw, eskf.pitch, eskf.roll] = quat2angle(eskf.quat,'ZYX');

%% 画图
figure('Name', 'euler in degree');
h1 = subplot(3,1,1);
plot(eskf.t, rad2deg(eskf.roll), 'r');
grid minor;title('roll')

h2 = subplot(3,1,2);
plot(eskf.t, rad2deg(eskf.pitch), 'r');
grid minor;title('pitch')

h3 = subplot(3,1,3);
plot(eskf.t, rad2deg(eskf.yaw), 'r');
grid minor;title('yaw')
linkaxes([h1 h2 h3],'x');

​

function [state_quat, state_gyb, state_cov] = eskf_imu(t, gyr, acc)

%% 参数初始化
quat = quaternion(1,0,0,0);
init_quat_var = power(4, 2);
init_gyb_var = power(1e-3, 2);

% 初始化P矩阵
P(1:3,1:3) = init_quat_var * eye(3);
P(4:6,4:6) = init_gyb_var * eye(3);
acc_meas_noise_var = power(2, 2); % 加速度计噪声方差
R = eye(3) * acc_meas_noise_var;
gyr_noise = power(1e-2, 2);
gyr_drift_noise = power(1e-3, 2);
gyb = zeros(1,3);

%% 申请内存，可以加快仿真速度
len = length(gyr);
state_quat = zeros(len,4);
state_gyb = zeros(len,3);
state_cov = zeros(len,6);

%% 循环计算
for index = 1:len
    if (index == 1)
        dt = saturate_user(t(2) - t(1),0.001,0.05);
    else
        dt = saturate_user(t(index) - t(index-1),0.001,0.05);
    end
    
    %                ---- 预测 -----                 %
    % 预测姿态角
    d_angle = (gyr(index,:) - gyb) * dt;
    dq = quaternion(d_angle, 'rotvec'); % 将旋转向量转化为四元数
    new_quat = quat * dq;
    quat = new_quat.normalize;
    
    % 预测协方差
    P = ESKF_predict_P(dt, P, gyr_noise, gyr_drift_noise);
    
    %                ---- 更新 -----                 %
    y = ESKF_predict_acc(quat); %
    H = ESKF_jacH(quat);    
    S = H * P * H' + R;
    K = P * H' / S;
    P = P - K * H * P;
    innov = K * (acc(index,:) - y)';
    dq = quaternion(innov(1:3)', 'rotvec');
    new_quat = quat * dq;
    quat = new_quat.normalize;
    
    gyb = gyb + innov(4:6)';
    
    state_quat(index,:) = quat.compact;
    state_gyb(index,:) = gyb;
    state_cov(index,:) =  diag(P)';
end

end

function out = saturate_user(in, min, max)
if (in < min)
    out = min;
elseif (in > max)
    out = max;
else
    out = in;
end
end

​

function new_P = ESKF_predict_P(dt, P, gyro_noise, gyr_drift_noise)

Fx = eye(6);
Fx(1:3,4:6) = -eye(3) * dt;
Fi = eye(6);
Qi(1:3, 1:3) = eye(3) * gyro_noise * dt * dt;
Qi(4:6, 4:6) = eye(3) * gyr_drift_noise * dt;

new_P = Fx * P * transpose(Fx) + Fi * Qi * transpose(Fi);
end

​

​

function pred_meas = ESKF_predict_acc(quat)

[qw, qx, qy, qz] = parts(quat);
gravity = 9.80665;
pred_meas(1) = -gravity * 2 * (qx * qz - qw * qy);
pred_meas(2) = -gravity * 2 * (qw * qx + qy * qz);
pred_meas(3) = -gravity * (qw * qw - qx * qx - qy * qy + qz * qz);

end

​

function H = ESKF_jacH(quat)

[qw, qx, qy, qz] = parts(quat);

Hx = zeros(3,7);
Xdx = zeros(7,6);

Hx(1:3,1:4) = -9.80665 * 2 * [  -qy,  qz,  -qw,  qx;...
                            qx, qw,  qz,  qy;...
                             qw, -qx, -qy,  qz];
Xdx(1:4,1:3) = 0.5 * [-qx, -qy, -qz;...
                       qw, -qz, qy;...
                       qz, qw, -qx;...
                      -qy, qx, qw];
Xdx(5:7,4:6) = eye(3);              

H = Hx * Xdx;

end

​

如果还有什么不清楚，欢迎评论区交流。
​

5 代码下载链接

可以在这里下载。链接: 点击下载。
​

​