GPS编程

开发平台：

Visual C++

GPS.CPP：源码内容

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

						