untitled5.m

根据GPS回传的数据

%飞机的经纬高度，得到飞机在地球固联坐标系下的坐标
j = pi/4; w = 0; h = 0;R = 6.371e6;
r = [-sin(j)   -sin(w)*cos(j)   cos(w)*cos(j);
      cos(j)    -sin(w)*sin(j)   cos(w)*sin(j);
     0          cos(w)           sin(j)];
 
 p = [cos(w)*cos(j) cos(w)*sin(j) sin(w)]'*(R+h);

 p1=[0 0 0]';
 p0 = r*p1+p;

 %将飞机在地球固联坐标系下的坐标变换到天线底座坐标系下
 %天线底座的经纬高度
 t_j = 0; t_w = 0; t_h = 0;
 t_r = [-sin(t_j)  -sin(t_w)*cos(t_j)  cos(t_w)*cos(t_j);
      cos(t_j)   -sin(t_w)*sin(t_j)  cos(t_w)*sin(t_j);
       0           cos(t_w)            sin(t_j)];
 t_p = [cos(t_w)*cos(t_j) cos(t_w)*sin(t_j) sin(t_w)]'*(R+t_h);
 p2 = inv(t_r) *(p0 - t_p);
 
 %将飞机在天线底座坐标系下的坐标变换为极坐标，即天线应该达到的姿态
 %因为只需要判断姿态，所以将飞机的坐标单位化
 unit_p2 = p2/sqrt(p2(1)^2+p2(2)^2+p2(3)^2);
 fy = asin(unit_p2(3))*180/pi  %还有180度角，如何判断象限？？？
 hx = -atan(unit_p2(1)/unit_p2(2))*180/pi