【组合导航】GNSS与惯性及多传感器组合导航附matlab代码

✅作者简介：热爱科研的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.

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料