运动学基于matlab EKF姿态估计含Matlab源码 1638期

Posted 2022-01-15 紫极神光

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【运动学】基于matlab EKF姿态估计【含Matlab源码 1638期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：
订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、卡尔曼滤波算法（EKF）简介

EKF滤波方法的实现步骤
组合测姿系统的状态方程形式上较为复杂，采用扩展卡尔曼滤波方法，需先将其线性化，并计算雅可比矩阵。
对于此非线性系统，其状态方程和观测方程可写为

式中：W¯¯¯¯和V¯¯¯分别为过程噪声与测量噪声期望，在本文的高斯分布下为0。令过程噪声协方差阵为Q,观测噪声协方差阵为R,P为状态协方差。

EKF的实现步骤如下。
①计算相应雅可比矩阵。
②时间更新：

三、部分源代码

% 基于EKF的姿态估计算法
% 导航坐标系:东北天坐标系(ENU)
% IMU:16375
% 载体坐标系:右前上(XYZ)
% 旋转方式:Z-X-Y(偏航-俯仰-横滚)右手定则

clc;
clear;
close all;

addpath('utils');
addpath('datafiles');

rad2deg = 180/pi;
deg2rad = pi/180;

%% 读取IMU原始数据
filename = 'session.txt';
sensor_data = load(filename);
gap=1;
data_num=round(size(sensor_data,1));
j=0;
for i=1:gap:data_num-gap
    j=j+1;
    DATA_CNTR(j,1) = i;
    X_GYRO(j,1) = mean(sensor_data(i:i+gap-1,1));
    Y_GYRO(j,1) = mean(sensor_data(i:i+gap-1,2));
    Z_GYRO(j,1) = mean(sensor_data(i:i+gap-1,3));
    X_ACCL(j,1) = mean(sensor_data(i:i+gap-1,4));
    Y_ACCL(j,1) = mean(sensor_data(i:i+gap-1,5));
    Z_ACCL(j,1) = mean(sensor_data(i:i+gap-1,6));
    X_MAG(j,1) = mean(sensor_data(i:i+gap-1,7));
    Y_MAG(j,1) = mean(sensor_data(i:i+gap-1,8));
    Z_MAG(j,1) = mean(sensor_data(i:i+gap-1,9));
end

%% 初始化数据
L = size(DATA_CNTR,1);
Time=zeros(L,1);
Time(1,1)=0.01;

acc = [X_ACCL, Y_ACCL, Z_ACCL];
gyro_bias = [0.01, 0.01, 0.01];
gyro = [X_GYRO - gyro_bias(1), Y_GYRO - gyro_bias(2), Z_GYRO - gyro_bias(3)];
mag = [X_MAG, Y_MAG, Z_MAG];

%角速度过程噪声,四元数噪声
w_process_noise = 0.0001 * ones(1, 4);
%角速度偏移过程噪声
w_bias_process_noise = 0.003 * ones(1,3);
%过程噪声矩阵
Q = diag([w_process_noise, w_bias_process_noise]);
%disp(Q);

%加速度计测量噪声
a_measure_noise = 0.03 * ones(1, 3);
%磁力计测量噪声
m_measure_noise = 1e-5 * ones(1, 3);
%测量噪声矩阵
R = diag(a_measure_noise);
%disp(R)

%状态矩阵
X = zeros(L, 7);
%初始化状态矩阵
[pitch_init, roll_init, yaw_init] = acc2euler_ENU(acc(1, :), mag(1, :));
%初始化角度
pitchEKF(1,1) = pitch_init*rad2deg;
rollEKF(1,1) = roll_init*rad2deg;
yawEKF(1,1) = yaw_init*rad2deg;
disp(pitchEKF);
disp(rollEKF);
disp(yawEKF);

%初始化四元数
[q1, q2, q3, q4] = euler2quat_ENU(pitch_init, roll_init, yaw_init);

q_init = [q1, q2, q3, q4];
[pitch_init1,roll_init1,yaw_init1] = quat2euler_ENU(q_init);
disp(pitch_init1);
disp(roll_init1);
disp(yaw_init1);

X(1,:) = [q1, q2, q3, q4, gyro_bias];

%协方差矩阵
Pk = zeros(7, 7, L);
Pk(:,:,1) = eye(7); 
%disp(P);

%观测矩阵
Z = zeros(L, 3);

%delta_t
T = 0.01;

att = zeros(L, 3);
acc_att = zeros(L, 3);

%% EKF滤波
for k=1:L
    Time(k+1,1)=Time(k)+T; 
    
    [pitch_, roll_, yaw_] = acc2euler_ENU(acc(k,:), mag(k,:));
    acc_att(k,:) = [pitch_*rad2deg, roll_*rad2deg, yaw_*rad2deg];
    
    %加速度计/磁力计数据归一化
    acc(k, :) = acc(k, :) / norm(acc(k, :), 2);
    mag(k, :) = mag(k, :) / norm(mag(k, :), 2);
    Z(k, :) = acc(k, :);
    

    
    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)] = quat2euler_ENU(q);
end

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]许立松,马国梁,闻泉,王雨时.基于无迹卡尔曼滤波的火箭弹姿态估计[J].弹道学报. 2021,33(04)