【目标定位】基于拓展卡尔曼滤波实现GPS-INS组合导航系统附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

个人主页：Matlab科研工作室

个人信条：格物致知。

更多Matlab仿真内容点击

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

惯性导航(INS)和全球定位导航(GPS)是现代航空武器中应用广泛的两种导航技术.运用组合导航技术,将INS与GPS两者有机组合,能很好地克服各自缺点,形成优势互补结构.在无人机应用GPS/INS组合导航的基础上,引入卡尔曼滤波技术,能够有效提高导航的性能和精度,从而构成比较理想的组合导航系统.

⛄ 部分代码

clc

clear 

% 生成仿真数据

generate_uav_sensors_structure;

% 重力加速度常量，m/s^2

g_mps2 = 9.81;  

% 定义directEKF算法相关变量

ekf = [];

% 磁强计和GPS测量噪声估计，用于构造 R 矩阵

ekf.sigmas.mag2D_meas_rad        = sqrt(uavSensors.sigmas.mag2D_yaw_noise_rad^2);

ekf.sigmas.pos_meas_m            = sqrt(uavSensors.sigmas.GPSpos_noise_m^2);

ekf.sigmas.vel_meas_mps          = sqrt(uavSensors.sigmas.GPSvel_noise_mps^2);

ekf.R_gps = diag([...

        [1 1 1]*ekf.sigmas.pos_meas_m ...            % North-East-Alt position measurement noise, m

        [1 1 1]*ekf.sigmas.vel_meas_mps ...          % NED velocity measurement noise, m/s

    ].^2);

ekf.R_gps = max(ekf.R_gps,(1e-3)^2*eye(size(ekf.R_gps)));    

ekf.R_mag2d = ekf.sigmas.mag2D_meas_rad;

ekf.R_mag2d = max(ekf.R_mag2d,(1e-3)^2*eye(size(ekf.R_mag2d)));    

% 陀螺仪和加速度计初始估计的不确定度，用于构造P矩阵（协方差矩阵）

ekf.sigmas.gyro_bias_rps         = uavSensors.sigmas.gyro_bias_rps;

ekf.sigmas.accel_bias_mps2       = uavSensors.sigmas.accel_bias_mps2;

% EKF过程噪声估计，用于构造 Q 矩阵

% Q 矩阵需要根据经验调节。Q 值代表了我们对状态模型的置信程度

ekf.sigmas.attitude_process_noise_rad = 0.002;%0.002;% Euler angle process noise, rad

ekf.sigmas.pos_process_noise_m = 0.005;%0;           % Position process noise, m

ekf.sigmas.vel_process_noise_mps = 0.001;%2;         % Velocity process noise, m/s

ekf.sigmas.gyroBias_process_noise_rps = 1e-5;%1e-6; % Gyro bias process noise, rad/s

ekf.sigmas.accelBias_process_noise_mps2=1e-5;%1e-6; % Accel bias process noise, m/s^2

ekf.Q = diag([...

            [1 1 1]*ekf.sigmas.attitude_process_noise_rad ...   % Euler angle process noise, rad

            [1 1 1]*ekf.sigmas.pos_process_noise_m ...          % North-East-Alt position process noise, m

            [1 1 1]*ekf.sigmas.vel_process_noise_mps ...        % NED velocity process noise, m/s

            [1 1 1]*ekf.sigmas.gyroBias_process_noise_rps ...   % XYZ gyro bias process noise, rad/s

            [1 1 1]*ekf.sigmas.accelBias_process_noise_mps2 ... % XYZ accel bias process noise, m/s^2

        ].^2);

ekf.Q = max(ekf.Q,(1e-3)^2*eye(size(ekf.Q)));    

% 设定EKF状态向量初始值

phi = 0;                                   % 横滚角初始值，rad

theta=0;                                   % 俯仰角初始值，rad

psi = uavSensors.mag2D_yaw_deg(1)*pi/180;  % 偏航角初始值, rad

euler_init = [ phi theta psi ];   

pos_init = [ uavSensors.GPS_north_m(1) uavSensors.GPS_east_m(1) uavSensors.GPS_h_msl_m(1) ];

vel_init = uavSensors.GPS_v_ned_mps(1,:);

ekf.xhat = [ ...

        euler_init ...                              %   1-3: 对 Euler angle (Roll, Pitch, Yaw) 状态进行初始化，rad

        pos_init ...                                %   4-6: 对 North-East-Alt 位置状态进行初始化, m

        vel_init ...                                %   7-9: 对 NED 速度状态初始化, m/s

        [0 0 0] ...                                 % 10-12: 对 XYZ 三轴陀螺仪的常值偏差进行初始化, rad/s

        [0 0 0] ...                                 % 13-15: 对 XYZ 三轴加速度计的长治偏差进行初始化, m/s^2

    ]';

clear psi theta phi euler_init pos_init vel_init

% 对初始的协方差矩阵（P矩阵）设定较大值，以便EKF算法在初始的几次滤波步骤中更多的相信测量方程

large_angle_uncertainty_rad = 30*pi/180;

large_pos_uncertainty_m = 100;

large_vel_uncertainty_mps = 10;

ekf.P = diag([...

        [1 1 1]*large_angle_uncertainty_rad ...     % init Euler angle (NED-to-body) uncertainties, rad

        [1 1 1]*large_pos_uncertainty_m ...         % init North-East-Alt position uncertainties, m

        [1 1 1]*large_vel_uncertainty_mps ...       % init NED velocity uncertainties, m/s

        [1 1 1]*ekf.sigmas.gyro_bias_rps ...        % init XYZ gyro bias uncertainties, rad/s

        [1 1 1]*ekf.sigmas.accel_bias_mps2 ...      % init XYZ accel bias uncertainties, m/s^2

    ].^2);

ekf.P = max(ekf.P,(1e-3)^2*eye(size(ekf.P)));    % 为了避免gyro_bias_rps和accel_bias_mps2设定为0

clear large_angle_uncertainty_rad large_pos_uncertainty_m large_vel_uncertainty_mps

⛄ 运行结果

⛄ 参考文献

[1] 孙熙, 祖峰, 李夏苗. 基于扩展卡尔曼滤波器的超紧耦合GPS/INS组合导航系统设计[J]. 微电子学与计算机, 2008, 25(6):4.

[2] 何秀凤, 陈永奇. 区间Kalman滤波器及其在GPS/INS组合导航系统中的应用(英文)[J]. Transactions of Nanjing University of Aeronautics & Astronau, 1999(1):41-47.

[3] 何秀凤, 杨光. 扩展区间Kalman滤波器及其在GPS/INS组合导航中的应用[J]. 测绘学报, 2004, 33(1):6.

[4] 杨莉. 基于GPS/SINS组合导航算法的优化设计与仿真[D]. 中国科学院大学(工程管理与信息技术学院), 2015.

[5] 张群, 洪志强. 抗差自适应无迹Kalman滤波在GPS/INS组合导航中的应用[J]. 北京测绘, 2021(011):035.

[6] 钱正祥, 杨鹭怡, 崔卫兵. 无人机GPS/INS组合导航卡尔曼滤波技术[C]// "测量与控制在资源节约,环境保护中的应用"学术会议. 2003.

⛳️ 代码获取关注我

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

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