%===============================
%Recursive least square 機動
%===============================
clear all
axis=linspace(1,330,330);
x_ErrorSum=zeros(1,330);
y_ErrorSum=zeros(1,330);
x_ErrorSquare=zeros(1,330);
y_ErrorSquare=zeros(1,330);
    %以下產生真實值
    for k=1:200
        x_real(k)=2000;
        y_real(k)=10000-15*2*k;
    end
    for k=201:300
        x_real(k)=x_real(200)+0.15*(k-200)^2;
        y_real(k)=y_real(200)-30*(k-200)-0.15*(k-200)^2;
    end
    for k=301:305
        x_real(k)=x_real(300)+30*(k-300);
        y_real(k)=y_real(300)-60*(k-300);
    end
    for k=306:330
        x_real(k)=x_real(305)+30*(k-305)-0.6*(k-305)^2;
        y_real(k)=y_real(305)-60*(k-305)+0.6*(k-305)^2;
    end
for i=1:50  %-------根據蒙特卡洛思想，做50次仿真
    for k=1:330
        x_observe(k)=x_real(k)+normrnd(0,100);
        y_observe(k)=y_real(k)+normrnd(0,100);
    end
    %以下用前兩個觀測值產生初始值
    x_estimate(1)=x_observe(1);
    y_estimate(1)=y_observe(1);
    x_estimate(2)=(x_observe(1)+x_observe(2))/2;
    y_estimate(2)=y_observe(2);
    begin_vector=[1 2;1 4]\[y_observe(1) y_observe(2)]';
    P=inv([1 2;1 4]'*inv(10000*eye(2))*[1 2;1 4]);
    %以下執行循環算法
    for k=3:330
        K=P*[1 2*k]'/([1 2*k]*P*[1 2*k]'+10000);
        begin_vector=begin_vector+K*(y_observe(k)-[1 2*k]*begin_vector);
        P=(eye(2)-K*[1 2*k])*P;
        y_estimate(k)=[1 2*k]*begin_vector;
        x_estimate(k)=(x_estimate(k-1)*(k-1)+x_observe(k))/k;
    end
    %以下紀錄每一個時刻的估計誤差和聚估計誤差的平方
    for k=1:330
        x_ErrorSum(k)=x_ErrorSum(k)+(x_real(k)-x_estimate(k));
        y_ErrorSum(k)=y_ErrorSum(k)+(y_real(k)-y_estimate(k));
        x_ErrorSquare(k)=x_ErrorSquare(k)+(x_real(k)-x_estimate(k))^2;
        y_ErrorSquare(k)=y_ErrorSquare(k)+(y_real(k)-y_estimate(k))^2;
    end
end
%以下計算誤差均值向量和標準差向量
x_ErrorMean=x_ErrorSum./50;
y_ErrorMean=y_ErrorSum./50;
x_ErrorSigma=(x_ErrorSquare./50-x_ErrorMean.^2).^0.5;
y_ErrorSigma=(y_ErrorSquare./50-y_ErrorMean.^2).^0.5;
%以下輸出顯示
figure(1);
plot(x_real,y_real,':',x_observe,y_observe,'*',x_estimate,y_estimate,'r-');
title('目標跟蹤軌跡');
figure(2);
subplot(2,1,1);
plot(axis,x_ErrorMean,'b'),grid on;
title('x方向誤差的均值');
subplot(2,1,2);
plot(axis,y_ErrorMean,'b'),grid on;
title('y方向誤差的均值');
figure(3);
subplot(2,1,1);
plot(axis,x_ErrorSigma,'b'),grid on;
title('x方向誤差的標準差');
subplot(2,1,2);
plot(axis,y_ErrorSigma,'b'),grid on;
title('y方向誤差的標準差');