1 模型
一般的Kalman滤波器要求有准确的动态和统计模型,而低成本的MEMS-IMU性能随着温度急剧变化,故在MEMS-IMU/GPS组合导航系统中使用一般的Kalman滤波器存在很多的局限性。针对低成本的MEMS-IMU/GPS组合导航系统,提出了多模态自适应滤波算法在MEMS-IMU/GPS组合导航系统中的应用;针对普通的多模态算法中的问题,采用修正的多模态自适应滤波算法来提高MEMS-IMU/GPS组合导航系统的性能。使用静态实时测试数据,验证了所提出的算法。测试结果表明,与普通Kalman滤波器相比,修正的多模态滤波算法提高了MEMS-IMU/GPS组合导航系统的性能;采用所提出的算法,MEMS-IMU/GPS组合导航系统的短时间静态位置精度小于5m(标准差),速度精度小于0.1m/s(标准差),姿态角精度小于0.5°
2 部分代码
%for testing clc clear close all pauseLen = 0; %%Initializations %TODO: load data here data = load('lib/IMU_GPS_GT_data.mat'); IMUData = data.imu; GPSData = data.gpsAGL; gt = data.gt; addpath([cd, filesep, 'lib']) initialStateMean = eye(5); initialStateCov = eye(9); deltaT = 1 / 30; %hope this doesn't cause floating point problems numSteps = 500000;%TODO largest timestamp in GPS file, divided by deltaT, cast to int results = zeros(7, numSteps); % time x y z Rx Ry Rz % sys = system_initialization(deltaT); Q = blkdiag(eye(3)*(0.35)^2, eye(3)*(0.015)^2, zeros(3)); %IMU noise characteristics %Using default values from pixhawk px4 controller %https://dev.px4.io/v1.9.0/en/advanced/parameter_reference.html %accel: first three values, (m/s^2)^2 %gyro: next three values, (rad/s)^2 filter = filter_initialization(initialStateMean, initialStateCov, Q); %IMU noise? do in filter initialization IMUIdx = 1; GPSIdx = 1; nextIMU = IMUData(IMUIdx, :); %first IMU measurement nextGPS = GPSData(GPSIdx, :); %first GPS measurement %plot ground truth, raw GPS data % plot ground truth positions plot3(gt(:,2), gt(:,3), gt(:,4), '.g') grid on hold on % plot gps positions % plot3(GPSData(:,2), GPSData(:,3), GPSData(:,4), '.b') axis equal axis vis3d counter = 0; MAXIGPS = 2708; MAXIIMU = 27050; isStart = false; for t = 1:numSteps currT = t * deltaT; if(currT >= nextIMU(1)) %if the next IMU measurement has happened % disp('prediction') filter.prediction(nextIMU(2:7)); isStart = true; IMUIdx = IMUIdx + 1; nextIMU = IMUData(IMUIdx, :); % plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'or'); end if(currT >= nextGPS(1) & isStart) %if the next GPS measurement has happened % disp('correction') counter = counter + 1; filter.correction(nextGPS(2:4)); GPSIdx = GPSIdx + 1; nextGPS = GPSData(GPSIdx, :); plot3(nextGPS(2), nextGPS(3), nextGPS(4), '.r'); % plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'ok'); % plotPose(filter.mu(1:3, 1:3), filter.mu(1:3, 5), filter.mu(1:3,4)); end results(1, t) = currT; results(2:4, t) = filter.mu(1:3, 5); %just position so far % plot3(results(2, t), results(3, t), results(4, t), 'or'); % disp(filter.mu(1:3, 1:3)); if pauseLen == inf pause; elseif pauseLen > 0 pause(pauseLen); end if IMUIdx >= MAXIIMU || GPSIdx >= MAXIGPS break end end plot3(results(2,:), results(3,:), results(4,:), '.b'); % xlim([-10 10]); % ylim([-10 10]); xlabel('x, m'); ylabel('y, m'); zlabel('z, m'); %% Evaluation gps_score = evaluation(gt, GPSData) results_eval = results.'; score = 0; estimation_idx = 1; count = 0; for i = 2:length(gt) score = score + norm(gt(i, 2:4) - results_eval(30 * (i-1), 2:4)) ^ 2; count = count + 1; end count score = sqrt(score / count) %% Function
3 仿真结果
4 参考文献
[1]唐康华, 吴美平, & 胡小平. (2007). Mems-imu/gps组合导航中的多模态kalman滤波器设计. 中国惯性技术学报(03), 307-311.