init_pm01.m

惯性导航系统传递对准系统模型程序

function [Algorithm_PM,data,Sensor,FilterPM,time,record]=Init_PM01(Algorithm_PM,step,initparameter)

global Re e Deg_Rad wie;
g0 = 9.8;
Re = 6378393.0;
e = 3.367e-3;
Deg_Rad = pi/180.0;
wie = 7.2921158e-5;

G_drift     = initparameter(1,:)';
A_bias      = initparameter(2,:)';
initLLH     = initparameter(3,:)';
initVel     = initparameter(4,:)';
initEuler   = initparameter(5,:)';
initMSEuler = initparameter(6,:)';
initLevel   = initparameter(7,:)';
initDelay   = initparameter(8,1);
initVe      = initparameter(9,:)';
initAtte    = initparameter(10,:)';

FilterPM=Init_Kalman(Algorithm_PM,G_drift,A_bias);

initENU = [0;0;0];
initDcm = eulr_dcm(initEuler); 
segparam = fly_type(Algorithm_PM.fly_type,step.fly);
profile = progen(initENU,initVel,initDcm,segparam);
orgxyz = llh2xyz(initLLH);
for i = 1:size(profile,1)
    xyz = enu2xyz(profile(i,1:3),orgxyz);
    profile(i,1:3) = xyz2llh(xyz);
end

profile(:,6)=0*profile(:,6);
record_MS=RawIMU01(profile,zeros(6,1),eye(3),zeros(length(profile(:,1)),3),zeros(3,1));

len=size(record_MS,1);
tim=ceil(step.strapdown/step.fly/2);
order=1:tim:len;

Sensor.fly_path=profile(order,:);
Sensor.fly_v=profile(order,4:6);

seg_time=record_MS(:,1);
Sensor.M_wib=record_MS(order,2:4);
Sensor.M_fib=record_MS(order,5:7);
Sensor.M_attitude=record_MS(order,8:10);
Sensor.M_Tbn=record_MS(order,19:27);

record_MS=RawIMU01(profile,[G_drift;A_bias],eulr_dcm(initMSEuler),Sensor.M_wib,initLevel);
Sensor.S_wib=record_MS(order,2:4);
Sensor.S_fib=record_MS(order,5:7);
Sensor.S_attitude=record_MS(order,8:10);
Sensor.S_Tbn=record_MS(order,19:27);

v0=Sensor.fly_v(1,:)'+initVe;
phi0=Sensor.fly_path(1,1);
lamda0=Sensor.fly_path(1,2);
attitude0=Sensor.M_attitude(1,:)';
Tbn0=[Sensor.M_Tbn(1,1:3);Sensor.M_Tbn(1,4:6);Sensor.M_Tbn(1,7:9)];
data=Initial_data(attitude0,Tbn0,v0,phi0,lamda0);

time=floor((size(Sensor.S_wib,1)-1)/2)*step.strapdown;
record=0;

Sensor.Level=initLevel;
Sensor.Delay=initDelay;
Sensor.MeasureVe = initVe;
Sensor.MeasureAtte = initAtte;