error_function.m

惯性导航精解算程序

function error_function(trajectory_type)
%处理误差方程的函数
% 1~2位置的误差，去掉天向的；纬度，经度

% 3~4速度的误差，去掉天向的；东向，北向
% 5~7平台失准误差；α，γ，φ
if trajectory_type==1
    load Storedata_line; 
    load imusolve_line;
    load acc_gyro_line.mat;
elseif trajectory_type==2
    load Storedata_line2;
    load imusolve_line2;
    
    load acc_gyro_line2;
elseif trajectory_type==3
    load Storedata_circle;
    load imusolve_circle;
    load acc_gyro_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
% solve_location = Storedata_solve(2:solve_step+1,1:3);

latitude = Storedata_solve(2:solve_step+1,1);
longitude = Storedata_solve(2:solve_step+1,2);
highth = Storedata_solve(2:solve_step+1,3);

v_e = Storedata_solve(2:solve_step+1,4);
v_n = Storedata_solve(2:solve_step+1,5);
v_u = Storedata_solve(2:solve_step+1,6);
a_e = Storedata_solve(2:solve_step+1,7);
a_n = Storedata_solve(2:solve_step+1,8);

a_u = Storedata_solve(2:solve_step+1,9);
%solve_attitude = Storedata_solve(2:solve_step+1,10:12);
thetatt = Storedata_solve(2:solve_step+1,10);
gamatt = Storedata_solve(2:solve_step+1,11);
faitt = Storedata_solve(2:solve_step+1,12);
%%
parameter2 = acc_gyro_data(1:5,1:6);
trajectory_t = parameter2(1,1);
if trajectory_t~=trajectory_type
    
    error('轨迹类型不一致')
end
caiyangT = parameter2(1,2);
step2 = 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:solve_step+5,1:3);
fe = acc_gyro_data(6:solve_step+5,1);
fn = acc_gyro_data(6:solve_step+5,2);
fu = acc_gyro_data(6:solve_step+5,3);
gyro = acc_gyro_data(6:solve_step+5,4:6);

Ra = 6378140;%6000000%地球参考椭球长轴半径
e = 1/298.257;%0%地球椭球的椭圆度
wie = 15.04088/3600/180*pi; %0%地球自转角速度
%%
hWaitBar = waitbar(0,'Please wait...');
X = zeros(7,1);A = zeros(7);
fprintf('正在进行误差补偿计算......\n');
Store_error = zeros(solve_step,7);
for i=1:solve_step
RN = Ra*(1-2*e+3*e*sin(latitude(i))^2);%参考椭球子午面内的曲率半径，南北方向的

RE = Ra*(1+e*sin(latitude(i))^2);%参考椭球子午面的法线平面内的曲率半径，东西方向的
dot_latitude = v_n(i)/RN;%纬度变化率
dot_longitude = v_e(i)/(RE*cos(latitude(i)));%经度变化率
theta = thetatt(i);
gama = gamatt(i);
fai = faitt(i);
Cn_b = [sin(fai)*sin(theta)*sin(gama)+cos(fai)*cos(gama)   sin(fai)*cos(theta)   cos(fai)*sin(gama)-sin(fai)*sin(theta)*cos(gama);  
       cos(fai)*sin(theta)*sin(gama)-sin(fai)*cos(gama)    cos(fai)*cos(theta)   -sin(fai)*sin(gama)-cos(fai)*sin(theta)*cos(gama) ;
       -cos(theta)*sin(gama)                               sin(theta)            cos(theta)*cos(gama)];
% 纬度，经度，东向的速度，北向的速度，theta俯仰角，gama滚动角，fai航向角

A(1,4) = 1/RN;
A(2,1) = dot_longitude*tan(latitude(i));
A(2,3) = 1/(RE*cos(latitude(i)));
A(3,1) = 2*wie*(v_n(i)*cos(latitude(i)))+v_n(i)*dot_longitude/cos(latitude(i));
A(3,3) = dot_latitude*tan(latitude(i))*RN/RE;
A(3,4) = (2*wie+dot_longitude)*sin(latitude(i));
A(3,6) = fu(i);
A(3,7) = -fn(i);
A(4,1) = -2*wie*v_e(i)*cos(latitude(i))-v_e(i)*dot_longitude/cos(latitude(i));
A(4,3) = -2*(wie+dot_longitude)*sin(latitude(i));
A(4,5) = -fu(i);
A(4,7) = fe(i);
A(5,4) = 1/RN;%牵扯到正负号的问题
A(5,6) = (wie+dot_longitude)*sin(latitude(i));
A(5,7) = -(wie+dot_longitude)*cos(latitude(i));%这里是修改过后的，原来的括号在后面
A(6,1) = wie*sin(latitude(i));
A(6,3) = -1/RE;%牵扯到正负号的问题
A(6,5) = -(wie+dot_longitude)*sin(latitude(i));

A(6,7) = -dot_latitude;%牵扯到正负号的问题
A(7,1) = -wie*cos(latitude(i))-dot_longitude/cos(latitude(i));
A(7,3) = -tan(latitude(i))/RE;
A(7,5) = (wie+dot_longitude)*cos(latitude(i));
A(7,6) = dot_latitude;
FI = expm(caiyangT*A);
B = [0         0         0         0         0         0        ;
     0         0         0         0         0         0        ; 
     0         0         0         Cn_b(1,1) Cn_b(1,2) Cn_b(1,3);
     0         0         0         Cn_b(2,1) Cn_b(2,2) Cn_b(2,3);
     Cn_b(1,1) Cn_b(1,2) Cn_b(1,3) 0         0         0        ;
     Cn_b(2,1) Cn_b(2,2) Cn_b(2,3) 0         0         0        ;
     Cn_b(3,1) Cn_b(3,2) Cn_b(3,3) 0         0         0        ;]; 
gyro_acc_noise = [We_chang We_chang We_chang Fe_chang Fe_chang Fe_chang]';
noise =caiyangT*B*gyro_acc_noise;










X2 = FI*X+ noise;
Store_error(i,:) = X2;
X =X2;
waitbar(i/solve_step,hWaitBar,[num2str( fix(i/solve_step*100) ),'%']);

end
close(hWaitBar);
if trajectory_type==1
save Store_error_line.mat Store_error -ASCII -DOUBLE -MAT;
elseif trajectory_type==2
    save Store_error_line2.mat Store_error -ASCII -DOUBLE -MAT;
elseif trajectory_type==3
    save Store_error_circle.mat Store_error -ASCII -DOUBLE -MAT;
end

figure();plot(Store_error(:,1));title('纬度的误差');
figure();plot(Store_error(:,2));title('经度的误差');
figure();plot(Store_error(:,3));title('东向速度的误差');
figure();plot(Store_error(:,4));title('北向速度的误差');
figure();plot(Store_error(:,5));title('俯仰角的误差');
figure();plot(Store_error(:,6));title('滚动角的误差');
figure();plot(Store_error(:,7));title('偏航角的误差');
% figure();plot(error_location(:,1)-Store_error(:,1))