【数据融合】基于matlab拓展卡尔曼滤波IMU和GPS数据融合【含Matlab源码 1600期】

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【数据融合】基于matlab拓展卡尔曼滤波IMU和GPS数据融合【含Matlab源码 1600期】

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

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

二、部分源代码

%for testing
clc
clear
close all

pauseLen = 0;

%%Initializations
%TODO: load data here
data = load('lib/IMU_GPS_GT_data.mat');
IMUData = data.imu;
GPSData = data.gpsAGL;
gt = data.gt;

addpath([cd, filesep, 'lib'])
initialStateMean = eye(5);
initialStateCov = eye(9);
deltaT = 1 / 30; %hope this doesn't cause floating point problems

numSteps = 500000;%TODO largest timestamp in GPS file, divided by deltaT, cast to int

results = zeros(7, numSteps);
% time x y z Rx Ry Rz

% sys = system_initialization(deltaT);
Q = blkdiag(eye(3)*(0.35)^2, eye(3)*(0.015)^2, zeros(3));
%IMU noise characteristics
%Using default values from pixhawk px4 controller
%https://dev.px4.io/v1.9.0/en/advanced/parameter_reference.html
%accel: first three values, (m/s^2)^2
%gyro: next three values, (rad/s)^2 

filter = filter_initialization(initialStateMean, initialStateCov, Q);

%IMU noise? do in filter initialization

IMUIdx = 1;
GPSIdx = 1;
nextIMU = IMUData(IMUIdx, :); %first IMU measurement
nextGPS = GPSData(GPSIdx, :); %first GPS measurement

%plot ground truth, raw GPS data

% plot ground truth positions
plot3(gt(:,2), gt(:,3), gt(:,4), '.g')
grid on
hold on
% plot gps positions
% plot3(GPSData(:,2), GPSData(:,3), GPSData(:,4), '.b')
axis equal
axis vis3d

counter = 0;
MAXIGPS = 2708;
MAXIIMU = 27050;
isStart = false;

for t = 1:numSteps
    currT = t * deltaT;
    if(currT >= nextIMU(1)) %if the next IMU measurement has happened
%         disp('prediction')
        filter.prediction(nextIMU(2:7));
        isStart = true;
        IMUIdx = IMUIdx + 1;
        nextIMU = IMUData(IMUIdx, :);
%         plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'or');
    end
    if(currT >= nextGPS(1) & isStart) %if the next GPS measurement has happened
%         disp('correction')
        counter = counter + 1;
        filter.correction(nextGPS(2:4));
        GPSIdx = GPSIdx + 1;
        nextGPS = GPSData(GPSIdx, :);
        plot3(nextGPS(2), nextGPS(3), nextGPS(4), '.r');
%         plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'ok');
%         plotPose(filter.mu(1:3, 1:3), filter.mu(1:3, 5), filter.mu(1:3,4));
        
    end
    results(1, t) = currT;
    results(2:4, t) = filter.mu(1:3, 5); %just position so far
%     plot3(results(2, t), results(3, t), results(4, t), 'or');
%     disp(filter.mu(1:3, 1:3));
    if pauseLen == inf
        pause;
    elseif pauseLen > 0
        pause(pauseLen);
    end
    if IMUIdx >= MAXIIMU || GPSIdx >= MAXIGPS
        break
    end
end
plot3(results(2,:), results(3,:), results(4,:), '.b');
% xlim([-10 10]);
% ylim([-10 10]);
xlabel('x, m');
ylabel('y, m');
zlabel('z, m');

%% Evaluation
gps_score = evaluation(gt, GPSData)

results_eval = results.';
score = 0;
estimation_idx = 1;
count = 0;    
for i = 2:length(gt)
    score = score + norm(gt(i, 2:4) - results_eval(30 * (i-1), 2:4)) ^ 2;
    count = count + 1;
end
count
score = sqrt(score / count)



%% Function



  

 

  
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
 

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 沈再阳.精通MATLAB信号处理[M].清华大学出版社，2015.
[2]高宝建,彭进业,王琳,潘建寿.信号与系统——使用MATLAB分析与实现[M].清华大学出版社，2020.
[3]王文光,魏少明,任欣.信号处理与系统分析的MATLAB实现[M].电子工业出版社，2018.

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

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