test_error.m

惯性导航精解算程序

function test_error()
trajectory;%生成真实轨迹
trajectory_type = 1;

caiyangT = .01;
totaltime = 300;
acc_gyro(trajectory_type,caiyangT,totaltime);%陀螺仪加速度的输出
if trajectory_type==1;%读取陀螺仪加速度计的数据
    load acc_gyro_line ;
elseif trajectory_type==2
    load acc_gyro_line2;
elseif trajectory_type==3
    load acc_gyro_circle;
    
end
 load acc_gyro_line1;
%%
%陀螺仪加速度计数据的参数
parameter2 = acc_gyro_data(1:5,1:6);
trajectory_t = parameter2(1,1);
if trajectory_t~=trajectory_type
    error('轨迹类型不一致')
end
caiyangT = parameter2(1,2);%采样的周期
step = parameter2(1,3);%采样的点数
location0 = parameter2(1,4:6)';%初始的位置
velocity0 = parameter2(2,1:3)';%初始的速度
acceleration0 = parameter2(2,4:6)';%初始的加速度
attitude0 = parameter2(3,1:3)';%初始的姿态
angle_ve0 = parameter2(3,4:6)';%初始的角速度
Fe_chang = parameter2(4,1);%加速度计的常值误差
Fe_randn = parameter2(4,2);%加速度计的随机误差
We_chang = parameter2(5,1);%陀螺仪的常值误差
We_randn = parameter2(5,2);%陀螺仪的随机误差
accelerometer = acc_gyro_data(6:step+5,1:3);%加速度计的数据
gyro = acc_gyro_data(6:step+5,4:6);%陀螺仪的数据

global Ra e wie;
Ra = 6378140;%6000000%地球参考椭球长轴半径
e = 1/298.257;%0%地球椭球的椭圆度
wie = 15.04088/3600/180*pi; %0%地球自转角速度










%%
%初始化q0，Cb_n0,存储的
Storedata_l = zeros(step,3);
Storedata_v = zeros(step,3);
Storedata_a = zeros(step,3);
Storedata_z = zeros(step,3);
Storedata_l(1,:) = location0';
Storedata_v(1,:) = velocity0';
Storedata_a(1,:) = acceleration0';
Storedata_z(1,:) = attitude0';
%%
%惯导初始对准
theta = attitude0(1);
gama = attitude0(2);
fai = attitude0(3);
[q0,Cb_n0]  = ImuAliment2(theta,gama,fai);
%%
%具体的解算过程
fprintf('正在解算数据……\n');
hWaitBar = waitbar(0,'Please wait...');
for i=1:step
    accelerometer0 = accelerometer(i,:);
    gyro0 = gyro(i,:);
    [location,velocity,acceleration,attitude,q1,Cb_n]=Imusolve2(accelerometer0,gyro0,location0,velocity0,acceleration0,attitude0,q0,Cb_n0,caiyangT);
     Storedata_l(i+1,:) = location';
     Storedata_v(i+1,:) = velocity';
     Storedata_a(i+1,:) = acceleration';   
     Storedata_z(i+1,:) = attitude'; 
%%
%更新
    location0 = location;
    velocity0 = velocity;
    acceleration0 = acceleration;
    q0 = q1;
    Cb_n0 = Cb_n;
    waitbar(i/step,hWaitBar,[num2str( fix(i/step*100) ),'%']);
end

close(hWaitBar);
%%
%存储解算后的数据Storedata_solve,没有估算角速率
parameter = zeros(1,12);
parameter(1) = trajectory_type;
parameter(2) = caiyangT;
parameter(3) = step;
Storedata_solve = [parameter;Storedata_l Storedata_v Storedata_a Storedata_z];
if trajectory_type==1 
    save imusolve_line.mat Storedata_solve -ASCII -DOUBLE -MAT;
elseif trajectory_type==2  
    save imusolve_line2.mat Storedata_solve -ASCII -DOUBLE -MAT;
elseif trajectory_type==3
    save imusolve_circle.mat Storedata_solve -ASCII -DOUBLE -MAT;
end
%%
%误差补偿以及画误差曲线
error_function(trajectory_type);
error_plot(trajectory_type);