error_function2.m

惯性导航精解算程序

function error_function2(trajectory_type)
%处理误差方程的函数
% 1~3位置的误差；纬度，经度,高度

% 4~6速度的误差；东向，北向，天向
% 7~9平台失准误差；α，γ，φ
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%地球自转角速度
X = zeros(9,1);
A = zeros(9);
%%
hWaitBar = waitbar(0,'Please wait...');
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)));%经度变化率

dot_highth = 0;
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,3) = -dot_latitude/(RN+highth(i));
A(1,5) = 1/(RN+highth(i));
A(2,1) = dot_longitude*tan(latitude(i));
A(2,3) = -dot_longitude/(RE+highth(i))
A(2,4) = 1/((RE+highth(i))*cos(latitude(i)));
A(3,6) = 1;
A(4,1) = 2*wie*(v_u(i)*sin(latitude(i))+v_n(i)*cos(latitude(i)))+v_n(i)*dot_longitude/cos(latitude(i));
A(4,4) = dot_latitude*tan(latitude(i)+highth(i))*(RN+highth(i))/(RE+highth(i));
A(4,5) = (2*wie+dot_longitude)*sin(latitude(i));
A(4,6) = -(2*wie+dot_longitude)*cos(latitude(i));
A(4,8) = fu(i);
A(4,9) = -fn(i);
A(5,1) = -2*wie*v_e(i)*cos(latitude(i))-v_e(i)*dot_longitude/cos(latitude(i));
A(5,4) = -2*(wie+dot_longitude)*sin(latitude(i));
A(5,5) = -dot_highth/(RN+highth(i));
A(5,6) = -dot_latitude;
A(5,7) = -fu(i);
A(5,9) = fe(i);
A(6,1) = -2*wie*v_e(i)*sin(latitude(i));
A(6,3) = 20/Ra;%??????
A(6,4) = 2*(wie+dot_longitude)*cos(latitude(i));
A(6,5) = 2*dot_latitude;
A(6,7) = fn(i);
A(6,8) = -fe(i);
A(7,3) = -dot_latitude/(RN+highth(i));
A(7,5) = 1/(RN+highth(i));
A(7,8) = (wie+dot_longitude)*sin(latitude(i));

A(7,9) = -(wie+dot_longitude*cos(latitude(i)));
A(8,1) = wie*sin(latitude(i));
A(8,3) = dot_longitude*cos(latitude(i))/(RE+highth(i));
A(8,4) = -1/(RE+highth(i));
A(8,7) = -(wie+dot_longitude)*sin(latitude(i));
A(8,9) = -dot_latitude;
A(9,1) = -wie*cos(latitude(i))-dot_longitude/cos(latitude(i));
A(9,3) = dot_latitude*sin(latitude(i))/(RE+highth(i));
A(9,4) = -tan(latitude(i))/(RE+highth(i));
A(9,7) = (wie+dot_longitude)*cos(latitude(i));
A(9,8) = dot_latitude;
FI = expm(caiyangT*A);



B = [0         0         0         0         0         0        ;
     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);
     0         0         0         Cn_b(3,1) Cn_b(3,2) Cn_b(3,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 = B*gyro_acc_noise;
X = FI*X+ noise;
Store_error(i,:) = X;  

waitbar(i/solve_step,hWaitBar,[num2str( fix(i/solve_step*100) ),'%'])
end
close(hWaitBar);
save Store_error.mat Store_error -ASCII -DOUBLE -MAT;