www.pudn.com > gps.zip > task_2.m, change:2015-11-25,size:2684b


%数据处理 Kalman滤波 
clear 
clc 
 
 
[GGA,Time,LAT,N,LON,E,STATUS,QUANTITY,HDOP,AltS,M,AltE,m,a] = textread('跑车数据.txt','%s %f %f %s %f %s %f %f %f %f %s %f %s %s','delimiter', ','); %#ok<DTXTRD> 
 
LAT=LAT(LAT~=0); 
LON=LON(LON~=0); 
HDOP=HDOP(HDOP~=0); 
 
Time=Time(Time~=0); 
Time = floor(Time/10000)*3600 + floor((Time - floor(Time/10000)*10000)/100)*60 + Time - floor(Time/100)*100;  %%换算时间 
 
AltS=AltS(AltS~=0); 
 
lat = floor(LAT/100) + (LAT - floor(LAT/100)*100)/60;   %换算纬度 
lon = floor(LON/100) + (LON - floor(LON/100)*100)/60;   %换算经度 
figure; 
plot(lat,lon); 
ylabel('经度'); 
xlabel('纬度'); 
 
Re = 6371393; 
r=Re+AltS; 
 
x = r.*cos(lat/180*pi).*cos(lon/180*pi);  %转换到地理直角坐标系 
y = r.*cos(lat/180*pi).*sin(lon/180*pi); 
z = r.*sin(lat/180*pi); 
 
figure; 
subplot(3,1,1); 
plot(Time,x); 
title('x-t'); 
xlabel('time(s)'); 
ylabel('x_distance(m)'); 
 
subplot(3,1,2); 
plot(Time,y); 
title('y-t'); 
xlabel('time(s)'); 
ylabel('y_distance(m)'); 
 
subplot(3,1,3); 
plot(Time,z); 
title('z-t'); 
xlabel('time(s)'); 
ylabel('z_distance(m)'); 
 
%Kalman filtering 
%利用低动态-定常速度模型 
F=[zeros(3),eye(3);zeros(3),zeros(3)]; 
I=eye(6); 
 
syms s t;           %转移矩阵的离散化 
T=1; 
FS=inv(s*I-F); 
Ft=ilaplace(FS,s,t); 
FT=subs(Ft,t,T); 
phy=FT; 
 
tao=[zeros(3),zeros(3);zeros(3),eye(3)]; 
 
Q=diag([50 50 50 100 100 100]');           %系统误差可调 
 
x1=0;                                   %滤波初值设置 
y1=0; 
z1=0; 
for i=1:11 
    x1=x1+x(i); 
    y1=y1+y(i); 
    z1=z1+z(i); 
end 
x2=0; 
y2=0; 
z2=0; 
for i=12:22 
    x2=x2+x(i); 
    y2=y2+y(i); 
    z2=z2+z(i); 
end 
vx0=(x2-x1)/100; 
vy0=(y2-y1)/100; 
vz0=(z2-z1)/100; 
x0=x(21); 
y0=y(21); 
z0=z(21); 
p_k_1=diag([80 80 120 30 30 45]'); 
p_k_k_1=zeros(6,1); 
X_k_1=[x0;y0;z0;vx0;vy0;vz0]; 
X_k_k_1=zeros(6,1); 
X_k=zeros(6,1); 
Z_k=zeros(3,1); 
x_k=zeros(800,1); 
y_k=zeros(800,1); 
z_k=zeros(800,1); 
 
H_k=[eye(3),zeros(3)]; 
 
for i=22:821 
    Z_k=[x(i);y(i);z(i)]; 
    R_k=diag([12.5*HDOP(i)^2,12.5*HDOP(i)^2,15*HDOP(i)^2]'); 
    X_k_k_1=phy*X_k_1; 
    p_k_k_1=phy*p_k_1*phy'+tao*Q*tao'; 
    K_k=p_k_k_1*H_k'/(H_k*p_k_k_1*H_k'+R_k); 
    p_k=(I-K_k*H_k)*p_k_k_1*(I-K_k*H_k)'+K_k*R_k*K_k'; 
    X_k=X_k_k_1+K_k*(Z_k-H_k*X_k_k_1); 
     
    x_k(i-21)=X_k(1); 
    y_k(i-21)=X_k(2); 
    z_k(i-21)=X_k(3); 
     
    X_k_1=X_k; 
    p_k_1=p_k; 
end 
  
figure; 
subplot(3,1,1); 
plot(Time(22:821),x_k); 
xlabel('t/s'); 
ylabel('x/m'); 
title('x postition - t'); 
 
subplot(3,1,2); 
plot(Time(22:821),y_k); 
xlabel('t/s'); 
ylabel('y/m'); 
title('y postition - t'); 
 
subplot(3,1,3); 
plot(Time(22:821),z_k); 
xlabel('t/s'); 
ylabel('z/m'); 
title('z postition - t');