exp3.zip_IMU_惯导里程计_组合导航_里程计 惯性_里程计组合

close all; clear; clc; global d2r T;%角度换算成弧度 global P Rk; disp('*******************'); disp('参数初始化'); initpara; disp('.........'); disp('载入数据'); load 'data_exp3.mat'; disp('.........'); disp('数据预处理'); [caldata,vo] = pre_data(data_exp3_imu);%惯导数据与里程计数据 vgps = data_exp3_gps(:,5:7);%gps速度信息 v_gps(:,1) = vgps(:,2);%坐标系转换 v_gps(:,2) = vgps(:,1); v_gps(:,3) =-vgps(:,3); disp('.........'); disp('初始对准'); %% %2分钟粗对准 t1 = 2*60; [roll,pitch,yaw] = coarse_align(caldata);%欧拉角单位为弧度 fprintf('粗对准结果\n 横滚角：%f\n 俯仰角：%f\n 偏航角：%f\n',roll/d2r,pitch/d2r,yaw/d2r); %% %8分钟精对准 t2 = 10*60; n1 = t1/T; n2 = t2/T; q0 = eul2qua(roll,pitch,yaw); q = q0;%初始姿态 v = [0,0,0];%初始速度 h = 0;%初始高度 lati = 28.2202*d2r;%初始纬度 longi = 112.9916*d2r;%初始经度 i = n1; j = 1; while i <= (n2-2) data = caldata(i+1:i+2,:); [q,v,h,lati,longi] = ins_navi(q,v,h,lati,longi,data); kf_data = mean(data); vs = [0;0;0];%车辆静止 [q,v] = KF_correct(q,v,vs,h,lati,longi,kf_data); [roll,pitch,yaw] = dcm2eul(qua2dcm(q)); eul(:,j) = [roll,pitch,yaw]; j=j+1; i = i+2; end cbn = qua2dcm(q); [roll,pitch,yaw] = dcm2eul(cbn); fprintf('精对准结果\n 横滚角：%f\n 俯仰角：%f\n 偏航角：%f\n',roll/d2r,pitch/d2r,yaw/d2r); %% disp('.........'); disp('导航开始'); [n,m] = size(caldata); % roll = 0.5128*d2r; % pitch = 0.2580*d2r; % yaw = 23.7879*d2r; q0 = eul2qua(roll,pitch,yaw); q = q0;%初始姿态 v = [0,0,0];%初始速度 h = 0;%初始高度 lati = 28.2202*d2r;%初始纬度 longi = 112.9916*d2r;%初始经度 i = n2; j = 1; a = (n-n2)/100; x_ins_odo = zeros(1,a);%ins/gps存储经度 y_ins_odo = zeros(1,a);%ins/gps存储纬度 z_ins_odo = zeros(1,a);%ins/gps存储高度 w_ins_odo = zeros(3,a);%ins/gps存储速度 eu_ins_odo = zeros(3,a);%ins/gps存储欧拉角 pos_n = zeros(1,a); pos_e = zeros(1,a); % P = eye(5)*1e-3;%重置初始协方差矩阵 % Rk = eye(2)*1e-8;%观测方程噪声 while i <= (n-2) %INS/GPS data = caldata(i+1:i+2,:); [q,v,h,lati,longi] = ins_navi(q,v,h,lati,longi,data); cbn = qua2dcm(q); kf_data_odo = mean(data); v_odo = (vo(i+1,:)+vo(i+2,:))/2; vs = v_odo*cbn'; [q,v] = KF_correct_odo(q,v,vs,h,lati,longi,kf_data_odo);%加入里程计信息的卡尔曼滤波 if mod((i+2-n2),100) == 0 w_ins_odo(:,j) = v; x_ins_odo(j) = longi/d2r; y_ins_odo(j) = lati/d2r; pos_n(j) = (longi/d2r-112.9916)*60*60*30.9*cos(lati); pos_e(j) = (lati/d2r-28.2202)*60*60*30.9; z_ins_odo(j) = h; [roll,pitch,yaw] = dcm2eul(qua2dcm(q)); eu_ins_odo(:,j) = [roll,pitch,yaw]; j = j+1; end i = i+2; end %% %数据处理 x_gps = data_exp3_gps(601:end,10)'./d2r;%经度 deg y_gps = data_exp3_gps(601:end,8)'./d2r;%纬度 deg z_gps = data_exp3_gps(601:end,9)'./d2r;%高程 disp('.........'); disp('导航结束'); disp('*******************'); figure;grid on;hold on; plot(x_ins_odo,y_ins_odo,'b',x_gps,y_gps,'r'); xlabel('东向 deg'); ylabel('北向 deg'); figure;grid on; subplot(3,1,1); plot((x_ins_odo-x_gps)); title('经度误差'); subplot(3,1,2); plot((y_ins_odo-y_gps)); title('纬度误差'); subplot(3,1,3); plot(z_ins_odo-z_gps); title('高度误差'); figure;grid on; subplot(3,1,1); plot(w_ins_odo(1,:)'-v_gps(601:end,1)); title('北向速度误差'); subplot(3,1,2); plot(w_ins_odo(2,:)'-v_gps(601:end,2)); title('东向速度误差'); subplot(3,1,3); plot(w_ins_odo(3,:)'-v_gps(601:end,3)); title('地向速度误差'); figure;grid on;hold on; title('滚动角、俯仰角'); plot(eu_ins_odo(1,:),'r'); plot(eu_ins_odo(2,:),'b'); xlabel('t(s)'); ylabel('roll_pitch(rad)'); figure;grid on;hold on; title('偏航角'); plot(eu_ins_odo(3,:)); xlabel('t(s)'); ylabel('yaw(rad)'); figure;grid on;hold on; title('速度'); plot(w_ins_odo(1,:),'r'); plot(w_ins_odo(2,:),'g'); plot(w_ins_odo(3,:),'b'); xlabel('t(s)'); ylabel('V-NED(m/s)'); figure;grid on;hold on; title('位置'); plot(pos_n,'r'); plot(pos_e,'g'); plot(z_ins_odo,'b'); xlabel('t(s)'); ylabel('pos-NED(rad)');