Initial-alignment.rar_INS_Initial Alignment_alignment_捷联惯导、

clear all; clc; g0 = 9.78049; Rad_Deg = 0.01745329; wie = 7.27220417e-5; Re = 6378393.0; e = 3.3670e-3; Hn=0.1; tem=10; Kgx=0.0001; Kgy=0.0001; Kgz=0.0001; Kax=0.0001; Kay=0.0001; Kaz=0.0001; dvx=0; dvy=0; tempx=0; tempy=0; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> G_Drift=[0.01*Rad_Deg/3600; 0.01*Rad_Deg/3600; 0.01*Rad_Deg/3600]; A_Bias=[1e-4*g0; 1e-4*g0; 1e-4*g0]; Kg=[1+Kgx 0 0; 0 1+Kgy 0 0 0 1+Kgz]; Ka=[1+Kax 0 0; 0 1+Kay 0; 0 0 1+Kaz]; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> T_p=8; T_r=10; T_y=6; %the magnify pitchM = 12*Rad_Deg; rollM = 15*Rad_Deg; yawM = 10*Rad_Deg; %the initial phase pitch0 = 30*Rad_Deg; roll0 = 30*Rad_Deg; yaw0 = 30*Rad_Deg; %the initial value pitchK = 0*Rad_Deg; rollK = 0*Rad_Deg; yawK = 30*Rad_Deg; %the initial error angle pitch_error = 1*Rad_Deg; roll_error = 1*Rad_Deg; yaw_error = 3*Rad_Deg; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> TimeS1 = 100; TimeS2 = 120; TimeS3 = 200; % n对应于MINS所在的导航坐标系 lati=45.7796*Rad_Deg;%>>>latitude纬度>>> longi=126.6705*Rad_Deg;%>>>>经度longitude>>> wien=[0; wie*cos(lati); wie*sin(lati)]; % p对应于SINS所在的导航坐标系 phi=45.7796*Rad_Deg; lamda=126.6705*Rad_Deg; g1=g0+0.051799*sin(phi)*sin(phi); wiep=[0; wie*cos(phi); wie*sin(phi)]; %>>>>>>>>>>>>>初真实姿>>>>>>>>>>>>>>>>> pitchTT = pitchM*sin(pitch0)+pitchK; rollTT = rollM*sin(roll0)+rollK; yawTT = yawM*sin(yaw0)+yawK; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> sea_mile = 1.852e3/3600; a0 = (7-4)*sea_mile/TimeS3;% 4 is the initial velocity aold=[a0*sin(yawTT); a0*cos(yawTT); 0.0]; v0 = [4.0*sea_mile*sin(yawTT); 4.0*sea_mile*cos(yawTT)]; v = v0; Rxp = Re*(1+e*sin(phi)*sin(phi)); Ryp = Re*(1-2*e+3*e*sin(phi)*sin(phi)); wepp = [-v(2)/Ryp;%>>>>平台相对于地球的角速率>>> v(1)/Rxp; v(1)*tan(phi)/Rxp]; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> q = [1 0 0 0]'; T = eye(3); %>>>>>>>>>>>>>>>>>>>>>>>>>Tbn>>>>>>>>>>方位角定义北偏东为正 Tbn(1,1) = cos(rollTT)*cos(yawTT) + sin(rollTT)*sin(pitchTT)*sin(yawTT); Tbn(1,2) = cos(pitchTT)*sin(yawTT); Tbn(1,3) = sin(rollTT)*cos(yawTT) - cos(rollTT)*sin(pitchTT)*sin(yawTT); Tbn(2,1) = -cos(rollTT)*sin(yawTT) + sin(rollTT)*sin(pitchTT)*cos(yawTT); Tbn(2,2) = cos(pitchTT)*cos(yawTT); Tbn(2,3) = -sin(rollTT)*sin(yawTT) - cos(rollTT)*sin(pitchTT)*cos(yawTT); Tbn(3,1) = -sin(rollTT)*cos(pitchTT); Tbn(3,2) = sin(pitchTT); Tbn(3,3) = cos(rollTT)*cos(pitchTT); %>>>>>>>>>>>>>>>>>include the initial error angles>>>>>>>>>>>>>>>>>>>>>>>>>> T(1,1) = cos(rollTT+roll_error)*cos(yawTT+yaw_error) + sin(rollTT+roll_error)*sin(pitchTT+pitch_error)*sin(yawTT+yaw_error); T(1,2) = cos(pitchTT+pitch_error)*sin(yawTT+yaw_error); T(1,3) = sin(rollTT+roll_error)*cos(yawTT+yaw_error) - cos(rollTT+roll_error)*sin(pitchTT+pitch_error)*sin(yawTT+yaw_error); T(2,1) = -cos(rollTT+roll_error)*sin(yawTT+yaw_error) + sin(rollTT+roll_error)*sin(pitchTT+pitch_error)*cos(yawTT+yaw_error); T(2,2) = cos(pitchTT+pitch_error)*cos(yawTT+yaw_error); T(2,3) = -sin(rollTT+roll_error)*sin(yawTT+yaw_error) - cos(rollTT+roll_error)*sin(pitchTT+pitch_error)*cos(yawTT+yaw_error); T(3,1) = -sin(rollTT+roll_error)*cos(pitchTT+pitch_error); T(3,2) = sin(pitchTT+pitch_error); T(3,3) = cos(rollTT+roll_error)*cos(pitchTT+pitch_error); %>>>>>>>>>>>>>>>>>>>>>>>the initial quaternion>>>>>>>>>>>>>>>>>>>>>>> q(1) = sqrt(1+T(1,1)+T(2,2)+T(3,3))/2.0; q(2) = (T(3,2)-T(2,3))/(4*q(1)); q(3) = (T(1,3)-T(3,1))/(4*q(1)); q(4) = (T(2,1)-T(1,2))/(4*q(1)); %>>>>>>>>>>>>>>>>>>>>>>> TT=T*inv(Tbn); dp(1) = TT(2,3)/Rad_Deg; dr(1) = TT(3,1)/Rad_Deg; dy(1) = TT(1,2)/Rad_Deg; Tn=(TimeS1+TimeS2+TimeS3+50)/Hn; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> %>>>>>>>>>>>>>>>>>>>>>>>>>>updating attitude matrix>>>>>>>>>>>> for k=1:Tn for i=1:3 pitchT(i) = pitchM*sin(2*pi*(k-(3-i)/2)*Hn/T_p+pitch0)+pitchK; rollT(i) = rollM*sin(2*pi*(k-(3-i)/2)*Hn/T_r+roll0)+rollK; yawT(i) = yawM*sin(2*pi*(k-(3-i)/2)*Hn/T_y+yaw0)+yawK; wpT(i) = 2*pi/T_p*pitchM*cos(2*pi*(k-(3-i)/2)*Hn/T_p+pitch0); wrT(i) = 2*pi/T_r*rollM*cos(2*pi*(k-(3-i)/2)*Hn/T_r+roll0); wyT(i) = 2*pi/T_y*yawM*cos(2*pi*(k-(3-i)/2)*Hn/T_y+yaw0); end Tbn(1,1) = cos(rollT(3))*cos(yawT(3)) + sin(rollT(3))*sin(pitchT(3))*sin(yawT(3)); Tbn(1,2) = cos(pitchT(3))*sin(yawT(3)); Tbn(1,3) = sin(rollT(3))*cos(yawT(3)) - cos(rollT(3))*sin(pitchT(3))*sin(yawT(3)); Tbn(2,1) = -cos(rollT(3))*sin(yawT(3)) + sin(rollT(3))*sin(pitchT(3))*cos(yawT(3)); Tbn(2,2) = cos(pitchT(3))*cos(yawT(3)); Tbn(2,3) = -sin(rollT(3))*sin(yawT(3)) - cos(rollT(3))*sin(pitchT(3))*cos(yawT(3)); Tbn(3,1) = -sin(rollT(3))*cos(pitchT(3)); Tbn(3,2) = sin(pitchT(3)); Tbn(3,3) = cos(rollT(3))*cos(pitchT(3)); % >>>>>>>>>>>>> for i=1:3 wnbb(1,i) = sin(rollT(i))*cos(pitchT(i))*wyT(i) + cos(rollT(i))*wpT(i); wnbb(2,i) = -sin(pitchT(i))*wyT(i) + wrT(i); wnbb(3,i) = -cos(rollT(i))*cos(pitchT(i))*wyT(i) + sin(rollT(i))*wpT(i); end wnbb1=[wnbb(1,1); wnbb(2,1); wnbb(3,1)]; wnbb2=[wnbb(1,2); wnbb(2,2); wnbb(3,2)]; wnbb3=[wnbb(1,3); wnbb(2,3); wnbb(3,3)]; % >>>>>>>>>>>>>>>>>>>>>>> Rxt = Re*(1+e*sin(lati)*sin(lati)); Ryt = Re*(1-2*e+3*e*sin(lati)*sin(lati)); % >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> if (k>(TimeS1+TimeS2)/Hn) &(k<=(TimeS1+TimeS2+TimeS3)/Hn) a = aold; else a = [0.0; 0.0; 0.0]; end %>>>>>>>>>>MINS velocity update>>>>>>>>>>>>>>>>>> v00=v0; v0(1) = v0(1)+a(1)*Hn; v0(2) = v0(2)+a(2)*Hn; wenn(1,1) = -v0(2)/Ryt; wenn(2,1) = v0(1)/Rxt; wenn(3,1) = v0(1)*tan(lati)/Rxt; winn=wien+wenn; %>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> longi= longi+(v00(1)+v0(1))/2*Hn/(Rxt*cos(lati)); lati = lati+(v00(2)+v0(2))/2*Hn/Ryt; wien=[0; wie*cos(lati); wie*sin(lati)]; %>>>>>>>>>Tnb>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> for i=1:3 Tnb11(i) = cos(rollT(i))*cos(yawT(i)) + sin(rollT(i))*sin(pitchT(i))*sin(yawT(i)); Tnb12(i) = -cos(rollT(i))*sin(yawT(i)) + sin(rollT(i))*sin(pitchT(i))*cos(yawT(i)); Tnb13(i) = -sin(rollT(i))*cos(pitchT(i)); Tnb21(i) = cos(pitchT(i))*sin(yawT(i)); Tnb22(i) = cos(pitchT(i))*cos(yawT(i)); Tnb23(i) = sin(pitchT(i)); Tnb31(i) = sin(rollT(i))*cos(yawT(i)) - cos(rollT(i))*sin(pitchT(i))*sin(yawT(i)); Tnb32(i) = -sin(rollT(i))*sin(yawT(i)) - cos(rollT(i))*sin(pitchT(i))*cos(yawT(i)); Tnb33(i) = cos(rollT(i))*cos(pitchT(i)); end Tnb1=[Tnb11(1) Tnb12(1) Tnb13(1); Tnb21(1) Tnb22(1) Tnb23(1); Tnb31(1) Tnb32(1) Tnb33(1)]; Tnb2=[Tnb11(2) Tnb12(2) Tnb13(2); Tnb21(2) Tnb22(2) Tnb23(2); Tnb31(2) Tnb32(2) Tn