firstduizhun.m

惯性导航粗对准

%此程序为自编捷联惯导粗对准程序，中间所用数据为真实惯导所采数据，包括陀螺和加速度计数据，经验证粗对准后精度可以控制在10角秒以内。 

%设置为北东天
g = [-9.7803 0 0];
g = g*0.01;

pi = 3.1415926535;

%地球自传（度/S）
w = [-0.0021205375753134  0   0.00359995843475473 ];

%加速度计数据
acce= [-0.010069194531807  0.0000053911800750311  0.000028869456467498];

acce = acce*9.7803;
%陀螺数据
gero = [0.001951023828   0.047506461762  0.065351831805];

gero = gero;

R = cross(g,w);

T1 = [g;w;R];

Z = cross(acce,gero);

T2 = [acce;gero;Z];

T1=inv(T1);

T=T1*T2;
%初次获得姿态角
position_angle(1,:)=[atan(-T(2,3)/T(3,3)),asin(-T(1,3)),atan(-T(1,2)/T(1,1))]*180/pi

Begin_Y = position_angle(1,1);
Begin_P = position_angle(1,2);
Begin_R = position_angle(1,3);

%构建四元数
    Q0(1,1)=  cos(Begin_Y/2/57.3)*cos(Begin_P/2/57.3)*cos(Begin_R/2/57.3)+sin(Begin_Y/2/57.3)*sin(Begin_P/2/57.3)*sin(Begin_R/2/57.3);
    Q0(1,2)=  cos(Begin_R/2/57.3)*cos(Begin_P/2/57.3)*sin(Begin_Y/2/57.3)-sin(Begin_R/2/57.3)*sin(Begin_P/2/57.3)*cos(Begin_Y/2/57.3);
    Q0(1,3)= -cos(Begin_Y/2/57.3)*sin(Begin_P/2/57.3)*cos(Begin_R/2/57.3)-sin(Begin_Y/2/57.3)*cos(Begin_P/2/57.3)*sin(Begin_R/2/57.3);
    Q0(1,4)=  sin(Begin_R/2/57.3)*cos(Begin_P/2/57.3)*cos(Begin_Y/2/57.3)-cos(Begin_R/2/57.3)*sin(Begin_P/2/57.3)*sin(Begin_Y/2/57.3);
    
%获取粗对准姿态矩阵
   TT  = {
			 (1-2*Q0(1,3)*Q0(1,3)-2*Q0(1,4)*Q0(1,4)),2*(Q0(1,2)*Q0(1,3)-Q0(1,1)*Q0(1,4))  ,  2*(Q0(1,2)*Q0(1,4)+Q0(1,1)*Q0(1,3)),
			  2*(Q0(1,2)*Q0(1,3)+Q0(1,1)*Q0(1,4)),    (1-2*Q0(1,2)*Q0(1,2)-2*Q0(1,4)*Q0(1,4)),2*(Q0(1,3)*Q0(1,4)-Q0(1,1)*Q0(1,2)),
			  2*(Q0(1,2)*Q0(1,4)-Q0(1,1)*Q0(1,3)),    2*(Q0(1,3)*Q0(1,4)+Q0(1,1)*Q0(1,2))  ,  (1-2*Q0(1,2)*Q0(1,2)-2*Q0(1,3)*Q0(1,3))
              
          };