error_plot.m

惯性导航精解算程序

function error_plot(trajectory_type)
%%
%载入真实轨迹和解算后的轨迹

if trajectory_type==1
    load Storedata_line; 
    load imusolve_line;
    load Store_error_line;
elseif trajectory_type==2
    load Storedata_line2;
    load imusolve_line2;
    load Store_error_line2;
elseif trajectory_type==3
    load Storedata_circle;
    load imusolve_circle;
    load Store_error_circle;
end

parameter = Storedata(1,1:3);
true_type = parameter(1);
true_caiyangT = parameter(2);
true_step = parameter(3);
parameter2 = Storedata_solve(1,1:3);
solve_type = parameter2(1);
solve_caiyangT = parameter2(2);
solve_step = parameter2(3);

if true_type~=solve_type
    error('类型不匹配');
end
tt = solve_caiyangT/true_caiyangT;%解算数据采样周期是真实采样周期的倍数
if mod(tt,1)~=0;%判断是否为整数
    fprintf('真实数据采样时间为：');disp(true_caiyangT);
    error('解算数据的采样周期应为原始数据的整数倍');
end
if(solve_caiyangT*solve_step>true_caiyangT*true_step)
    error('仿真时间超过真实数据仿真时间');
end
%Storedata1 = Storedata(2:row,1:15);
true_location = Storedata(2:tt:solve_step*tt+1,1:3);
true_velocity = Storedata(2:tt:solve_step*tt+1,4:6);
true_acceleration = Storedata(2:tt:solve_step*tt+1,7:9);
true_attitude = Storedata(2:tt:solve_step*tt+1,10:12);
%ture_angle_ve = Storedata1(1:tt:solve_step*tt,13:15);




solve_location = Storedata_solve(2:solve_step+1,1:3);
solve_velocity = Storedata_solve(2:solve_step+1,4:6);

solve_acceleration = Storedata_solve(2:solve_step+1,7:9);
solve_attitude = Storedata_solve(2:solve_step+1,10:12);

error_location = solve_location-true_location;
error_velocity = solve_velocity-true_velocity;

error_acceleration = solve_acceleration-true_acceleration;
error_attitude = solve_attitude-true_attitude;
figure
subplot(2,3,1),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_location(:,1)');title('真纬度')
subplot(2,3,2),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_location(:,2)');title('真经度');
subplot(2,3,3),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_velocity(:,1)');title('真东速');
subplot(2,3,4),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_velocity(:,2)');title('真北速');
subplot(2,3,5),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_acceleration(:,1)');title('真东加');
subplot(2,3,6),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_acceleration(:,2)');title('真北加');

figure
subplot(2,3,1),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_location(:,1));title('e纬度');
subplot(2,3,2),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_location(:,2));title('e经度');
subplot(2,3,3),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_velocity(:,1));title('e东速');
subplot(2,3,4),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_velocity(:,2));title('e北速');
subplot(2,3,5),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_acceleration(:,1));title('e东加');
subplot(2,3,6),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_acceleration(:,2));title('e北加');

figure

subplot(3,2,1),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_attitude(:,1));title('真俯仰');
subplot(3,2,2),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_attitude(:,2));title('真滚动');
subplot(3,2,3),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,true_attitude(:,3));title('真航向');
subplot(3,2,4),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_attitude(:,1));title('e俯仰');
subplot(3,2,5),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_attitude(:,2));title('e滚动');
subplot(3,2,6),plot(solve_caiyangT:solve_caiyangT:solve_caiyangT*solve_step,error_attitude(:,3));title('e航向');

figure
subplot(3,2,1),plot(error_location(:,1)-Store_error(:,1));title('纬修');
subplot(3,2,2),plot(error_location(:,2)-Store_error(:,2));title('经修');
subplot(3,2,3),plot(error_velocity(:,1)-Store_error(:,3));title('东速修');
subplot(3,2,4),plot(error_velocity(:,2)-Store_error(:,4));title('北速修');
subplot(3,2,5),plot(error_attitude(:,1)-Store_error(:,5));title('俯修');
subplot(3,2,6),plot(error_attitude(:,2)-Store_error(:,6));title('滚修');
figure
plot(error_attitude(:,3)-Store_error(:,7));title('航修');