基于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 = [ q t ω b t ] x_t= \begin{bmatrix} q_t \\ \omega_{bt} \end{bmatrix} xt​=[qt​ωbt​​]， q t q_t qt​为真实四元数， ω b t \omega_{bt} ωbt​为陀螺仪的真实bias。
​

2）状态量（名义状态量）：
x = [ q ω b ] x= \begin{bmatrix} q \\ \omega_b \end{bmatrix} x=[qωb​​]， q q q为四元数， ω b \omega_{b} ωb​为陀螺仪的bias。
​

3）误差状态量：
δ x = [ δ q δ ω b ] \delta x= \begin{bmatrix} \delta q \\ \delta\omega_b \end{bmatrix} δx=[δqδωb​​]， δ q \delta q δq、 δ ω b \delta\omega_b δωb​分别真实状态量和名义状态量的误差。
​

那么他们之间有这样的关系：
x t = x ⊕ δ x = [ q × δ q ω b + δ ω b ] x_t = x\oplus\delta x= \begin{bmatrix} q\times \delta q \\ \omega_b + \delta\omega_b \end{bmatrix} xt​=x⊕δx=[q×δqωb​+δωb​​]​
其中， δ q \delta q δq和 δ θ \delta\theta δθ的关系为： δ q = e δ θ / 2 \delta q=e^{\delta\theta/2} δq=eδθ/2，因为有

q ≜ E x p ( ϕ u ) = e ϕ u / 2 = c o s ( ϕ / 2 ) + u s i n ( ϕ / 2 ) = [ c o s ( ϕ / 2 ) u s i n ( ϕ / 2 ) ] q\triangleq Exp(\phi \boldsymbol{u}) =e^{\phi \boldsymbol{u}/2} = cos (\phi/2) + \boldsymbol{u}sin(\phi/2) = \begin{bmatrix} cos(\phi/2) \\ \boldsymbol{u}sin(\phi/2) \end{bmatrix} q≜Exp(ϕu)=eϕu/2=cos(ϕ/2)+usin(ϕ/2)=[cos(ϕ/2)usin(ϕ/2)​]

也就是常用的轴角公式，由于 δ θ \delta\theta δθ只需要3个变量表示，所以误差状态量又可以表示为更简洁的形式：
δ x = [ δ θ δ ω b ] \delta x= \begin{bmatrix} \delta \theta \\ \delta\omega_b \end{bmatrix} δx=[δθδωb​​]
​

2.2 imu的测量值

1）加速度计
a t = a m + a n a_t=a_m+a_n at​=am​+an​
其中 a t a_t at​为真值， a m a_m am​为测量值， a n a_n an​为测量噪声。
​
2）陀螺仪
ω t = ω m + ω n \omega_t = \omega_m + \omega_n ωt​=ωm​+ωn​
其中 ω t \omega_t ωt​为真值， ω m \omega_m ωm​为测量值， ω n \omega_n ωn​为测量噪声。

2.3 预测方程及雅克比矩阵

这里我们是用陀螺仪的数据对姿态进行预测。
对于名义状态量，有：
{ q ˙ = 1 2 q ⊗ ( ω m − ω b ) ω ˙ b = 0 \left \{ \begin{array}{ll} \dot{q} = \frac{1}{2}q \otimes(\omega_m - \omega_b) \\ \dot{\omega}_b = 0 \end{array} \right. {q˙​=21​q⊗(ωm​−ωb​)ω˙b​=0​

对于误差状态量，有：
{ δ θ ˙ = − [ ω m − ω b ] × δ θ − δ ω b − ω n δ ω b ˙ = ω ω \left \{ \begin{array}{ll} \dot{\delta \theta} = -[\omega_m - \omega_b]_{\times}{\delta \theta} - \delta{\omega_b} - \omega_n \\ \dot{\delta \omega _b} = \omega_{\omega} \\ \end{array} \right. {δθ˙=−[ωm​−ωb​]×​δθ−δωb​−ωn​δωb​˙​=ωω​​
上式中，
ω n \omega_n ωn​为陀螺仪数据噪声， ω ω \omega_{\omega} ωω​为陀螺仪零偏的噪声（仔细品味一下，象征陀螺仪零偏的稳定性）； [ ∙ ] × [\bullet]_{\times} [∙]×​为反对称矩阵。

这里，我们把误差状态的系统方程的一般形式表示为：
δ x ← f ( x , δ x , u m , i ) = F x ( x , u m ) ⋅ δ x + F i ⋅ i \delta x \gets f(x,\delta x, u_m, i) = F_x(x, u_m) \cdot \delta x + F_i \cdot i δx←f(x,δx,um​,i)=Fx​(x,um​)⋅δx+Fi​⋅i
上式中， u m u_m um​为系统的输入， i i i为噪声。
​
那么其预测方程可以写为：
δ x ^ ← F x ( x , u m ) ⋅ δ x ^ \hat{\delta x} \gets F_x(x, u_m) \cdot \hat{\delta x} δx^←Fx​(x,um​)⋅δx^
P ← F x P F x T + F i Q i F i T P \gets F_xPF^T_x + F_iQ_iF^T_i P←Fx​PFxT​+Fi​Qi​FiT​
其中:
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​，有 [ u ] × 2 = u u T − I [\boldsymbol{u}]^2_{\times} = \boldsymbol{u}\boldsymbol{u}^T-I [u]×2​=uuT−I
F i = ∂ f ∂ i ∣ x , u m = [ I 0 0 I ] F_i = \frac{\partial f}{\partial i} \lvert_{x,u_m}= \begin{bmatrix} I & 0 \\ 0&I \end{bmatrix} Fi​=∂i∂f​∣x,um​​=[I0​0I​]

Q i = [ σ ω n 2 Δ t 2 I 0 0 σ ω ω 2 Δ t I ] Q_i = \begin{bmatrix} \sigma^2_{\omega_n}\Delta t^2 I & 0 \\ 0 & \sigma^2_{\omega_{\omega}}\Delta t I \end{bmatrix} Qi​=[σωn​2​Δt2I0​0σωω​2​ΔtI​]​

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

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

2.4 测量方程及雅克比矩阵

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

带入 R T R^T RT，可以把上式化简为：
y = h ( x t ) + v = − R t T g + a n = − R t T [ 0 0 g ] + a n = [ 2 ( q x q z − q w q y ) 2 ( q y q z + q w q x ) q w 2 − q x 2 − q y 2 + q z 2 ] ( − g ) + a n \begin{aligned} y & =h(x_t)+v \\ & = -R^T_t \boldsymbol{g} + a_n \\ & = -R^T_t \begin{bmatrix} 0\\0\\g \end{bmatrix}+ a_n =\begin{bmatrix} 2(q_xq_z-q_wq_y) \\ 2(q_yq_z+q_wqx) \\ q^2_w -q^2_x-q^2_y+q^2_z \end{bmatrix} (-g) + a_n \end{aligned} y​=h(xt​)+v=−RtT​g+an​=−RtT​⎣⎡​00g​⎦⎤​+an​=⎣⎡​2(qx​qz​−qw​qy​)2(qy​qz​+qw​qx)qw2​−qx2​−qy2​+qz2​​⎦⎤​(−g)+an​​

然后将 h ( x t ) h(x_t) h(xt​)对 δ x \delta x δx求偏导，即
H ≜ ∂ h ∂ δ x ∣ x = ∂ h ∂ x t ∣ x ∂ x t ∂ δ x ∣ x = H x X δ 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∂h​∣x​=∂xt​∂h​∣x​∂δx∂xt​​∣x​=Hx​Xδx​

​ H x = ∂ h ∂ δ x ∣ x = [ − 2 q y 2 q z − 2 q w 2 q x 2 q x 2 q w 2 q z 2 q y 2 q w − 2 q x − 2 q y 2 q z ] ( − g ) H_x = \frac{\partial h}{\partial \delta x}|_x = \begin{bmatrix} -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{bmatrix} (-g) Hx​=∂δx∂h​∣x​=⎣⎡​−2qy​2qx​2qw​​2qz​2qw​−2qx​​−2qw​2qz​−2qy​​2qx​2qy​2qz​​⎦⎤​(−g)

X δ x = [ ∂ ( q ⊗ δ q ) ∂ δ θ 0 0 ∂ ( ω b + δ ω b ) ∂ δ ω b ] = [ Q δ θ 0 0 I ] X_{\delta x} = \begin{bmatrix} \frac{\partial(q \otimes \delta q)}{\partial \delta \theta} & 0 \\ 0 & \frac{\partial(\omega_b + \delta \omega _b)}{\partial \delta \omega _b} \end{bmatrix} = \begin{bmatrix} Q_{\delta_\theta} & 0 \\ 0 & I \end{bmatrix} Xδx​=[∂δθ∂(q⊗δq)​0​0∂δωb​∂(ωb​+δωb​)​​]=[Qδθ​​0​0I​]

Q δ θ = 1 2 [ − q x − q y − q z q w − q z q y q z q w − q x − q y q x q w ] Q_{\delta \theta} = \frac{1}{2} \begin{bmatrix} -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{bmatrix} Qδθ​=21​⎣⎢⎢⎡​−qx​qw​qz​−qy​​−qy​−qz​qw​qx​​−qz​qy​−qx​qw​​⎦⎥⎥⎤​

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 代码下载链接

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

​