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');
```