simu_gps.m

来自「南航写的gps仿真器源码」· M 代码 · 共 38 行

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                   GPS仿真输出
%
%  输入参数:t-仿真时间；
%  posi       航迹发生器对应的经度、纬度、高度（单位：度、度、米）
%  atti       航迹发生器对应的横滚、俯仰、航向（单位：度）
%  veloB      航迹发生器对应的飞机运动速度——X右翼、Y机头、Z天向（单位：米/秒）
%  输出参数：
%           posiG-GPS输出的飞行器位置（经度（度）、纬度（度）、高度（米））； 
%           veloG-GPS输出的飞行器速度（东向（米/秒），北向（米/秒），天向（米/秒））
%
%                           程序设计：熊智  日期：2003/9/29
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [posiG,veloG]=simu_gps(t,posi,atti,veloB)

  Re=6378137.0;                                      %地球半径（米） 
  f=1/298.257;                                        %地球的椭圆率
  Wie=7.292115147e-5;                          %地球自转角速度
  g=9.7803698;                                      %重力加速度

  long=posi(1,1)*pi/180.0;
  lati=posi(2,1)*pi/180.0;
  heig=posi(3,1);
    %飞行器位置

  %地球曲率半径求解
  Rm=Re*(1-2*f+3*f*sin(lati)*sin(lati));
  Rn=Re*(1+f*sin(lati)*sin(lati));
   
  Err = [20*randn(1,1)/(Rn+heig)/cos(lati);20*randn(1,1)/(Rm+heig);50*randn(1,1)];
  posiG = posi + Err;
 
  Err = [0.2*randn(1,1);0.2*randn(1,1);0.2*randn(1,1)];   
  [veloG]=veloN0(atti,veloB)+Err;
     %计算速度