?作者簡介：熱愛科研的Matlab仿真開發(fā)者，修心和技術(shù)同步精進(jìn)，matlab項目合作可私信。

個人主頁： Matlab科研工作室

個人信條：格物致知。

更多Matlab仿真內(nèi)容點(diǎn)擊

智能優(yōu)化算法       神經(jīng)網(wǎng)絡(luò)預(yù)測       雷達(dá)通信       無線傳感器         電力系統(tǒng)

信號處理               圖像處理               路徑規(guī)劃       元胞自動機(jī)         無人機(jī)

? 內(nèi)容介紹

慣性導(dǎo)航(INS)和全球定位導(dǎo)航(GPS)是現(xiàn)代航空武器中應(yīng)用廣泛的兩種導(dǎo)航技術(shù).運(yùn)用組合導(dǎo)航技術(shù),將INS與GPS兩者有機(jī)組合,能很好地克服各自缺點(diǎn),形成優(yōu)勢互補(bǔ)結(jié)構(gòu).在無人機(jī)應(yīng)用GPS/INS組合導(dǎo)航的基礎(chǔ)上,引入卡爾曼濾波技術(shù),能夠有效提高導(dǎo)航的性能和精度,從而構(gòu)成比較理想的組合導(dǎo)航系統(tǒng).

? 部分代碼

clc

clear 

% 生成仿真數(shù)據(jù)

generate_uav_sensors_structure;

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

g_mps2 = 9.81;  

% 定義directEKF算法相關(guān)變量

ekf = [];

% 磁強(qiáng)計和GPS測量噪聲估計，用于構(gòu)造 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)));    

% 陀螺儀和加速度計初始估計的不確定度，用于構(gòu)造P矩陣（協(xié)方差矩陣）

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

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

% EKF過程噪聲估計，用于構(gòu)造 Q 矩陣

% Q 矩陣需要根據(jù)經(jīng)驗(yàn)調(diào)節(jié)。Q 值代表了我們對狀態(tài)模型的置信程度

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)));    

% 設(shè)定EKF狀態(tài)向量初始值

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) 狀態(tài)進(jìn)行初始化，rad

        pos_init ...                                %   4-6: 對 North-East-Alt 位置狀態(tài)進(jìn)行初始化, m

        vel_init ...                                %   7-9: 對 NED 速度狀態(tài)初始化, m/s

        [0 0 0] ...                                 % 10-12: 對 XYZ 三軸陀螺儀的常值偏差進(jìn)行初始化, rad/s

        [0 0 0] ...                                 % 13-15: 對 XYZ 三軸加速度計的長治偏差進(jìn)行初始化, m/s^2

    ]’;

clear psi theta phi euler_init pos_init vel_init

% 對初始的協(xié)方差矩陣（P矩陣）設(shè)定較大值，以便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設(shè)定為0

clear large_angle_uncertainty_rad large_pos_uncertainty_m large_vel_uncertainty_mps

? 運(yùn)行結(jié)果

? 參考文獻(xiàn)

[1] 孫熙, 祖峰, 李夏苗. 基于擴(kuò)展卡爾曼濾波器的超緊耦合GPS/INS組合導(dǎo)航系統(tǒng)設(shè)計[J]. 微電子學(xué)與計算機(jī), 2008, 25(6):4.

[2] 何秀鳳, 陳永奇. 區(qū)間Kalman濾波器及其在GPS/INS組合導(dǎo)航系統(tǒng)中的應(yīng)用(英文)[J]. Transactions of Nanjing University of Aeronautics & Astronau, 1999(1):41-47.

[3] 何秀鳳, 楊光. 擴(kuò)展區(qū)間Kalman濾波器及其在GPS/INS組合導(dǎo)航中的應(yīng)用[J]. 測繪學(xué)報, 2004, 33(1):6.

[4] 楊莉. 基于GPS/SINS組合導(dǎo)航算法的優(yōu)化設(shè)計與仿真[D]. 中國科學(xué)院大學(xué)(工程管理與信息技術(shù)學(xué)院), 2015.

[5] 張群, 洪志強(qiáng). 抗差自適應(yīng)無跡Kalman濾波在GPS/INS組合導(dǎo)航中的應(yīng)用[J]. 北京測繪, 2021(011):035.

[6] 錢正祥, 楊鷺怡, 崔衛(wèi)兵. 無人機(jī)GPS/INS組合導(dǎo)航卡爾曼濾波技術(shù)[C]// "測量與控制在資源節(jié)約,環(huán)境保護(hù)中的應(yīng)用"學(xué)術(shù)會議. 2003.

?? 代碼獲取關(guān)注我

??部分理論引用網(wǎng)絡(luò)文獻(xiàn)，若有侵權(quán)聯(lián)系博主刪除

?? 關(guān)注我領(lǐng)取海量matlab電子書和數(shù)學(xué)建模資料