function [x,y,zx,zy,XE,YE,XERB,YERB,XSTD,YSTD]=LS(T,M,d)
%輸入?yún)?shù)：觀測噪聲方差d,采樣間隔T,蒙特卡洛次數(shù)M
totalTime=800;  
%真實(shí)航跡產(chǎn)生
[x,y]=realTrack(T,totalTime);
%隨機(jī)初始化
randn('state',sum(100*clock));
D=d*d;
N=ceil(totalTime/T);
for mont=1:M
    %觀測數(shù)據(jù)產(chǎn)生
    for i=1:N
        zx(i)=x(i)+d*randn(1);
        zy(i)=y(i)+d*randn(1);
    end
    %開始濾波
    t=T:T:N*T;
    %X方向
    XXX2=[zx(1)*t(2)-zx(2)*t(1)  zx(2)-zx(1)]'/(t(2)-t(1));
    XG(2)=XXX2(1);
    VXG(2)=XXX2(2);
    XG(1)=XG(2);
    VXG(1)=VXG(2);
    for k=2:(N-1)
        K=[-2/(k+1) 6/((k+1)*(k+2)*T)]';
        H=[1 t(k+1)];
        XXX2=XXX2+K*(zx(k+1)-H*XXX2);
        XG(k+1)=XXX2(1);
        VXG(k+1)=XXX2(2);
    end
    %Y方向
    YYY2=[zy(1)*t(2)-zy(2)*t(1)  zy(2)-zy(1)]'/(t(2)-t(1));
    YG(2)=YYY2(1);
    VYG(2)=YYY2(2);
    YG(1)=YG(2);
    VYG(1)=VYG(2);
    for k=2:(N-1)
        K=[-2/(k+1) 6/((k+1)*(k+2)*T)]';
        H=[1 t(k+1)];
        YYY2=YYY2+K*(zy(k+1)-H*YYY2);
        YG(k+1)=YYY2(1);
        VYG(k+1)=YYY2(2);
    end
    %計(jì)算位置估計(jì)
    for k=1:N
        XE(k)=XG(k)+VXG(k)*t(k);
    end
    for k=1:N
        YE(k)=YG(k)+VYG(k)*t(k);
    end
    %誤差矩陣
    XER(1:N,mont)=x(1:N)-(XE(1:N))';
    YER(1:N,mont)=y(1:N)-(YE(1:N))';
end  %end for mont
%濾波誤差的均值
XERB=mean(XER,2);
YERB=mean(YER,2);
%濾波誤差的標(biāo)準(zhǔn)差
XSTD=std(XER,1,2);
YSTD=std(YER,1,2);
%作圖 
% figure(1) 
% plot(zx,zy,'g',x,y,'red',XE,YE,'b')
% axis([1500 4500 -10000 11000]); 
% figure(2) 
% subplot(2,2,1)
% plot(XERB) 
% xlabel('觀測次數(shù)') 
% ylabel('X方向?yàn)V波誤差均值') 
% subplot(2,2,2) 
% plot(YERB)
% xlabel('觀測次數(shù)') 
% ylabel('Y方向?yàn)V波誤差均值') 
% subplot(2,2,3) 
% plot(XSTD)
% xlabel('觀測次數(shù)') 
% ylabel('X方向?yàn)V波誤差標(biāo)準(zhǔn)值') 
% subplot(2,2,4) 
% plot(YSTD)
% xlabel('觀測次數(shù)') 
% ylabel('Y方向?yàn)V波誤差標(biāo)準(zhǔn)值')