EKF扩展卡尔曼滤波九轴imu mpu9250数据融合代码学习【附matlab源码】

最新推荐文章于 2025-04-25 17:16:24 发布

水森usagi

最新推荐文章于 2025-04-25 17:16:24 发布

阅读量2.1k

点赞数 43

文章标签：学习 matlab 人工智能机器人

本文链接：https://blog.csdn.net/weixin_44704641/article/details/144697747

版权

完整源码链接在最后~

示例数据是MPU9250的九轴数据，包括加速度计，陀螺仪，磁力计，数据最后一行是时间。

首先是单位转换值和各类初值设定，并对数据进行了处理：

1.单位的转换，把acc，gyro和mag都转换成需要的单位，注意这里acc使用的最终数据需要是以g为单位的。

2.对过程噪声协方差Q为1 ，量测噪声协方差R为100。

3.对陀螺仪的零偏校准就比较简单粗暴了，直接取了前三十个值平均计算。

4.磁力计校准采用的是常规的最小二乘法，最后做归一化处理。

%% INITIALIZING
g = 9.8;
unit_transform_acc = 16384;
unit_transform_gyro = (pi/(180*131));
unit_transform_mag=0.6;
Gyro_Compen_k = 30;
Mag_Compen_k = 1000;
ref_mag = 30;
DATA_SI = (size(DATA));
N_Q = 1;
N_R = 100;
N_P = 1;

%% LSB to SI Unit
for k = 1:Nsamples
    %Acc LSB -> N/m^2
    DATA_SI(1,k)= (g/unit_transform_acc)*DATA(1,k);
    DATA_SI(2,k)= (g/unit_transform_acc)*DATA(2,k);
    DATA_SI(3,k)= (g/unit_transform_acc)*DATA(3,k);    
    %Gyro LSB -> deg/s -> rad/s
    DATA_SI(4,k)= (unit_transform_gyro)*DATA(4,k);
    DATA_SI(5,k)= (unit_transform_gyro)*DATA(5,k);
    DATA_SI(6,k)= (unit_transform_gyro)*DATA(6,k);   
    %Mag LSB -> uT
    DATA_SI(7,k)= unit_transform_mag*DATA(7,k);
    DATA_SI(8,k)= unit_transform_mag*DATA(8,k);
    DATA_SI(9,k)= unit_transform_mag*DATA(9,k);
    %Time ms -> s
    DATA_SI(10,k)= DATA(10,k)/1000;
end

%% Gyroscope Compensation
for k = 1:Gyro_Compen_k
    Bias_GyroX=mean(DATA_SI(4,k));
    Bias_GyroY=mean(DATA_SI(5,k));
    Bias_GyroZ=mean(DATA_SI(6,k));
end

Bias_Gyro = [Bias_GyroX Bias_GyroY Bias_GyroZ]

for k = 1:Nsamples
    DATA_SI(4,k)=DATA_SI(4,k)-Bias_GyroX;
    DATA_SI(5,k)=DATA_SI(5,k)-Bias_GyroY;
    DATA_SI(6,k)=DATA_SI(6,k)-Bias_GyroZ;
end

%% Magnetometer Compensation

for k = 1:Mag_Compen_k
    Y(k,:) = [DATA_SI(7,k)^2+DATA_SI(8,k)^2+DATA_SI(9,k)^2];
    X(k,:) = [DATA_SI(7,k) DATA_SI(8,k) DATA_SI(9,k) 1];
end

N_X = [size(X)];
Xsamples = N_X(2);
Bias_Mag = 0.5*((X'*X)\eye(Xsamples))*X'*Y

for k = 1:Nsamples
    DATA_SI(7,k) = DATA_SI(7,k) - Bias_Mag(1);
    DATA_SI(8,k) = DATA_SI(8,k) - Bias_Mag(2);
    DATA_SI(9,k) = DATA_SI(9,k) - Bias_Mag(3);
end

%% Set Reference Magnetic vector (NORMALIZATION)
M=sqrt(DATA_SI(7,ref_mag)^2+DATA_SI(8,ref_mag)^2+DATA_SI(9,ref_mag)^2);
B=[DATA_SI(7,ref_mag)/M DATA_SI(8,ref_mag)/M DATA_SI(9,ref_mag)/M];

然后传到EKF函数中处理，传入参数为，Gyro三轴数据p、q、r，归一化的磁场向量B，Mag三轴数据mx、my、mz，三轴Acc数据ax、ay、az，时间戳dt，三个协方差参数，Q、R、P。

1.利用isempty做初始化。

2.使用Gyro的数据作为先验估计值，Acc和Mag作为测量值进行校正。

3.F雅各比矩阵用于预测步骤，描述四元数随时间的变化率。F矩阵的计算基于四元数的微分方程。

这里其实我自己不是很明白为什么，查询资料：

四元数的微分方程是描述四元数随时间变化的数学表达式。在三维空间中，四元数常用于表示物体的姿态，因为它可以避免万向节死锁问题。四元数的微分方程与角速度（p, q, r）有关，这些角速度描述了物体绕三个轴的旋转速度。

四元数的微分方程可以表示为：

\[
\frac{d\mathbf{q}}{dt} = \frac{1}{2} \mathbf{q} \otimes \mathbf{\omega}
\]

其中，\(\mathbf{q} = [q_0, q_1, q_2, q_3]^T\) 是四元数，\(\mathbf{\omega} = [0, p, q, r]^T\) 是角速度四元数，\(\otimes\) 表示四元数乘法。

将这个微分方程离散化，可以得到四元数的预测方程：

\[
\mathbf{q}_{k+1} = \mathbf{q}_k + \frac{1}{2} \mathbf{q}_k \otimes \mathbf{\omega}_k \Delta t
\]

其中，\(\Delta t\) 是时间步长。

为了将这个方程写成矩阵形式，我们需要将四元数乘法展开。四元数乘法的展开形式为：

\[
\mathbf{q} \otimes \mathbf{\omega} = \begin{bmatrix}
q_0 & -q_1 & -q_2 & -q_3 \\
q_1 & q_0 & q_3 & -q_2 \\
q_2 & -q_3 & q_0 & q_1 \\
q_3 & q_2 & -q_1 & q_0
\end{bmatrix}
\begin{bmatrix}
0 \\
p \\
q \\
r
\end{bmatrix}
\]

将这个乘法结果代入预测方程，可以得到：

\[
\mathbf{q}_{k+1} = \mathbf{q}_k + \frac{1}{2} \begin{bmatrix}
-q_1 p - q_2 q - q_3 r \\
q_0 p + q_2 r - q_3 q \\
q_0 q - q_1 r + q_3 p \\
q_0 r + q_1 q - q_2 p
\end{bmatrix} \Delta t
\]

将这个方程写成矩阵形式，可以得到F雅各比矩阵：

\[
F = \begin{bmatrix}
1 & -\frac{1}{2} p \Delta t & -\frac{1}{2} q \Delta t & -\frac{1}{2} r \Delta t \\
\frac{1}{2} p \Delta t & 1 & \frac{1}{2} r \Delta t & -\frac{1}{2} q \Delta t \\
\frac{1}{2} q \Delta t & -\frac{1}{2} r \Delta t & 1 & \frac{1}{2} p \Delta t \\
\frac{1}{2} r \Delta t & \frac{1}{2} q \Delta t & -\frac{1}{2} p \Delta t & 1
\end{bmatrix}
\]

因此，F雅各比矩阵的元素反映了四元数的微分方程，它描述了四元数随时间的变化率。

这里其实是一个离散化处理。

4.H雅各比矩阵用于校正步骤，描述四元数与测量值（加速度和磁场）之间的关系。H矩阵的计算基于四元数到加速度和磁场的转换方程。

function [q0, q1, q2, q3] = EKF(p, q, r, B, mx, my, mz, ax, ay, az, dt, N_Q, N_R, N_P)
%
%
global Q R
global x P
global firstRun

%Initialize
if isempty(firstRun)
    
  Q = N_Q*eye(4);
     
  R = N_R*eye(6);

  x = [1 0 0 0]';  
  P = N_P*eye(4);
  
  firstRun = 1;  
end

%1.Predict xp and Pp
%Calculate F
F = Fjacob(p, q, r, dt);

xp = F*x;
Pp = F*P*F' + Q;

%2.Correct xp and P
%Calculate H
H = Hjacob(x(1),x(2),x(3),x(4), B);

S= (H*Pp*H' + R);
K = Pp*H'*(S\eye(6));

 z = [ax;
    ay;
    az;
    mx;
    my;
    mz];

x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;

x_sc=(x(1)^2+x(2)^2+x(3)^2+x(4)^2)^0.5;

q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);

%------------------------------
function F = Fjacob(p, q, r, dt)
%
%
F = zeros(4);

F(1,1)=1;
F(1,2)= -p*dt/2;
F(1,3)= -q*dt/2;
F(1,4)= -r*dt/2;

F(2,1)= p*dt/2;
F(2,2)=1;
F(2,3)= r*dt/2;
F(2,4)=-q*dt/2;

F(3,1)=q*dt/2;
F(3,2)=-r*dt/2;
F(3,3)=1;
F(3,4)=p*dt/2;

F(4,1)=r*dt/2;
F(4,2)=p*dt/2;
F(4,3)=-q*dt/2;
F(4,4)=1;

%------------------------------
function H = Hjacob(qt0, qt1, qt2, qt3, B)
%
%
g=9.8;
H = zeros(6,4);

H(1,1) = -qt2;
H(1,2) = qt3;
H(1,3) = -qt0;
H(1,4) = qt1;

H(2,1) = qt1;
H(2,2) = qt0;
H(2,3) = qt3;
H(2,4) = qt2;

H(3,1) = qt0;
H(3,2) = -qt1;
H(3,3) = -qt2;
H(3,4) = qt3;

H(4,1) = qt0*B(1)+qt3*B(2)-qt2*B(3);
H(4,2) = qt1*B(1)+qt2*B(2)+qt3*B(3);
H(4,3) = -qt2*B(1)+qt1*B(2)-qt0*B(3);
H(4,4) = -qt3*B(1)+qt0*B(2)+qt1*B(3);

H(5,1) = -qt3*B(1)+qt0*B(2)+qt1*B(3);
H(5,2) = qt2*B(1)-qt1*B(2)+qt0*B(3);
H(5,3) = qt1*B(1)+qt2*B(2)+qt3*B(3);
H(5,4) = -qt0*B(1)-qt3*B(2)+qt2*B(3);

H(6,1) = qt2*B(1)-qt1*B(2)+qt0*B(3);
H(6,2) = qt3*B(1)-qt0*B(2)-qt1*B(3);
H(6,3) = qt0*B(1)+qt3*B(2)-qt2*B(3);
H(6,4) = qt1*B(1)+qt2*B(2)+qt3*B(3);

最后画图处理，这里作者是动态呈现的，就不详细展示了，贴一个最终欧拉角的展示图。

总结：其实是一个很经典的流程，作者逻辑也比较清晰简洁，适合初学者参考理解，其中很多细节并不太适合实际应用，但是可以替换尝试。

我最初接触到这套代码实际上是半年前，花了钱在csdn上买的，但后来又兜兜转转在github上下载到了，也是想分享给跟我一样的小白，只求一个关注，如果能点赞收藏支持我一下就更好啦~祝大家学习顺利~

项目源码Github：https://github.com/Chang-yup/IMU_Kalman-filter_MATLAB

如果GitHub实在进不去可以留言给我处理