crafrate.m

来自「惯性导航系统传递对准系统模型程序」· M 代码 · 共 55 行

function rho = crafrate(lat,vx,vy,height,DCMel,earthflg,vertmech)
%该函数的功能是实现计算运载体的旋转角速度向量rho，输入量是当地地理纬度lat、载体速度值、载体高度值、方向余弦矩阵DCMel，以及函数标志数值ea
%rthflg和向量vertmech标志值，输出载体旋转角速度值。
if nargin<7, vertmech = 0; end
if nargin<6,error('insufficient number of input arguments'),end
if (earthflg ~= 0) & (earthflg ~= 1), error('EARTHFLG not specified correctly'),end

if earthflg == 0,
   rm = 6378137;
   rp = rm;
else,
   [rm,rp] = radicurv(lat);%根据earthflg符号判断地球模型的形态：是圆形还是椭球形？如果是圆形，则模型的长短半轴都取为6378137；
   %否则是椭球形，按照radicurv函数计算地球模型的长短半轴半径；
end

if vertmech == 0,
   Ve = vx;%东向速度
   Vn = vy;%北向速度
   rho(1,1) = -Vn/(rm + height);
   rho(2,1) = Ve/(rp + height);
   rho(3,1) = Ve*tan(lat)/(rp + height);%计算运载体旋转角速度
else
   h = height;
   f = 1/298.257223563;
   [b,a] = radicurv(0);
   DCMelYx = DCMel(1,3);
   DCMelYy = DCMel(2,3);
   DCMelYz = DCMel(3,3);

   rho(1,1) = -(vy/a)*(1 - h/a - f*(1-3*DCMelYy*DCMelYy-DCMelYx*DCMelYx)) ...
      - (vx/a)*(2*f*DCMelYx*DCMelYy);
   rho(2,1) = (vx/a)*(1 - h/a - f*(1-3*DCMelYx*DCMelYx-DCMelYy*DCMelYy)) ...
      + (vy/a)*(2*f*DCMelYx*DCMelYy);

   if vertmech == 1,
     rho(3,1) = 0;
   end

   if vertmech == 2,
      OMEGA = 7.292115e-5;   %Earth's rate in rad/s
      rho(3,1) = -OMEGA*DCMelYz;
   end

   if vertmech == 3,
      num = -( rho(2,1)*DCMelYy + rho(1,1)*DCMelYx );
      if lat >= 0,
         den = DCMelYz + 1;
      else
         den = DCMelYz - 1;
      end
      rho(3,1) = num/den;
   end
   
end