GPS编程

开发平台：
Visual C++

GPSDRzuhe.m：源码内容
							%GPSDRzuhe.m   <Kalman滤波理论及其在导航系统中的应用>
clear all;
clc;
format long ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%555
%采集数据
Re=6367650; 
g=9.81;               %重力加速度
dt=0.02;              %采样时间0.02秒 
lon0=116.06666667/180*pi;
lat0=39.15/180*pi;
%yaw=168.8/180*pi;    %磁罗盘量测得到航向角初始值，有误差
gps=load('E:hejKalmangps.txt');%6000组数据
dr=load('E:hejKalmandr.txt');
    dy=dr(1,1)/180*pi;%将角度转化成弧度
    yaw=dy*dt;   %航向角递增
    ax=dr(1,2)*g;
    ay=dr(1,3)*g;
    aeDR=ax*sin(yaw)+ay*cos(yaw);%东向加速度递增
    anDR=ax*cos(yaw)-ay*sin(yaw);%北向加速度递增
    veDR=aeDR*dt; %东向速度递增
    vnDR=anDR*dt; %北向速度递增
    eDR=veDR*dt;  %东向位置递增
    nDR=vnDR*dt;  %北向位置递增
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%模型 卡尔曼滤波方程
%状态变量为东向位置，北向位置，东向速度，北向速度，东向加速度，北向加速度
time=74;
detT=10;			
t_ae=0.1;           
t_an=0.1;
x=[eDR nDR veDR vnDR aeDR anDR]';%给定初始值
P=diag([0.1 0.1 0.1^2 0.1^2 0.01^2 0.01^2]);%给定初始值
Q=diag([10 10 1 1 0.1 0.1]);
R=diag([10 10 1]);
eDR=0;nDR=0;veDR=0;vnDR=0;aeDR=0;anDR=0;yaw=0;
for i=1:6000
    dy=dr(i,1)/180*pi;%将角度转化成弧度
    ax=dr(i,2)*g;
    ay=dr(i,3)*g;
    yaw=yaw+dy*dt;   %航向角递增
    aeDR=ax*sin(yaw)+ay*cos(yaw);%东向加速度递增
    anDR=ax*cos(yaw)-ay*sin(yaw);%北向加速度递增
    veDR=veDR+aeDR*dt; %东向速度递增
    vnDR=vnDR+anDR*dt; %北向速度递增
    eDR=eDR+veDR*dt;   %东向位置递增
    nDR=nDR+vnDR*dt;   %北向位置递增
    
    lon=gps(i,1)/180*pi;
    lat=gps(i,2)/180*pi;
    veGPS=gps(i,3);
    vnGPS=gps(i,4);
    eGPS=(lon-lon0)*Re*cos(lat);
    nGPS=(lat-lat0)*Re;
   
A=[0 0 1 0 0 0; 
    0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1;
    0 0 0 0 -t_ae 0;
    0 0 0 0 0 -t_an];
F=eye(6)+detT*A+detT*detT*A*A/2;
  Z_k(1)=eGPS;		%GPS观测量e,n
  Z_k(2)=nGPS;
  Z_k(3)=dy; %DR观测量角度		
  %Z_k(4)=s; %里程计在一个采样周期的距离
  %Z_k=[Z_k(1) Z_k(2) Z_k(3) Z_k(4)]'; 
  Z_k=[Z_k(1) Z_k(2) Z_k(3)]';
   x_1=F*x;
   P_1= F*P*F' + Q;
     
   e_ep=x_1(1);
   n_ep=x_1(2);
   v_ep=x_1(3);
   v_np=x_1(4);
   a_ep=x_1(5);
   a_np=x_1(6);
   
   h33=(a_np*v_ep^2-2*v_ep*v_np*a_ep-a_np*v_np^2)/(v_ep^2+v_np^2)^2;
   h34=(a_ep*v_ep^2+2*v_ep*v_np*a_np-a_ep*v_np^2)/(v_ep^2+v_np^2)^2;
   h35=v_np/(v_ep^2+v_np^2);
   h36=-v_ep/(v_ep^2+v_np^2);
   %h43=dt*v_ep/(v_ep^2+v_np^2)^0.5;
   %h44=dt*v_np/(v_ep^2+v_np^2)^0.5;
   
   H=[1 0 0 0 0 0;
       0 1 0 0 0 0;
       0 0 h33 h34 h35 h36];
   %   0 0 h43 h44 0 0];%DR的观测方程
    
   K = P_1*H'*inv(H*P_1*H'+R);
%h2_xPre是观测方程的离散化形式，其中使用的速度、加速度都是一步预测计算值
    %ATAN(X) is the arctangent of the elements of X
   h_xPre=[e_ep;n_ep;
           dt*(v_np*a_ep-v_ep*a_np)/(v_ep^2+v_np^2)];% dt*(v_ep^2+v_np^2)^0.5];
   x = x_1 + K*(Z_k-h_xPre);
   P= (eye(6)-K*H)*P_1;     
   
  edr(i)=eDR;ndr(i)=nDR;vedr(i)=veDR;vndr(i)=vnDR;
  egps(i)=eGPS;ngps(i)=nGPS;vegps(i)=veGPS;vngps(i)=vnGPS;
  eekf(i)=x(1);  nekf(i)=x(2);  veekf(i)=x(3);  vnekf(i)=x(4);
  y(i)=yaw*180/pi;
end
i=i/50;     
j=0.02:0.02:i;      
figure
plot(j,edr,'b',j,egps,'r',j,eekf,'y'),xlabel('/t'),ylabel('东向位置');
legend('edr','egps','eKalman');
figure 
plot(j,ndr,'b',j,ngps,'r',j,nekf,'y');,xlabel('/t'),ylabel('北向位置');
legend('ndr','ngps','nKalman');
figure 
plot(j,vedr,'b',j,vegps,'r',j,veekf,'y');,xlabel('/t'),ylabel('东向速度');
legend('vedr','vegps','veKalman');
figure 
plot(j,vndr,'b',j,vngps,'r',j,vnekf,'y');,xlabel('/t'),ylabel('北向速度');
legend('vndr','vngps','vnKalman');
figure 
plot(j,y,'b'),xlabel('/t'),ylabel('航向角');

						