lsfilter.m

基于matlab的几个kalman滤波以及多传感器融合和wk算法的程序

clear;
clc;

IdealTrack=zeros(2,200);
for k=1:200
    IdealTrack(1,k)=2000;
    IdealTrack(2,k)=10000-30*k;
end
plot(IdealTrack(1,:),IdealTrack(2,:),'b');
axis([1000,3000,2000,12000]);
hold on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Noise=100*randn(2,200);
MeasureTrack=zeros(2,200);
MeasureTrack=IdealTrack+Noise;
plot(MeasureTrack(1,:),MeasureTrack(2,:),'r');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FilterTrack=zeros(4,200);

for n=200:-1:1
    %clear NewMeasure;
    NewMeasure=zeros(2*n,1);
    for j=1:n
        NewMeasure(2*j-1:2*j,1)=MeasureTrack(:,n-j+1);
    end
    FilterTrack(:,n)=lsrecfun(NewMeasure);
end
%figure(2);
plot(FilterTrack(1,:),FilterTrack(3,:),'k');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%