velocity_attitude_matching.m

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

function Velocity_Attitude_Matching(f)
clear;
clc;
close all;
type=[  1 1 2 1 1
        0 0 1 6 0];
step.fly=0.01;
step.strapdown=1*2*step.fly;
step.fliter=1*step.strapdown;
initparameter=[0.01*pi/180/3600    0.01*pi/180/3600    0.01*pi/180/3600       
                  (5e-5)*9.8             (5e-5)*9.8         (5e-5)*9.8     
                45.7796*pi/180        126.6705*pi/180           0         
                      20                  20                 0            
                   0*pi/180              0*pi/180           45*pi/180      
                4*60/60*pi/180        5*60/60*pi/180      6*60/60*pi/180  
                     30                      15                 2         
                      0                      0                  0            
                     0                    0                 0 
                 0*pi/180            0*pi/180        0*pi/180]; 
[Algorithm_PM,data,Sensor,FilterPM,time,record]=Init_Algorithm(type,step,initparameter);
n=step.fliter/step.strapdown;
for i = 1:floor((time-Sensor.Delay)/step.fliter)
    for j = 1:n
        [data,FilterPM,record]=transfer_alignment((i-1)*n+j,i,Algorithm_PM,data,Sensor,FilterPM,step,record,n-j+1);
    end
end
DrawError(record,Algorithm_PM);