xinxijs.m

惯性导航仿真matlab程序

% 惯性器件信号发生器
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
function kk=xinxijs(xinxi,wa)  
         Rr=6378160;   
         e=1/298.3;   
         wie=7.2921151467e-5;   
         g0=9.7803267714;
    
         %通用变量计算
         Rn=Rr*(1-2*e+3*e*sin(xinxi(7))*sin(xinxi(7)));
         Re=Rr*(1+e*sin(xinxi(7))*sin(xinxi(7)));
	
         %姿态增量
     	datt=wa(1:3);
    
     	%速度增量
         g=g0*(1+5.27094e-3*sin(xinxi(7))^2+2.32718e-5*sin(xinxi(7))^4) - 3.086e-6*xinxi(9);
         
         Cnt=[cos(xinxi(3)) -sin(xinxi(3)) 0; 
              sin(xinxi(3)) cos(xinxi(3)) 0;
              0 0 1;]*[cos(xinxi(2)) 0 sin(xinxi(2));
                       0 1 0;
                       -sin(xinxi(2)) 0 cos(xinxi(2));];
	     dvn=Cnt'*wa(4:6)*g;
        Cnt'
	     %位置增量
         
         dpos=[xinxi(4)/(Rn+xinxi(9));xinxi(6)*sec(xinxi(7))/(Re+xinxi(9));xinxi(5);];
    
	     %角增量
         wnie=[wie*cos(xinxi(7));wie*sin(xinxi(7));0];
         wnen=[xinxi(6)/(Re+xinxi(9));xinxi(6)*tan(xinxi(7))/(Re+xinxi(9));-xinxi(4)/(Rn+xinxi(9))];
         wnin=wnie + wnen;
    
         Cbn=[cos(xinxi(3))*cos(xinxi(2)),-sin(xinxi(3))*cos(xinxi(2))*cos(xinxi(1))+sin(xinxi(2))*sin(xinxi(1)),sin(xinxi(3))*cos(xinxi(2))*sin(xinxi(1))+sin(xinxi(2))*cos(xinxi(1));
              sin(xinxi(3)),cos(xinxi(3))*cos(xinxi(1)),-cos(xinxi(3))*sin(xinxi(1));
             -sin(xinxi(2))*cos(xinxi(3)),sin(xinxi(2))*sin(xinxi(3))*cos(xinxi(1))+cos(xinxi(2))*sin(xinxi(1)),-sin(xinxi(3))*sin(xinxi(2))*sin(xinxi(1))+cos(xinxi(2))*cos(xinxi(1));];
     
         Cc=[1 -sin(xinxi(3)) 0;
             0 cos(xinxi(1))*cos(xinxi(3)) -sin(xinxi(1));
             0 cos(xinxi(3))*sin(xinxi(1)) cos(xinxi(1));];

         wbnb=Cc*wa(1:3);
     	 dwm=Cbn'*wnin+wbnb;
	
         %比力增量
         
	     gn=[0;-g;0];
       	 dvm=Cbn'*(dvn+cross(2*wnie+wnen,xinxi(4:6))+gn);
    
         %xinxi增量
         kk=[datt;dvn;dpos;dwm;dvm/g];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%