chzf.m

惯性导航系统中求解航向及姿态角的仿真程序

function hz = chzf(T)

% 本程序由姿态矩阵T计算姿态角和航向角，导航坐标系用NWU系。
% hz=[航向  俯仰角 倾斜角]

espp=1e-5;

if(abs(T(3,1))<=1-espp)
   theta=atan(T(3,1)/sqrt(1-T(3,1).^2));
   if(abs(T(1,1))>espp)
      ksim=atan(-T(2,1)/T(1,1));
      if(T(1,1)>0)
         if(abs(T(2,1))>espp)
            if(-T(2,1)>0)
            ksi=ksim;
            else
            ksi=ksim+2*pi;
            end
         else
            ksi=0;
         end
      else
         ksi=ksim+pi;
      end
   else
      if(-T(2,1)>0)
         ksi=pi/2;
      else
         ksi=3*pi/2;
  		end
	end
	if(abs(T(3,3))>espp)
  		gamam=atan(T(3,2)/T(3,3));
  		if(T(3,3)>0)
    		gama=gamam;
  		else
    		if(abs(T(3,2))>espp)
      		if(T(3,2)>0)
        			gama=gamam+pi;
      		else
        			gama=gamam-pi;
      		end
    		else
      		gama=pi;
    		end
  		end
	else
  		if(T(3,2)>0)
     		gama=pi/2;
  		else
     		gama=-pi/2;
  		end
	end
	hz=[ksi,theta,gama]';  
else
   if T(3,1)>0
      theta=pi/2;    
   else
      theta=-pi/2;
   end
   gama=0;
   if(abs(T(2,2))>espp)
      ksim=atan(T(1,2)/T(2,2));
      if T(2,2)>0
         if(abs(T(1,2))>espp)
            if T(1,2)>0
               ksi=ksim;
            else
               ksi=ksim+2*pi;
            end
         else
            ksi=0;
         end
      else
         ksi=ksim+pi;   
      end
   else
      if T(1,2)>0
         ksi=pi/2;
      else 
         ksi=3/2*pi;
      end
   end
   hz=[ksi theta gama]';
end