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

一、获取代码方式

获取代码方式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

  

 

  
 
 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
  
106
  
107
  
108
  
109
  
110
  
111
  
112
  
113
  
114
  
115
  
116
  
117
  
118
  
119
  
120
  
121
  
122
  
123
  
124
  
125
  
126
  
127
  
128
  
129
  
130
  
131
  
132
  
133
  
134
  
135
  
136
  
137
  
138
  
139
  
140
  
141
  
142
  
143
  
144
  
145
  
146
  
147
  
148
  
149
  
150
  
151
  
152
  
153
  
154
  
155
  
156
  
157
  
158
 

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

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

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/122139396