✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
本文是一种GNSS组合导航方法及装置,该方法包括:S1.使用惯性导航系统与GNSS的组合导航系统进行组合导航,输出导航信息,姿态角信息;S2.接收姿态角信息,并使用多GNSS定姿定向系统计算方位角信息,输出计算得到的方位角信息;S3.组合导航系统接收计算得到的方位角信息进行更新,输出更新后的导航信息.本发明具有实现方法简单,能够实现惯性/GNSS组合导航与多GNSS定姿定向的双向融合,且导航精度高且收敛速度快等优点.
⛄ 部分代码
clear all; close all; tic;
% 3DTraj_0528_04是一段动态数据
inputDataPath = strcat('.\data', '\', '3DTraj_220422_01.mat');
load(inputDataPath);
rng(1233, 'twister');
% 创建陀螺和加计，设置其误差参数并生成imu测量误差
G1 = Gyroscope();
G1.ts = 0.01;
G1.bias = 0.04 * [1; 1; 1] * Unit.degPerHour;
G1.noiseDensity = 0.01 * [1; 1; 1] * Unit.degPerSqhour;
gyroMeasErr = G1.generateError(trueValue.epoch);
A1 = Accelerometer();
A1.ts = 0.01;
A1.bias = 100 * [1; 1; 1] * Unit.ug;
A1.noiseDensity = 100 * [1; 1; 1] * Unit.ugPerSqhz;
accelMeasErr = A1.generateError(trueValue.epoch);
% 生成带误差的全局位置和速度
DVL = SensorDVL(); % 两次采样的间隔为1s
DVL.ts = 1;
DVL.velocityNoise = 0.1 * [1; 1; 1];
DVLVelMeasErr= DVL.generateError(trueValue.epoch, trueValue.ts);
DVLVel = zeros(trueValue.epoch,3);
for k = 1:1:trueValue.epoch
DVLVel(k,:) = ( anglezxy2dcm(trueValue.attitude(k,2:4)')' * trueValue.velocity(k,2:4)' )';
%加故障
% if (k > 10000)&&(k < 12000)
% DVLVel(k,:) = DVLVel(k,:) + [5, 0, 0];
% end
end
DVLVelMeas = DVLVel(:, 1:3) + DVLVelMeasErr(:, 2:4);
% 初始化导航参数
% C_b_n = anglezxy2dcm(trueValue.attitude(1, 2:4)' + [6; 6; 6] * Unit.angleSec );
C_b_n = anglezxy2dcm(trueValue.attitude(1, 2:4)'+ [0.01; 0.01; 0.1] * Unit.deg );
vel = trueValue.velocity(1, 2:4)'+ [0.01; 0.01; 0];
pos = trueValue.position(1, 2:4)'+ [2/WGS84Parameter.Re, 2/WGS84Parameter.Re, 2];
% 创建状态和量测模型
stateModel = StateModelSINS15Dim(0.01); % 离散化步长是0.01s
measModel = MeasModelSINS15DimWithDVL();
% 创建一个Kalamn滤波器
kf = KalmanFilter();
kf.X = zeros(15, 1);
kf.P = diag([[8, 8, 8] * Unit.angleSec,...
[0.1, 0.1, 0.1],...
[5/WGS84Parameter.Re, 5/ WGS84Parameter.Re, 5],...
[0.01, 0.01, 0.01] * Unit.degPerHour,...
[100, 100, 100] * Unit.ug])^2;
kf.Q = diag([[0.01, 0.01, 0.01] * Unit.degPerSqhour,...
[100, 100, 100] * Unit.ugPerSqhz])^2;
kf.R = diag([0.15, 0.15, 0.15])^2;
% 预分配存储空间
epoch = trueValue.epoch;
calAtt = zeros(epoch - 10, 3); calVel = zeros(epoch - 10, 3); calPos = zeros(epoch - 10, 3);
errAtt = zeros(epoch - 10, 3); errVel = zeros(epoch - 10, 3); errPos = zeros(epoch - 10, 3);
for k = 1:1:epoch - 10
kk = k+1;
w_ib_b = trueValue.w(k, 2:4)' + gyroMeasErr(k, 2:4)';
f_ib_b = trueValue.a(k, 2:4)' + accelMeasErr(k, 2:4)';
[C_b_n, vel, pos] = insUpdateENU(C_b_n, vel, pos, w_ib_b, f_ib_b, trueValue.ts);
stateModel.setParameter(C_b_n, vel, pos, f_ib_b);
stateModel.update();
kf.Phi = stateModel.transitionMatrix;
kf.Gamma = stateModel.noiseDriveMatrix;
kf.stateUpdate();
if mod(k, 10) == 0
% vv1(kk,:) = ( C_b_n* DVLVelMeas(kk, :)')'; % 此处验证了 问题应该出现在将DVL速度转换到导航系中
% vv2(kk,:) = (anglezxy2dcm(trueValue.attitude(kk,2:4)')* DVLVelMeas(kk, :)')';
% vv3(kk,:) = trueValue.velocity(kk,2:4);
measModel.setParameter(vel, DVLVelMeas(kk, :)', C_b_n);
measModel.update();
kf.dZ = measModel.innovation;
% AAA = kf.dZ
kf.H = measModel.measurementMatrix;
kf.measurementUpdate();
C_b_n = anglezxy2dcm(kf.X(1:3)) * C_b_n;
vel = vel - kf.X(4:6);
pos = pos - kf.X(7:9);
kf.zero(1,2,3,4,5,6,7,8,9);
end
calAtt(k, :) = dcm2anglezxy(C_b_n)';
calVel(k, :) = vel';
calPos(k, :) = pos';
errAtt(k, :) = standarlizePRYAngle(calAtt(k, :) - trueValue.attitude(kk, 2:4));
errVel(k, :) = calVel(k, :) - trueValue.velocity(kk, 2:4);
errPos(k, :) = calPos(k, :) - trueValue.position(kk, 2:4);
end
%% 绘图
t = 0.01:0.01:0.01*(epoch - 10);
figure(1) % 真实轨迹图
plot3(trueValue.position(:, 3)/Unit.deg, trueValue.position(:, 2)/Unit.deg,trueValue.position(:, 4),'b-','linewidth',2.0);
axis tight; grid on;
xlabel('经度 / deg'); ylabel('纬度 / deg'); zlabel('高度 / m');hold on;
plot3(trueValue.position(1, 3)/Unit.deg, trueValue.position(1, 2)/Unit.deg,...
trueValue.position(1, 4), 'r*');
legend( '真实轨迹','起点');
figure
subplot(311)
plot(t,errAtt(:, 1) / Unit.deg);
title('姿态误差');
subplot(312)
plot(t,errAtt(:, 2) / Unit.deg);
subplot(313)
plot(t,errAtt(:, 3) / Unit.deg);
figure
subplot(311)
plot(t,errVel(:, 1));
title('速度误差');
subplot(312)
plot(t,errVel(:, 2));
subplot(313)
plot(t,errVel(:, 3));
figure
subplot(311)
plot(t,errPos(:, 1) * WGS84Parameter.Re, 'linewidth', 2.0); hold on;
ylabel('北向误差 / m');
title('位置误差');
subplot(312)
plot(t,errPos(:, 2) * (WGS84Parameter.Re * cos(45*Unit.deg)), 'linewidth', 2.0); hold on;
ylabel('东向误差 / m');
subplot(313)
plot(t,errPos(:, 3), 'linewidth', 2.0); hold on;
ylabel('天向误差 / m');
mean( abs(errVel(:, 1)) )
mean( abs(errVel(:, 2)) )
mean( abs(errVel(:, 3)) )
toc;
⛄ 运行结果
⛄ 参考文献
[1] 万年红, 王雪蓉. 基于边缘特征点的全景图像拼接算法[J]. 温州大学学报：自然科学版, 2016(1):10.
[2] 格鲁夫, P. D. ). GNSS与惯性及多传感器组合导航系统原理[M]. 国防工业出版社, 2011.
[3] 苏景岚. 车载视觉/INS/GNSS多传感器融合定位定姿算法研究[D]. 武汉大学, 2019.
[4] 胡恩伟. 基于MEMS多传感器数据融合的惯性组合导航系统算法设计与实现[D]. 重庆大学.
[5] PaulD.Groves. GNSS与惯性及多传感器组合导航系统原理.第2版[M]. 国防工业出版社, 2015.
[6] 格鲁夫练军想, 唐康华, 潘献飞. GNSS与惯性及多传感器组合导航系统原理 : Principles of GNSS, inertial, and multisensor integrated navigation systems[M]. 国防工业出版社, 2015.
