main.m

一个kalman滤波的仿真程序

hold on;
M=50;
for i=1:M;
%直线部分1
%直线部分理想轨迹生成
Nline=34;%数据点数
XLreal=-19400:600:400;
YLreal=ones(1,Nline)*4500;
%偏差部分生成
mu=0;
sigma=100;
Wx=normrnd(mu,sigma,1,Nline);
Wy=normrnd(mu,sigma,1,Nline);
%观测数据生成
Xobs=XLreal+Wx;
Yobs=YLreal+Wy;
Xn=[Xobs;Yobs];%观测数据应为实际测得.
Xobs0=-20000+normrnd(mu,100,1,1);
Yobs0=4500+normrnd(mu,100,1,1);
startstate=[Xobs0,Yobs0,300,0].';
statelineend1=linepart(startstate,Nline,Xn);
%画观测曲线与真实曲线
labelx=[-20000,Xn(1,:)];
labely=[4500,Xn(2,:)];
plot(labelx,labely,'r-');
plot(XLreal,YLreal,'g-');
Xrem=Xn(1,Nline);
Yrem=Xn(2,Nline);%保留观测值末状态,供下面画图用.
XL1temperror=XLreal-statelineend1(1,:);%滤波误差数组
YL1temperror=YLreal-statelineend1(2,:);
XL1estierror=XLreal-Xobs;
YL1estierror=YLreal-Yobs;

%圆周部分1
%圆周部分理想轨迹生成
Ncircle=12;%数据点数
n=1:Ncircle;
XCreal=sin(n*2/15)*4500;
YCreal=cos(n*2/15)*4500;
%偏差部分生成
mu=0;
sigma=100;
Wx=normrnd(mu,sigma,1,Ncircle);
Wy=normrnd(mu,sigma,1,Ncircle);
%观测数据生成
Xobs=XCreal+Wx;
Yobs=YCreal+Wy;
Xn=[Xobs;Yobs];
startstate=[statelineend1(:,Nline).',-13,-15].';
statecircleend1=circlepart(startstate,Ncircle,Xn);
%画观测曲线与真实曲线.
labelx=[Xrem,Xn(1,:)];
labely=[Yrem,Xn(2,:)];
plot(labelx,labely,'r-');
plot(XCreal,YCreal,'g-');
Xrem=Xn(1,Ncircle);
Yrem=Xn(2,Ncircle);%保留观测值末状态,供下面画图用.
XC1temperror=XCreal-statecircleend1(1,:);%滤波误差数组
YC1temperror=YCreal-statecircleend1(2,:);
XC1estierror=XCreal-Xobs;
YC1estierror=YCreal-Yobs;

%圆周部分2
%圆周部分理想轨迹生成
Ncircle=12;%数据点数
n=(Ncircle+1):24;
XCreal=sin(n*2/15)*4500;
YCreal=cos(n*2/15)*4500;
%偏差部分生成
mu=0;
sigma=100;
Wx=normrnd(mu,sigma,1,Ncircle);
Wy=normrnd(mu,sigma,1,Ncircle);
%观测数据生成
Xobs=XCreal+Wx;
Yobs=YCreal+Wy;
Xn=[Xobs;Yobs];
startstate=[statecircleend1(1:4,Ncircle).',-13,15].';
statecircleend2=circlepart(startstate,Ncircle,Xn);
%画观测曲线与真实曲线.
labelx=[Xrem,Xn(1,:)];
labely=[Yrem,Xn(2,:)];
plot(labelx,labely,'r-');
plot(XCreal,YCreal,'g-');
Xrem=Xn(1,Ncircle);
Yrem=Xn(2,Ncircle);%保留观测值末状态,供下面画图用.
XC2temperror=XCreal-statecircleend2(1,:);%滤波误差数组
YC2temperror=YCreal-statecircleend2(2,:);
XC2estierror=XCreal-Xobs;
YC2estierror=YCreal-Yobs;


%直线部分2
%直线部分理想轨迹生成
Nline=34;%数据点数
XLreal=-200:-600:-20000;
YLreal=ones(1,Nline)*(-4500);
%偏差部分生成
mu=0;
sigma=100;
Wx=normrnd(mu,sigma,1,Nline);
Wy=normrnd(mu,sigma,1,Nline);
%观测数据生成
Xobs=XLreal+Wx;
Yobs=YLreal+Wy;
Xn=[Xobs;Yobs];%观测数据应为实际测得.
startstate=[statecircleend2(1:2,Ncircle).',-300,0].';
statelineend2=linepart(startstate,Nline,Xn);
%画观测曲线与真实曲线.
labelx=[Xrem,Xn(1,:)];
labely=[Yrem,Xn(2,:)];
plot(labelx,labely,'r-');
plot(XLreal,YLreal,'g-');
axis([-25000,5000,-10000,10000]);
XL2temperror=XLreal-statelineend2(1,:);%滤波误差数组
YL2temperror=YLreal-statelineend2(2,:);
XL2estierror=XLreal-Xobs;
YL2estierror=YLreal-Yobs;
Xerror(i,:)=[XL1temperror,XC1temperror,XC2temperror,XL2temperror];
Yerror(i,:)=[YL1temperror,YC1temperror,YC2temperror,YL2temperror];
Xestierror(i,:)=[XL1estierror,XC1estierror,XC2estierror,XL2estierror];
Yestierror(i,:)=[YL1estierror,YC1estierror,YC2estierror,YL2estierror];
end;


sizeX=size(Xerror);
sizeX=sizeX(2);
sizeY=size(Yerror);
sizeY=sizeY(2);
%用公式计算滤波误差的均值  
sumX=zeros(1,sizeX);
sumY=zeros(1,sizeY);
sumXesti=zeros(1,sizeX);
sumYesti=zeros(1,sizeY);
for i=1:M;
    sumX=sumX+Xerror(i,:);
    sumY=sumY+Yerror(i,:);
    sumXesti=sumXesti+Xestierror(i,:);
    sumYesti=sumYesti+Yestierror(i,:);
end;
eX=sumX/M;
eY=sumY/M;
eXesti=sumXesti/M;
eYesti=sumYesti/M;
n=1:sizeX;
figure(2);
subplot(1,2,1);
plot(n,eX,'b-');
hold on;
plot(n,eXesti,'r');
title('坐标X方向的误差均值');
xlabel('n');
ylabel('eX(m)');
legend('滤波后数据误差','观测数据误差');
subplot(1,2,2);
plot(n,eY,'b-');
hold on;
plot(n,eYesti,'r');
title('坐标Y方向的误差均值');
xlabel('n');
ylabel('eY(m)');
% 滤波误差的标准差
sumX=zeros(1,sizeX);
sumY=zeros(1,sizeY);
sumXesti=zeros(1,sizeX);
sumYesti=zeros(1,sizeY);
for i=1:M;
    sumX=sumX+(Xerror(i,:).^2-eX.^2);
    sumY=sumY+(Yerror(i,:).^2-eY.^2);
    sumXesti=sumXesti+(Xestierror(i,:).^2-eXesti.^2);
    sumYesti=sumYesti+(Yestierror(i,:).^2-eYesti.^2);
end;
sqX=sqrt(sumX/M);
sqY=sqrt(sumY/M);
sqXesti=sqrt(sumXesti/M);
sqYesti=sqrt(sumYesti/M);
n=1:sizeX;
figure(3);
subplot(1,2,1);
plot(n,sqX,'b-');
hold on;
plot(n,sqXesti,'r');
title('坐标X方向的误差标准差');
legend('滤波后数据误差','观测数据误差');
xlabel('n');
ylabel('sqX(m)');
subplot(1,2,2);
plot(n,sqY,'b-');
hold on;
plot(n,sqYesti,'r');
title('坐标Y方向的误差标准差');
xlabel('n');
ylabel('sqY(m)');
figure(1);
legend('滤波后轨迹','观测轨迹','真实轨迹');