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