GPS编程

开发平台：

Visual C++

DR.CPP：源码内容

							#include "DR_GPS_EKF.h"
#include <math.h>
//将DR采集的x，y轴上的加速度和航向角速率转化为东北向位置和速度
//参数是：存放导航数据的数组dr，DR采的x，y轴加速度，航向角速率
void DR(double *pdr,double ax,double ay,double dy)
{   
	double edr,ndr;     //DR的位置
    double vedr,vndr;   //DR的速度
	double aedr,andr;   //DR在东北向的加速度
	double yaw;         //DR的航向角
	//DR的速度和位置递增
    dy=dy/180*pi;       //将角度转化成弧度
	yaw=pdr[6];         //将前一时刻的航向角值赋给变量yaw
	yaw=yaw+dy*dt;      //航向角递增
	aedr=ax*sin(yaw)+ay*cos(yaw);//东向加速度递增
	andr=ax*cos(yaw)-ay*sin(yaw);//北向加速度递增
	vedr=pdr[2];        //将前一时刻的东向速度值赋给变量vedr
	vndr=pdr[3];        //将前一时刻的北向速度值赋给变量vndr
	vedr=vedr+aedr*dt;  //东向速度递增
	vndr=vndr+andr*dt;  //北向速度递增
	edr=pdr[0];         //将前一时刻的东向位置值赋给变量edr
	ndr=pdr[1];         //将前一时刻的北向位置值赋给变量ndr
	edr=edr+vedr*dt;    //东向位置递增
	ndr=ndr+vndr*dt;    //北向位置递增
	//修改数组dr的内容(角度)
	pdr[0]=edr;         //东向位置
	pdr[1]=ndr;         //北向位置
	pdr[2]=vedr;        //东向速度
	pdr[3]=vndr;        //北向速度
    pdr[4]=aedr;        //东向加速度
	pdr[5]=andr;        //北向加速度
	pdr[6]=yaw;         //航向角
	pdr[7]=dy;
    
}

						