【运动学】EKF姿态估计【含Matlab源码 1638期】

最新推荐文章于 2024-07-19 14:22:30 发布

Matlab领域

最新推荐文章于 2024-07-19 14:22:30 发布

阅读量798

点赞数 23

分类专栏： Matlab物理应用（初级版）文章标签： matlab

本文链接：https://blog.csdn.net/weixin_63266434/article/details/135895389

版权

Matlab物理应用（初级版）专栏收录该内容

272 篇文章 8 订阅

订阅专栏

在这里插入图片描述

⛄一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【运动学】基于matlab EKF姿态估计【含Matlab源码 1638期】
点击上面蓝色字体，直接付费下载，即可。

获取代码方式2：
付费专栏Matlab物理应用（初级版）

备注：
点击上面蓝色字体付费专栏Matlab物理应用（初级版），扫描上面二维码，付费29.9元订阅海神之光博客付费专栏Matlab物理应用（初级版），凭支付凭证，私信博主，可免费获得1份本博客上传CSDN资源代码（有效期为订阅日起，三天内有效）；
点击CSDN资源下载链接：1份本博客上传CSDN资源代码

⛄二、卡尔曼滤波算法（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®

%状态矩阵
X = zeros(L, 7);
%初始化状态矩阵
[pitch_init, roll_init, yaw_init] = acc2euler_ENU(acc(1, 😃, mag(1, 😃);
%初始化角度
pitchEKF(1,1) = pitch_initrad2deg;
rollEKF(1,1) = roll_initrad2deg;
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§;

%观测矩阵
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