使用EKF算法处理IMU数据

研一已经开学了，在大四学习四旋翼的时候，深刻意识到自己的数学功底有多差，所以接下来不再接着搞四旋翼了，要专心学习了，不过南京的小伙伴想约飞可以带上我哦，不飞手痒。因为不忍心把自己的博客断了，所以想直接把最重要的部分分享出来，希望对搞四旋翼研究的人有所帮助。

在实现四旋翼自稳功能的时候，最重要的就是两点，一个是PID，另一个就是IMU数据处理。我本身不是自控专业，自己对PID的理解也是从胡寿松的《自动控制原理》这本书上看来的，有需要的可以参考。然后这里我想讲一下IMU数据处理。

关于IMU数据处理网络上已经有很多开源的代码，但是学习四旋翼绝对不仅仅是为了让飞机能飞起来，而是要搞懂其中的数学原理，为进一步的开发做准备。我这里想讲的EKF算法，大家可以参考清华大学《信号检测与估计》这本书。然后具体EKF算法是怎样应用到IMU数据处理上来的，可以参考这篇论文《A Double-Stage Kalman Filter for Orientation Tracking with an Integrated Processor in 9-D IMU》，这篇论文我简单翻译了一下，但是我想愿意读这篇博客的人应该都愿意花时间去啃这篇论文吧。在看这篇论文之前，还要看完北航可靠飞行控制组（http://rfly.buaa.edu.cn/resources.html）前5章的PPT。

下面开始步入正题，贴代码：

clear all;close all;
load('pix_log.mat');
ATT_T=0.1;  %每100ms执行一次ATT数据的log
IMU_T=0.04; %每40ms执行一次IMU数据的log
hTime = 0.5*IMU_T;
imumin = 1;
imumax = 15636;
ax = IMU(:,6);
ay = IMU(:,7);
az = IMU(:,8);
gx = IMU(:,3);
gy = IMU(:,4);
gz = IMU(:,5);
t_us=IMU(:,2);

angle = zeros(1,imumax);
angle_g = angle;angle_g(1)=0;
Pk1 = zeros(1,imumax);
Pk2 = zeros(1,imumax);
roll=zeros(1,imumax);
error=zeros(1,imumax);
for n =1:imumax-1
    roll(n)=ATT(ceil(n/2.5),4);
end

q=[0;0;0;1];
P=5*[0.125,0.003,0.003,0.003;
    0.003,0.125,0.003,0.003;
    0.003,0.003,0.125,0.003;
    0.003,0.003,0.003,0.125];
Q=4e-5*[1,0,0,0;
    0,1,0,0;
    0,0,1,0;
    0,0,0,1];
R=0.4*[2,0,0;
    0,2,0;
    0,0,2];
V=[1,0,0;
    0,1,0;
    0,0,1];
I=[1,0,0,0;
    0,1,0,0;
    0,0,1,0;
    0,0,0,1];
for n=2:imumax
   Pk1(n-1)=mean(abs(P(:)));
   %Pk(n-1)=P(1,1);
    hTime = 0.5*(t_us(n)-t_us(n-1))*1e-6;
A=[   1,         -gx(n)*hTime,   -gy(n)*hTime,  -gz(n)*hTime;
    gx(n)*hTime,      1,         gz(n)*hTime,   -gy(n)*hTime;
    gy(n)*hTime, -gz(n)*hTime,         1,         gx(n)*hTime;
    gz(n)*hTime, gy(n)*hTime,    -gx(n)*hTime,        1     ]; 
q=A * q;
P=A*P*A'+Q;
Pk2(n-1)=mean(abs(P(:)));
H=[2*q(3),-2*q(4),2*q(1),-2*q(2);
    -2*q(2),-2*q(1),-2*q(4),-2*q(3);
    -2*q(1),2*q(2),2*q(3),-2*q(4)];
K=P*H'*(H*P*H'+V*R*V')^-1;

norm = sqrt(ax(n)^2+ay(n)^2+az(n)^2);
ax(n) = ax(n)/norm;
ay(n) = ay(n)/norm;
az(n) = az(n)/norm;
z=[ax(n);
    ay(n);
    az(n)];
h=[-2*q(2)*q(4)+2*q(1)*q(3);
    -2*q(1)*q(2)-2*q(3)*q(4);
    -q(1)^2+q(2)^2+q(3)^2-q(4)^2];
delt_q = K*(z-h);
temp=[1,0,0,0;
    0,1,0,0;
    0,0,1,0;
    0,0,0,0];
delt_q = temp*delt_q;
q = q+delt_q;
P=(I-K*H)*P;


norm = sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);
q(1) = q(1)/norm;
q(2) = q(2)/norm;
q(3) = q(3)/norm;
q(4) = q(4)/norm;
angle(n) = atan2(2*(q(1)*q(2)+q(3)*q(4)),1-2*(q(2)^2+q(3)^2));
angle_g(n) = angle_g(n-1)+gx(n)*0.04;
error (n)= roll(n)-57.3*angle(n);
end
figure(1)
plot(0.04*(1:imumax),angle*57.3,'-b');hold on;
plot(0.04*(1:imumax),roll,'--g');hold on;
plot(0.04*(1:imumax-1),error(1:imumax-1),'-.r');
legend('EKF滤波结果','真实姿态角','误差');
xlabel('时间（s）');ylabel('角度（°）');
title('EKF算法滤波结果');


figure(2)
plot(Pk1);hold on;
plot(Pk2);hold off;title('后验均方误差');xlabel('递推步数');ylabel('角度平方');

这些代码是用来分析PX4日志的，我使用EKF算法来处理PX4日志数据，然后将解算得到的姿态角与PX4解算出来的姿态角进行对比，并认为PX4解算出来的姿态角就是飞行过程中真实的姿态角。结果如下：

在运行这个代码之前，请确保MATLAB路径里有一份PX4的日志。如果没有也可以在这里获取。https://download.csdn.net/download/qq_22707525/10643956

扫描二维码关注公众号，回复： 3554729 查看本文章

有了这个代码，自己改成C添加到自己的工程里去吧。祝爽飞。

使用EKF算法处理IMU数据

猜你喜欢