硕士期间要做基于伪距,伪距率和IMU的紧耦合,正好找到了一个开源的MATLAB代码,决定在学习的过程中把笔记放在这里,方便和大家一起学习讨论。
以下是github链接:
benzenemo/TightlyCoupledINSGNSSgithub.com
这个代码是基于Paul D. Groves教授的Principles of GNSS, inertial, and multisensor integrated navigation systems书上的开源代码修改得到的,结构很清晰。
主文件是“INS_GNSS_Demo_7.m”,在这里输入组合导航需要的参数。
GNSS部分有:
% Interval between GNSS epochs (s)时间间隔
GNSS_config.epoch_interval = 1.0;
% Max Number of satellites 选取的最大卫星数量
GNSS_config.intend_no_GNSS_meas = 30;%If the number of observations exceeds this value, start satellite screening
% Mask angle (deg) 最小角度
GNSS_config.mask_angle = 10;
% Mask Carrier to noise ratio(dbhz) 最小SNR
GNSS_config.mask_SignalStrenth = 30;
% Inner System Bias(m) BDS和GPS的系统时间差
GNSS_config.ISBBDS_GPS = 69.13*c/1e9;%2018165 ClockBDS-ClockGPS
% Satellite to Omit 不使用的GNSS卫星列表
GNSS_config.omit = 33:56;%Control the GNSS type involved in the solution through this configuration
% GNSS type and PRN mapping list 每个卫星系统的卫星编号排序
GNSS_config.GPSPRNList=1:32;
P矩阵协方差:
% Initial attitude uncertainty per axis (deg, converted to rad)
TC_KF_config.init_att_unc = degtorad(20);
% Initial velocity uncertainty per axis (m/s)
TC_KF_config.init_vel_unc = 0.1;
% Initial position uncertainty per axis (m)
TC_KF_config.init_pos_unc = 10;
% Initial accelerometer bias uncertainty per instrument (micro-g, converted
% to m/s^2)
TC_KF_config.init_b_a_unc = 10000 * micro_g_to_meters_per_second_squared;
% Initial gyro bias uncertainty per instrument (deg/hour, converted to rad/sec)
TC_KF_config.init_b_g_unc = 10 * deg_to_rad / 3600;
% Initial clock offset uncertainty per axis (m)
TC_KF_config.init_clock_offset_unc = 10;
% Initial clock drift uncertainty per axis (m/s)
TC_KF_config.init_clock_drift_unc = 0.1;
KF的Q和R矩阵有:
% Gyro noise PSD (deg^2 per hour, converted to rad^2/s) 陀螺仪零偏
TC_KF_config.gyro_noise_PSD = (0.01)^2;
% Accelerometer noise PSD (micro-g^2 per Hz, converted to m^2 s^-3) 加速度计零偏
TC_KF_config.accel_noise_PSD = (0.1)^2;
% Accelerometer bias random walk PSD (m^2 s^-5) 加速度随机游走
TC_KF_config.accel_bias_PSD = 1.0E-5;
% Gyro bias random walk PSD (rad^2 s^-3) 陀螺仪随机游走
TC_KF_config.gyro_bias_PSD = 4.0E-11;
% Receiver clock frequency-drift PSD (m^2/s^3) 接收机钟差
TC_KF_config.clock_freq_PSD = 1;
% Receiver