single6.m

标准的卡尔曼滤波MATLAB程序。。。简单实用的

%未加遗忘因子的卡尔曼滤波程序
clear all
A=textread('H:\new\20071023.txt','%s','headerlines',1);
b=A(1:390,:);%读取GPGGA字符串
c=char(b);%转换为字符数组
weidu=c(:,15:23);%提取纬度数组
jingdu=c(:,27:36);%提取经度数组
format long
weidu=str2num(weidu)/100;
jingdu=str2num(jingdu)/100;
weizhi=zeros(2,390);
for i=1:390
weizhi(:,i)=gaosi(jingdu(i),weidu(i))';
x=weizhi(1,:);
y=weizhi(2,:);
%s=[s]';
end
figure(1) 
%subplot(2,2,2);
t=1:1:390;
plot(x,y,':');
%axis([842 852 962 972]);
%xlabel('xp position');
%ylabel('yp position');
%title('蓝色+原始数据点');
hold on
Y=zeros(2,390);
Y1=[y(1),0];
Y(:,1)=Y1;
X=zeros(2,390);
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*389)];
%Q=(0.001)^2;
Q=(0.01)^2;
R=10; 


% kalman algorithm ieration
for n=1:390
   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);
    %Y(:,n+1)=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,390);
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*389)];
%Q=(0.001)^2;
Q1=(0.01)^2;
R1=10; 


% kalman algorithm ieration
for m=1:389
   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);
   %X(:,m+1)=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=1:1:150;
xp=X(1,:);
yp=Y(1,1:390);
plot(xp,yp,'-');
%axis([842 852 962 972]);
xlabel('xp position');
ylabel('yp position');
title('标准卡尔曼滤波后的载体动态轨迹仿真图');