【雷达通信】基于matlab间接卡尔曼滤波IMU与GPS融合【含Matlab源码 1360期】

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【雷达通信】基于matlab间接卡尔曼滤波IMU与GPS融合【含Matlab源码 1360期】

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

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

二、IMU与GPS融合简介

理论知识参考文献：可量测影像与GPS/IMU融合高精度定位定姿方法研究

三、部分源代码

%%*****************************************************************************************************%%

clear;clc;
% global attiCalculator;
attiCalculator = AttitudeBase();
step = 0.01;
start_time = 0;
end_time = 50;
tspan = [start_time:step:end_time]';
N = length(tspan);
Ar = 10;
r = [Ar*sin(tspan) Ar*cos(tspan) 0.5*tspan.*tspan];         %生成实验轨迹数据
v = [Ar*cos(tspan) -Ar*sin(tspan) tspan];
acc_inertial = [-Ar*sin(tspan) -Ar*cos(tspan) ones(N,1)];


atti = [0.1*sin(tspan) 0.1*sin(tspan) 0.1*sin(tspan)];
Datti = [0.1*cos(tspan) 0.1*cos(tspan) 0.1*cos(tspan)];
g = [0 0 -9.8]';
gyro_pure = zeros(N,3);
acc_pure = zeros(N,3);
gps_pure= r;



a = wgn(N,1,1)/5;
b = zeros(N,1);
b(1) = a(1)*step;
%生成无噪声的imu数据
for iter = 1:N
    A = attiCalculator.Datti2w(atti(iter,:));
    gyro_pure(iter,:) = Datti(iter,:)*A';
    cnb = attiCalculator.a2cnb(atti(iter,:));
    acc_pure(iter,:) = cnb*(acc_inertial(iter,:)' - g);
%     acc_pure(iter,:) = cnb*(acc_inertial(iter,:)');
end
% state0 = zeros(10,1);
% state0(7) = 1;

%加速度计和陀螺仪加噪声
acc_noise = acc_pure + randn(N,3)/10;           %生成惯导及GPS测量值，同时加入噪声
gyro_noise = gyro_pure + randn(N,3)/10;
gps_noise=[zeros(N,1) gps_pure+randn(N,3)/10];
for i=1:10:N
    gps_noise(i,1)=1;
end
% acc_noise = acc_pure ;
% gyro_noise = gyro_pure;
state0 = zeros(16,1);
state0(2) = 10;
state0(4) = 10;
state0(7) = 1;

errorstate0=zeros(15,1);%误差初始状态赋值
Cov=[0.01*ones(3,1);zeros(3,1);0.01*ones(3,1);zeros(3,1)];
Qc0=diag(Cov);%初始噪声方差
Rc0=diag([0.01,0.01,0.01]);%GPS测量噪声误差方差
% Qc0=diag(zeros(12,1));%初始噪声方差
% Rc0=diag(zeros(3,1));%GPS测量噪声误差方差

%可以改变量使输入的惯性元件的数据带噪声或者不带
ins = InsSolver(Qc0,Rc0);
% [state,errorstate] = ins.imu2state(acc_pure,gyro_pure,gps_pure,state0,errorstate0,tspan,step,0);
[state,errorstate] = ins.imu2state(acc_noise,gyro_noise,gps_noise,state0,errorstate0,tspan,step,0);
%plot trajactory
figure(4)
plot3(r(:,1),r(:,2),r(:,3));
title('真实轨迹');
grid on;
figure(1);
plot3(state(:,1),state(:,2),state(:,3));
title('滤波轨迹');
grid on;

% plot postion error
figure(2),subplot(1,3,1);
plot(tspan,state(:,1) - r(:,1));
grid on;
subplot(1,3,2);
plot(tspan,state(:,2) - r(:,2));
grid on;
subplot(1,3,3);
plot(tspan,state(:,3) - r(:,3));
grid on;

%convert quat to attitue angle
% fprintf('press any key to continue\n');
% pause(5);
newatti = zeros(N,3);

for i = 1:N
   newatti(i,:) = attiCalculator.cnb2atti(attiCalculator.quat2cnb(state(i,7:10)));
end

%plot attitute error
figure(3),subplot(1,3,1);
title('attitute angle error');
plot(tspan,newatti(:,1) - atti(:,1));
grid on;
subplot(1,3,2);
plot(tspan,newatti(:,2) - atti(:,2));
grid on;
subplot(1,3,3);
plot(tspan,newatti(:,3) - atti(:,3));
grid on;



  

 

  
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
 

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 沈再阳.精通MATLAB信号处理[M].清华大学出版社，2015.
[2]高宝建,彭进业,王琳,潘建寿.信号与系统——使用MATLAB分析与实现[M].清华大学出版社，2020.
[3]王文光,魏少明,任欣.信号处理与系统分析的MATLAB实现[M].电子工业出版社，2018.
[4]李树锋.基于完全互补序列的MIMO雷达与5G MIMO通信[M].清华大学出版社.2021
[5]何友,关键.雷达目标检测与恒虚警处理（第二版）[M].清华大学出版社.2011
[6]张晓东.可量测影像与GPS/IMU融合高精度定位定姿方法研究[J].

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

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