GPS编程

开发平台：
Visual C++

Kalman.cpp：源码内容
							#include "DR_GPS_EKF.h"
// 本程序实现matrix类
// 对matrix类的定义
#include "matrix.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <iostream.h>
//将GPS和DR数据进行Kalman滤波，参数是：
//dr导航数组指针,gps导航数组指针,ekf导航数组指针,初始经纬度
void EKF(double *pdr,double *pgps,double *pekf,
		 double lon0,double lat0) 
{
    double egps=pgps[0];  //将gps这一时刻的东向位置赋予变量egps
    double ngps=pgps[1];  //将gps这一时刻的北向位置赋予变量ngps
    double edr=pdr[0];    //将dr这一时刻的东向位置赋予变量edr
    double ndr=pdr[1];    //将dr这一时刻的北向位置赋予变量ndr
    double vedr=pdr[2];   //将dr这一时刻的东向速度赋予变量vedr
    double vndr=pdr[3];   //将dr这一时刻的北向速度赋予变量vndr
    double aedr=pdr[4];   //将dr这一时刻的东向加速度赋予变量aedr
    double andr=pdr[5];   //将dr这一时刻的北向加速度赋予变量andr
	double yaw=pdr[6];    //将dr这一时刻的航向角赋予变量yaw
	double dy=pdr[7];     //将dr这一时刻的航向角速率赋予变量dy
	double e,n,lon,lat;
    double detT=10;	      //将A阵离散化的时间
	double t_ae=0.1,t_an=0.1;//车辆东北向机动加速度变化率的相关时间常数
    double val;           //定义中间变量          
	double e_ep,n_ep,v_ep,v_np,a_ep,a_np;//定义中间变量 
	//将下面的向量和矩阵都定义为matrix类的对象
    static matrix X(6,1); //状态向量,定义X阵(XX阵)为matrix类的对象
    static matrix P(6,6); //估计误差方差阵,定义P阵(PP阵)为matrix类的对象
    matrix X1(6,1),Z(3,1);//状态一步预测阵和观测阵
	matrix h_x(3,1);      //观测方程里非线性函数对x求导
	matrix P1(6,6);       //一步预测方差阵
    matrix R(3,3);        //系统观测噪声方差阵
	matrix Q(6,6);        //系统过程噪声方差阵
	matrix A(6,6);        //状态阵
	matrix F(6,6);        //状态阵A的离散化
	matrix H(3,6);        //观测阵
	matrix K(6,3);        //滤波增益阵
	static char define='N';//状态向量和估计误差方差阵的初始定义标识
	if (define=='N')     //若X阵和P阵没有初始化则对其进行初始化
	{
		X=0;
		X.set(0,0,edr);X.set(1,0,ndr);X.set(2,0,vedr);
		X.set(3,0,vndr);X.set(4,0,aedr);X.set(5,0,andr);
		P=0;
		P.set(0,0,0.1);P.set(1,1,0.1);P.set(2,2,0.1*0.1);
		P.set(3,3,0.1*0.1);P.set(4,4,0.01*0.01);P.set(5,5,0.01*0.01);
		define='Y';
		cout<<"X,P的初始化:"<<define<<"n";
	}
	//R,Q,A,H的初始化
	R=0;
	R.set(0,0,10);R.set(1,1,10);R.set(2,2,1);
    Q=0;
	Q.set(0,0,10);Q.set(1,1,10);Q.set(2,2,1);
	Q.set(3,3,1);Q.set(4,4,0.1);Q.set(5,5,0.1);
	A=0;
    A.set(0,2,1);A.set(1,3,1);A.set(2,4,1);	
	A.set(3,5,1);A.set(4,4,-t_ae);A.set(5,5,-t_an);
    H=0;	
 
	F=unit(6)+detT*A+detT*detT*A*A/2; //状态矩阵A的离散化
    //观测向量：GPS东北向位置、角速率陀螺的输出以及dr在一个采样周期的距离
	Z.set(0,0,egps);		//GPS东向位置
    Z.set(1,0,ngps);        //GPS北向位置
    Z.set(2,0,dy);          //DR航向角速率陀螺的输出  
    //Z.set(3,0,s);         //里程计在一个采样周期的距离
	X1=F*X;                 //状态一步预测向量
	//将状态一步预测向量对应值分别赋于中间变量
    e_ep=X1(0,0);           
    n_ep=X1(1,0);
    v_ep=X1(2,0);
    v_np=X1(3,0);
    a_ep=X1(4,0);
    a_np=X1(5,0);
    //用状态一步预测向量计算得到Kalman滤波的观测阵H
	H.set(0,0,1);
	H.set(1,1,1);
	val=(a_np*v_ep*v_ep-2*v_ep*v_np*a_ep-a_np*v_np*v_np)/pow((v_ep*v_ep+v_np*v_np),2);
    H.set(2,2,val);
	val=(a_ep*v_ep*v_ep+2*v_ep*v_np*a_np-a_ep*v_np*v_np)/pow((v_ep*v_ep+v_np*v_np),2);
    H.set(2,3,val);
	val=v_np/(v_ep*v_ep+v_np*v_np);
    H.set(2,4,val);
	val=-v_ep/(v_ep*v_ep+v_np*v_np);
    H.set(2,5,val);
	//val=dt*v_ep/sqrt(v_ep*v_ep+v_np*v_np);
    //H.set(3,2,val);
	//val=dt*v_np/sqrt(v_ep*v_ep+v_np*v_np);
    //H.set(3,3,val);
    P1= F*P*F.t() + Q;            //求一步预测误差方差阵
    K = P1*H.t()*~(H*P1*H.t()+R); //求滤波增益阵
	//h2_xPre是非线性观测矩阵H的雅可比矩阵,
	//其中使用的速度、加速度都是一步预测估计值
    h_x.set(0,0,e_ep);
	h_x.set(1,0,n_ep);
	val=dt*(v_np*a_ep-v_ep*a_np)/(v_ep*v_ep+v_np*v_np);
	h_x.set(2,0,val);
	//val=dt*sqrt(v_ep*v_ep+v_np*v_np);
	//h_x.set(3,0,val);
    X = X1 + K*(Z-h_x);          //状态估计
    P= (unit(6)-K*H)*P1;         //估计误差方差阵
	e=X(0,0);n=X(1,0);           //更新导航数据
	lat=(n/Re+lat0)*180/pi;          //纬度(度)
	lon=(e/Re/cos(lat)+lon0)*180/pi; //经度(度)
	//用Kalman滤波后的导航数据修正DR的导航数据
	//pdr[0]=e;pdr[1]=n;              //东北向位置修正
	//pdr[2]=ve;pdr[3]=vn;            //东北向速修正
    //修改指针pekf的内容(位置,速度,加速度,经纬度,航向角)
	for (int i=0;i<6;i++)
		pekf[i]=X(i,0);             //pekf[]的前6个数与X一样
	pekf[i]=lat;pekf[i+1]=lon;pekf[i+2]=yaw*180/pi;//后三个数为经纬度和航向角
}

						