Category

Development Platform
Home > SourceCode/Document > Communication-Mobile > GPS develop > View Code
GPS.CPP
Package: KalmanGPSDR.rar　[view]
Upload User: wyp_nj
Upload Date: 2022-05-03
Package Size: 484k
Code Size: 1k
Category:

GPS develop

Development Platform:

Visual C++

GPS.CPP：Code Content
  1. #include "DR_GPS_EKF.h"
  2. #include <math.h>
  3. //将GPS采集的经纬度和速度转化为东北向位置和速度，参数是：
  4. //存放导航数据的数组gps，GPS采得的东北向速度，经纬度和初始经纬度
  5. void GPS(double *pgps,double ve,double vn,
  6.  double lon,double lat,double lon0,double lat0)
  7. {
  8.     double e,n;               //东北向位置变量
  9. lon=lon/180*pi;           //将角度转化成弧度
  10. lat=lat/180*pi;           //将角度转化成弧度
  11. e=(lon-lon0)*Re*cos(lat); //东向位置
  12. n=(lat-lat0)*Re;          //北向位置
  13.     //修改gps导航数组pgps的内容
  14. pgps[0]=e;                //修改东向位置
  15. pgps[1]=n;                //修改北向位置
  16. pgps[2]=ve;               //修改东向速度
  17. pgps[3]=vn;               //修改北向速度
  18. pgps[4]=lon;              //修改经度
  19. pgps[5]=lat;              //修改纬度
  20. }