%未加遺忘因子的卡爾曼濾波程序
clear all
A=textread('E:\Rec1.txt','%s','headerlines',1);
 for i=1:30;
n=2*i-1;
A(i)=A(n);
end
b=A(1:30,:);%讀取GPGGA字符串
c=char(b);%轉(zhuǎn)換為字符數(shù)組
weidu=c(:,17:24);%提取緯度數(shù)組
jingdu=c(:,28:36);%提取經(jīng)度數(shù)組
format long
weidu=str2num(weidu)/100;
jingdu=str2num(jingdu)/100;
weizhi=zeros(2,30);
for i=1:30
weizhi(:,i)=gaosi(jingdu(i),weidu(i))';
x=weizhi(1,:);
y=weizhi(2,:);
%s=[s]';
end
figure(1) 
t=1:1:30;
plot(x,y,'+');
axis([842 852 962 972]);
xlabel('xp position');
ylabel('yp position');
title('the track before kalman filtering');
Y=zeros(2,30);
Y1=[y(1),0];
Y(:,1)=Y1;
X=zeros(2,30);
X1=[x(1),0];
X(:,1)=X1;
A=[1 1
    0 1];          
%B=[1/2*(T)^2 T]';
H=[1 0];

C0=[0 0
    0 1];
C=[C0 zeros(2,2*29)];
%Q=(0.001)^2;
Q=(0.001)^2;
R=10; 


% kalman algorithm ieration
for n=1:30
   i=(n-1)*2+1;
   K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
   Y(:,n)=Y(:,n)+K*(weizhi(2,n)-H*Y(:,n));
    Y(:,n+1)=A*Y(:,n);
    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+Q;
 %C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end

% the diagram of position after filtering
X=zeros(2,30);
X1=[x(1),0];
X(:,1)=X1;
A1=[1 1
    0 1];          
%B=[1/2*(T)^2 T]';
H1=[1 0];

C0=[0 0
    0 1];
CC=[C0 zeros(2,2*29)];
%Q=(0.001)^2;
Q1=(0.001)^2;
R1=10; 


% kalman algorithm ieration
for m=1:30
   ii=(m-1)*2+1;
   K=CC(:,ii:ii+1)*H1'*inv(H1*CC(:,ii:ii+1)*H1'+R1);
   X(:,m)=X(:,m)+K*(weizhi(1,m)-H1*X(:,m));
   X(:,m+1)=A1*X(:,m);
    CC(:,ii:ii+1)=(eye(2,2)-K*H1)*CC(:,ii:ii+1);
    CC(:,ii+2:ii+3)=A1*CC(:,ii:ii+1)*A1'+Q1;
 %C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end

figure(2) 
t=0:1:30;
xp=X(1,:);
yp=Y(1,:);
plot(xp,yp,'+');
axis([842 852 962 972]);
xlabel('xp position');
ylabel('yp position');
title('the track after kalman filtering');