我要评论⛄ 内容介绍
本文是一种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.
⛳️ 代码获取关注我
❤️部分理论引用网络文献，若有侵权联系博主删除
❤️ 关注我领取海量matlab电子书和数学建模资料
发表评论