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

Posted 2021-10-24 紫极神光

一、获取代码方式

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

获取代码方式2：
通过紫极神光博客主页开通CSDN会员，凭支付凭证，私信博主，可获得此代码。

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

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

二、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;

四、运行结果

五、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].