基于EKF的IMU姿态解算
代码
参考代码的地址如下：
EKF_AHRS
主要代码如下：
% 基于EKF的IMU姿态解算(ADIS16470)
% 导航坐标：东北天坐标系(ENU)
% ADIS16470载体坐标系:X:右; Y:前; Z:上 ,逆时针欧拉角角度为正
% X:pitch; Y:roll; Z:yaw;
% 旋转顺序：z,x,y(偏航、俯仰，滚转)
% 备注：加速度数据以g为单位，不是以m/s2为单位
clc;
clear;
close all;
addpath('utils');
addpath('datasets');
rad2deg = 180/pi;
deg2rad = pi/180;
%% 读取数据
filename = 'ADIS16465-rm3100-2020-01-11-18-57-yaw90.txt';
nav_data = load(filename);
gap=1;
data_num=round(size(nav_data,1));
j=0;
for i=1:gap:data_num-gap
j=j+1;
DATA_CNTR(j,1) = mean(nav_data(i:i+gap-1,1)); %取值范围：0-65535
X_ACCL(j,1) = mean(nav_data(i:i+gap-1,2)); %单位:g
Y_ACCL(j,1) = mean(nav_data(i:i+gap-1,3));
Z_ACCL(j,1) = mean(nav_data(i:i+gap-1,4));
X_GYRO(j,1) = mean(nav_data(i:i+gap-1,5))/4;
Y_GYRO(j,1) = mean(nav_data(i:i+gap-1,6))/4;
Z_GYRO(j,1) = mean(nav_data(i:i+gap-1,7))/4;
TEMP_OUT(j,1) = mean(nav_data(i:i+gap-1,8));
X_MAG(j,1) = mean(nav_data(i:i+gap-1,9));
Y_MAG(j,1) = mean(nav_data(i:i+gap-1,10));
Z_MAG(j,1) = mean(nav_data(i:i+gap-1,11));
GAP_TIME(j,1) = mean(nav_data(i:i+gap-1,12))/1000000; %单位：us
end
%% 初始化数据
L = size(DATA_CNTR,1);
Time=zeros(L,1);
Time(1,1)=0;
acc = [X_ACCL,Y_ACCL,Z_ACCL];
gyro_bias = [0,0,0];
gyro = [(X_GYRO+gyro_bias(1))*deg2rad,(Y_GYRO+gyro_bias(2))*deg2rad,(Z_GYRO+gyro_bias(3))*deg2rad]; % gyro 单位：deg/sec
mag = [X_MAG,Y_MAG,Z_MAG];
wn_var = 1e-6 * ones(1,4);
wbn_var = 1e-8 * ones(1,3);
an_var = 1e-1 * ones(1,3);
Q = diag([wn_var, wbn_var]);
R = diag(an_var);
% EKF递推(AHRS)
g = 9.87;
acce_data0 = [X_ACCL(1),Y_ACCL(1),Z_ACCL(1)];
mag_data0=[X_MAG(1),Y_MAG(1),Z_MAG(1)];
[pitchEKF0,rollEKF0,yawEKF0] = Get_Init_AHRS(acce_data0,mag_data0);
[q0,q1,q2,q3]=quatfromeuler(pitchEKF0,rollEKF0,yawEKF0);
pitchEKF(1,1) = pitchEKF0*rad2deg;
rollEKF(1,1) = rollEKF0*rad2deg;
yawEKF(1,1) = yawEKF0*rad2deg;
pitch_gyro(1,1) = pitchEKF0*rad2deg;
roll_gyro(1,1) = rollEKF0*rad2deg;
yaw_gyro(1,1) = yawEKF0*rad2deg;
x_gyro = zeros(L,4);
x_gyro(1,:) = [q0,q1,q2,q3];
q_gyro(1,:) = [q0,q1,q2,q3];
x = zeros(L,7);
Pk = zeros(7,7,L);
x(1,:) = [q0,q1,q2,q3,0,0,0];
Pk(:,:,1) = eye(7);
% 磁场融合参数
yaw_mag = zeros(L,1);
yaw_mag(1,1) = yawEKF(1,1);
z_bias = zeros(L,1);
P_mag = zeros(2,2,L);
Q_mag = diag([1e-6,1e-8]);
R_mag= 1e-3;
x_mag = zeros(L,2);
%% 解算过程
for k=1:L-1
T = abs(GAP_TIME(k+1,1));
Time(k+1,1)=Time(k)+T;
fprintf("i = %d,process ratio = %f\n",k,k/L);
tic;
acc(k,:) = acc(k,:)/norm(acc(k,:),2);
z(k,:) = acc(k,:);
if(k==1)
x_gyro(k,:)=x(1,1:4);
else
A_gyro=[1,-(T/2)*gyro(k,1),-(T/2)*gyro(k,2),-(T/2)*gyro(k,3);
(T/2)*gyro(k,1),1,(T/2)*gyro(k,3),-(T/2)*gyro(k,2);
(T/2)*gyro(k,2),-(T/2)*gyro(k,3),1,(T/2)*gyro(k,1);
(T/2)*gyro(k,3),(T/2)*gyro(k,2),-(T/2)*gyro(k,1),1];
x_gyro(k,:)= (A_gyro*x_gyro(k-1,:)')';
x_gyro(k,:) = x_gyro(k,:)/norm(x_gyro(k,:),2);
[pitch_gyro(k,1),roll_gyro(k,1),yaw_gyro(k,1)] = quattoeuler(x_gyro(k,:));
end
gyro_x_bias = gyro(k,1)-x(k,5);
gyro_y_bias = gyro(k,2)-x(k,6);
gyro_z_bias = gyro(k,3);
A_11=[1,-(T/2)*gyro_x_bias,-(T/2)*gyro_y_bias,-(T/2)*gyro_z_bias;
(T/2)*gyro_x_bias,1,(T/2)*gyro_z_bias,-(T/2)*gyro_y_bias;
(T/2)*gyro_y_bias,-(T/2)*gyro_z_bias,1,(T/2)*gyro_x_bias;
(T/2)*gyro_z_bias,(T/2)*gyro_y_bias,-(T/2)*gyro_x_bias,1];
A_12=[0,0,0;
0,0,0;
0,0,0;
0,0,0];
A_21=[0,0,0,0;
0,0,0,0;
0,0,0,0];
A_22=[1,0,0;
0,1,0;
0,0,1];
A=[A_11,A_12;A_21,A_22];
Ak = eye(7)+T/2*...
[0 -(gyro_x_bias) -(gyro_y_bias) -(gyro_z_bias) x(k,2) x(k,3) x(k,4);
(gyro_x_bias) 0 (gyro_z_bias) -(gyro_y_bias) -x(k,1) x(k,4) -x(k,3);
(gyro_y_bias) -(gyro_z_bias) 0 (gyro_x_bias) -x(k,4) -x(k,1) x(k,2);
(gyro_z_bias) (gyro_y_bias) -(gyro_x_bias) 0 x(k,3) -x(k,2) -x(k,1);
0 0 0 0 0 0 0;
0 0 0 0 0 0 0;
0 0 0 0 0 0 0];
x_ = (A*x(k,:)')';
x_(1:4) = x_(1:4)/norm(x_(1:4),2);
Pk_ = Ak * Pk(:,:,k) * Ak' + Q;
hk = [2*(x_(2)*x_(4)-x_(1)*x_(3)) 2*(x_(1)*x_(2)+x_(3)*x_(4)) x_(1)^2+x_(4)^2-x_(2)^2-x_(3)^2];
Hk = 2*[-x_(3) x_(4) -x_(1) x_(2) 0 0 0;
x_(2) x_(1) x_(4) x_(3) 0 0 0;
x_(1) -x_(2) -x_(3) x_(4) 0 0 0];
Kk = Pk_ * Hk' * ((Hk * Pk_ * Hk' + R)^(-1));
x(k+1,:) = (x_' + Kk * (z(k,:) - hk)')';
x(k+1,1:4) = x(k+1,1:4)/norm(x(k+1,1:4),2);
Pk(:,:,k+1) = (eye(7) - Kk*Hk) * Pk_;
q=[x(k,1),x(k,2),x(k,3),x(k,4)];
[pitchEKF(k,1),rollEKF(k,1),yawEKF(k,1)] = quattoeuler(q);
mag_data = mag(k,:);
yawEKF(k,1) = Get_Yaw(pitchEKF(k,1),rollEKF(k,1),mag_data);
%加入磁力计的融合算法
A_2 = [1,-T;0,1];
H_2 = [1,0];
yaw_mag(k+1,1) = CalEndAngle_Zcoord(yaw_mag(k,1),(gyro(k,3)-z_bias(k,1))*T );
z_bias(k+1,1) = z_bias(k,1);
x_mag(k+1,:) = [yaw_mag(k+1,1),z_bias(k+1,1)];
P_22 = A_2 * P_mag(:,:,k) * A_2' + Q_mag;
z_2(k+1,1) = Get_Yaw(pitchEKF(k,1),rollEKF(k,1),mag_data);
K_2 = P_22 * H_2' * ((H_2 * P_22 * H_2' + R_mag)^(-1));
x_mag(k+1,:) = x_mag(k+1,:)+ (K_2 * (z_2(k+1,:) - x_mag(k+1,1)))';
P_mag(:,:,k+1) = (eye(2) - K_2*H_2) * P_22;
yaw_mag(k+1,1) = x_mag(k+1,1);
z_bias(k+1,1) = x_mag(k+1,2);
if(abs(z_bias(k+1,1))>0.1)
z_bias(k+1,1)=0.1 ;
end
end
%% 画图
end_num = min([size(Time,1),size(pitchEKF,1),size(yaw_mag,1)]);
gap_plot = 1;
figure;
pitch_ekf_plot = plot(Time(1:gap_plot:end_num),pitchEKF(1:gap_plot:end_num),'b-','LineWidth',2);
legend([pitch_ekf_plot],{'pitch-ekf'},'FontSize',10);
xlabel('t / s','FontSize',20)
ylabel('pitch','FontSize',20)
title('pitch','FontSize',20);
figure;
roll_ekf_plot = plot(Time(1:gap_plot:end_num),rollEKF(1:gap_plot:end_num),'b-','LineWidth',2);
legend([roll_ekf_plot],{'roll-ekf'},'FontSize',10);
xlabel('t / s','FontSize',20)
ylabel('roll','FontSize',20)
title('roll','FontSize',20);
figure;
yaw_gyro_plot = plot(Time(1:gap_plot:end_num),yaw_gyro(1:gap_plot:end_num),'r-','LineWidth',2);
hold on;
yaw_mag_plot = plot(Time(1:gap_plot:end_num),yawEKF(1:gap_plot:end_num),'g-','LineWidth',2);
hold on;
new_yaw_plot = plot(Time(1:gap_plot:end_num),yaw_mag(1:gap_plot:end_num),'b-','LineWidth',2);
legend([yaw_gyro_plot,yaw_mag_plot,new_yaw_plot],{'yaw-gyro-plot','yaw-mag-plot','new-yaw-plot'},'FontSize',10);
xlabel('t / s','FontSize',20)
ylabel('yaw','FontSize',20)
title('yaw','FontSize',20);
前言
EKF（扩展卡尔曼滤波器）是进行IMU姿态解算的主流方法，我主要参考了论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》和网路上的一些资源，对这部分内容相关公式进行推导和后面代码的走读，同时也有一些不太明白的地方，希望能和大家一起交流学习。
一些说明
按照习惯，推导过程中使用的导航坐标系为东北天坐标系（ENU），IMU的载体坐标系为载体坐标系为：右、前，上（
, 
, 
）,欧拉角的旋转顺序为：
, 
, 
(偏航、俯仰，滚转)，另外使用的IMU型号为ADI的ADIS16470，磁力计型号为RM3100。
推导过程
0.主要框架
主要的数据融合框架如上图所示。论文中用了所谓的两层卡尔曼滤波，首先用了陀螺仪预估出姿态角，后面的测量模型中用加速度计的测量模型和磁力的测量模型做后验估计，完成整个卡尔曼滤波的过程，其中加速度主要参与俯仰角和滚转角的后验，磁力计主要参与偏航角的后验。EKF部分的内容可以参考《概率机器人》，书中有一个比较清晰的讲解。
1.计算初始欧拉角
首先计算初始欧拉角，一般由加速度计和磁力计计算出初始欧拉角，先给出其公式：
上面公式中
, 
，
分别为：初始俯仰角，初始滚转角，初始偏航角。其详细的推导公式可以参考我的另外一篇博客，以下是链接：由磁场数据和加速度数据计算初始姿态角
另外一个需要进行的步骤是把欧拉角转换为四元数，得到初始四元数，因为后续的卡尔曼滤波器中是用四元数来表示旋转，关于欧拉角到四元数的详细推导可以参考我的另外一篇博客：欧拉角和四元数之间转换公式推导，现在给出其公式：
上式中的
，
，
分别表示
，
，
，即IMU的载体坐标系的
，
，
轴的旋转角。
2.初始化噪声
初始化
过程噪声协方差矩阵和
观测噪声协方差矩阵，假设各个轴之间的噪声之间是相关独立的，所以写成下面形式：
上式中
为陀螺仪噪声，
为陀螺仪偏置噪声，
为加速度计的测量噪声，为磁力计的测量噪声。
3.先验过程
状态转移方程主要是用了四元数微分方程，其详细推导可以参考我的另外一篇博客:四元数微分方程的推导
其公式如下：
以上公式
是从导航坐标系到载体坐标系的四元数,四元数微分方程在连续域中的表示，因为在实际使用卡尔曼滤波器的时候，最常用的还是离散域，所以要转换到离散域，推导如下，这部分可以参考上面提到的那篇关于EKF的论文。
状态转移方程为：
四元数微分方程可以表示为：
即为状态转移矩阵的一部分，因为还要考虑到陀螺仪偏置，在我们的代码中状态变量维度为7:四元数+偏置，这部分的处理在后面的代码走读中会讲述。
最终的状态转移矩阵为：
有一点需要注意的是别忘了对状态转移方程计算出来的四元数进行归一化处理。
设
, 
,则状态转移方程的雅克比矩阵求解如下：
最终的求解结果为：
的求解依然是借助了matlab,用matlab求偏导很方便，代码如下所示：
%对状态转移矩阵求雅克比矩阵
syms q0
syms q1
syms q2
syms q3
syms w_x
syms w_y
syms w_z
syms wb_x
syms wb_y
syms wb_z
syms T
q_k = [q0;q1;q2;q3;wb_x;wb_y;wb_z];
gyro_x_bias = w_x-wb_x;
gyro_y_bias = w_y-wb_y;
gyro_z_bias = w_z-wb_z;
A_11=[1,-(T/2)*gyro_x_bias,-(T/2)*gyro_y_bias,-(T/2)*gyro_z_bias;
(T/2)*gyro_x_bias,1,(T/2)*gyro_z_bias,-(T/2)*gyro_y_bias;
(T/2)*gyro_y_bias,-(T/2)*gyro_z_bias,1,(T/2)*gyro_x_bias;
(T/2)*gyro_z_bias,(T/2)*gyro_y_bias,-(T/2)*gyro_x_bias,1];
A_12=[0,0,0;
0,0,0;
0,0,0;
0,0,0]; %4*3
A_21=[0,0,0,0;
0,0,0,0;
0,0,0,0]; %3*4
A_22=[1,0,0;
0,1,0;
0,0,1]; %3*3
A=[A_11,A_12;A_21,A_22];
q=A*q_k;
q_dif=[diff(q,q0),diff(q,q1),diff(q,q2),diff(q,q3),diff(q,wb_x),diff(q,wb_y),diff(q,wb_z)];
% 运行结果
% [ 1, -(T*(w_x - wb_x))/2, -(T*(w_y - wb_y))/2, -(T*(w_z - wb_z))/2, (T*q1)/2, (T*q2)/2, (T*q3)/2]
% [ (T*(w_x - wb_x))/2, 1, (T*(w_z - wb_z))/2, -(T*(w_y - wb_y))/2, -(T*q0)/2, (T*q3)/2, -(T*q2)/2]
% [ (T*(w_y - wb_y))/2, -(T*(w_z - wb_z))/2, 1, (T*(w_x - wb_x))/2, -(T*q3)/2, -(T*q0)/2, (T*q1)/2]
% [ (T*(w_z - wb_z))/2, (T*(w_y - wb_y))/2, -(T*(w_x - wb_x))/2, 1, (T*q2)/2, -(T*q1)/2, -(T*q0)/2]
% [ 0, 0, 0, 0, 1, 0, 0]
% [ 0, 0, 0, 0, 0, 1, 0]
% [ 0, 0, 0, 0, 0, 0, 1]
先验过程的第二步为协方差计算公式，公式如下所示：
4.计算测量方程
首先求加速度计的测量方程，推导过程如下，注意这里加速度计的单位为g:
为导航系到载体坐标系的转换矩阵，需要注意的是此矩阵和旋转顺序没有关系。加速度计数据在载体坐标下的投影如下：
由以上公式可以知道，因为加速度计计算出来的是非重力加速度，所以以上测量方程只有在加速度计运动不剧烈的情况下才满足，所以一种思路就是根据运动的剧烈程度自适应调节加速度测量方程的测量噪声。
设加速度计的三轴数据分别为
， 
， 
，所以加速度计的测量方程如下所示，主要注意加速度数据的归一化。
是前面提到的加速度计测量方程。
接下来对以上测量方程求雅克比矩阵，同样借助matlab辅助推导，matlab代码如下：
%对加速度计测量方程求雅克比矩阵
syms q0
syms q1
syms q2
syms q3
q = [q0;q1;q2;q3];
matrix_n2b = quatToRotMat(q);
matrix_b2n = matrix_n2b';
acce_n = [0;0;1]; %加速度计数据在导航坐标系下的投影，注意加速度计单位为g
acce_b = matrix_b2n*acce_n; %加速度计数据在载体坐标系下的投影
acce_dif=[diff(acce_b,q0),diff(acce_b,q1),diff(acce_b,q2),diff(acce_b,q3)] %求雅克比矩阵
% 结果
% acce_dif =
% [ -2*(q2), 2*(q3), -2*(q0), 2*(q1)]
% [ 2*(q1), 2*(q0), 2*(q3), 2*(q2)]
% [ 2*(q0), -2*(q1), -2*(q2), 2*(q3)]
% 四元数转旋转矩阵函数
function R = quatToRotMat(q)
R = [ q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4) 2*(q(2)*q(3)-q(1)*q(4)) 2 *(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)) q(1)*q(1)+q(3)*q(3)-q(2)*q(2)-q(4)*q(4) 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)*q(1)+q(4)*q(4)-q(2)*q(2)-q(3)*q(3)];%cnb
end
注意以上代码求解结果中的
意思是取实部，去掉即，最终求解结果如下：
其实还需要对陀螺仪偏置求偏导，只是为零，所以整体的雅克比矩阵如下所示：
接下来求磁力计的测量方程。先把磁场数据投影到导航坐标系下，这部分会用到坐标变换的四元数表示法，可以参考《惯性导航 第二版》251页的讲解。设载体坐标系下的磁场数据为: 
， 
， 
，导航坐标系下的磁场数据为：
， ， 
，这里同样需要对磁场数据归一化。
在磁场测量模型中，我们认为导航坐标坐标系下的磁场只分布在北方向和天方向，东方向没有磁场数据，所以对数据作以下处理：
再把磁场数据转换到载体坐标系上，所以：
所以最终的测量方程如下式，ωm
为前面提到的磁力计测量方程。
接下来求磁力计测量方程的雅克比矩阵。
辅助推导的matlab代码如下：
% 测试基于EKF的磁力计观测方程
addpath('utils');
syms m_x % x
syms m_y % y
syms m_z % z
syms q0 % qw
syms q1 % qx
syms q2 % qy
syms q3 % qz
q = [q0,q1,q2,q3];
m_n = [0;m_y;m_z]; %东北天坐标系，假设磁场只分布在北方向和天方向
R_n2b = quatToRotMat_test(q);
m_b = R_n2b'*m_n;
m_dif=[diff(m_b,q0),diff(m_b,q1),diff(m_b,q2),diff(m_b,q3)]
% m_b =
%
% m_y*(2*(q0)*(q3) + 2*(q1)*(q2)) - m_z*(2*(q0)*(q2) - 2*(q1)*(q3))
% m_z*(2*(q0)*(q1) + 2*(q2)*(q3)) + m_y*((q0)^2 - (q1)^2 + (q2)^2 - (q3)^2)
% m_z*((q0)^2 - (q1)^2 - (q2)^2 + (q3)^2) - m_y*(2*(q0)*(q1) - 2*(q2)*(q3))
% m_dif =
% [ 2*m_y*(q3) - 2*m_z*(q2), 2*m_y*(q2) + 2*m_z*(q3), 2*m_y*(q1) - 2*m_z*(q0), 2*m_y*(q0) + 2*m_z*(q1)]
% [ 2*m_y*(q0) + 2*m_z*(q1), 2*m_z*(q0) - 2*m_y*(q1), 2*m_y*(q2) + 2*m_z*(q3), 2*m_z*(q2) - 2*m_y*(q3)]
% [ 2*m_z*(q0) - 2*m_y*(q1), - 2*m_y*(q0) - 2*m_z*(q1), 2*m_y*(q3) - 2*m_z*(q2), 2*m_y*(q2) + 2*m_z*(q3)]
完整的磁场测量方程的雅克比矩阵如下：
加速度计测量方程和磁力计总的雅克比矩阵如下：
总的测量方程如下：
5. 计算卡尔曼增益
6.更新后验状态
注意
为加速度数据和磁场数据经过归一化处理后的数据。
7.更新后验协方差矩阵
8.四元数转欧拉角
在上面的扩展卡尔曼滤波中姿态数据是用四元数表示的，下一步还要转换为欧拉角，方便显示。设偏航、俯仰，滚转的角度分别为
，
, ，以下为转换公式。
这部分的推导可以参考我的博客欧拉角和四元数之间转换公式推导
姿态解算优化设计
1.组合导航系统AHRS模型优化
上述姿态解算存在一个问题，就是由于磁力计容易受到干扰，当磁力计受到干扰时，系统的三个姿态角都要牵连挂掉，这是非常不合理的。正常情况下，加速计和陀螺仪融合即可得到稳定的滚转角和俯仰角，大可不必用磁力计再去更新这两个角度，因为上述的加速度计测量模型对系统的偏航角计算并没有做任何贡献，对偏航角来说并不具有客观性，所以偏航角只有靠磁力计才能得到观测。下面介绍利用磁力计只更新偏航角的方法，具体步骤如下：
1.利用先验欧拉角和磁力计数据计算出偏航角，公式见前面介绍的姿态角初始化部分，只不过这时候的俯仰角和滚转角是IMU融合出来的数据，不只是加速度数据。
2. 利用陀螺仪数据和磁力计数据融合得到偏航角，相当于又加了一层卡尔曼滤波，只不过因为方程是线性的，所以不需要用EKF来解算。
运动方程如下：
公式中
为偏航角，
为陀螺仪
轴偏置，
为陀螺仪角速度，
为更新周期。
测量方程如下：
上面的测量对象即为第一步得到的测量偏航角，和未优化前的磁场融合算法相比，这里相当于只用到了利用磁力计的偏航角观测，而未优化前用到了3个量的观测，测试结果表明，在干扰较小的情况下，磁力计的偏航角观测还是相对比较准确的，偏差在2度以内。
实际效果验证与测试
为了比较
模型优化中的第一步，即利用先验欧拉角和磁力计数据计算出偏航角和再融合陀螺仪数据之后的偏航角之间的差异，可以看下面两张图，蓝色线为利用先验欧拉角和磁力计数据计算出偏航角，绿色线为再融合陀螺仪数据之后的偏航角，很明显可以看出，无论是动态精度还是静态精度上，有有了比较明显的精度上升，未优化之前大约是
左右，优化之后是
左右。
下图场景是偏航角来回转动大约
，磁场未受导干扰下的偏航角数据，可以看到相比红线的单纯陀螺仪数据，没有漂移现象，相比先验欧拉角和磁力计数据计算出偏航角数据，其在精度上有所上升。
参考
1. A Double Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9D IMU
2.《概率机器人》
3. https://blog.csdn.net/u012871872/article/details/78269420
4. https://blog.csdn.net/qq_22707525/article/details/77996333