clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化
W=200;  %該區域的長度
L=200;  %該區域的寬度
M=36;   %該區域內節點數
Nod=NodGen(W,L,M,3);    %生成節點分布圖
ar=3;   %測距方差
ao=(3/180)*pi;  %測角方差
T=50;   %總的仿真時間
V=5;    %目標運動速度,這在本仿真中為已知量
av=1;   %策動噪聲方差




Target_Real{1}=[25 25];     %第一時刻目標參考位置
Target_Real{1}=[25 25]+av*[randn randn];    %第一時刻目標真實位置
for t=1:T
%     Target_Real{t+1}=Target_Real{t}+V^0.5*[1 1]+av*randn*[1 1];
    Target_Real{t+1}=Target_Real{t}+V/2^0.5*[1 1]+av*randn*[1 1];
end
%以下生成量測
for t=1:T
    for m=1:M
        r(m)=( ( Nod{m}(1) - Target_Real{t}(1) )^2 + ( Nod{m}(2) - Target_Real{t}(2) )^2 )^0.5;    
    end
    Dot(t)=find(r==min(r));
    x=Target_Real{t}(1)-Nod{Dot(t)}(1);
    y=Target_Real{t}(2)-Nod{Dot(t)}(2);
    [thr r]=cart2pol(x,y);
    thr=thr+ao*randn;
    r=r+ar*randn;
    [x y]=pol2cart(thr,r);
    Target_Z{t}=[x y]+[Nod{Dot(t)}(1) Nod{Dot(t)}(2)];
end
%生成參考坐標
for t=1:T
    Refer{t}=[round(Target_Real{t}(1)) round(Target_Real{t}(2))];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%進行第一時刻的初始化,生成第一時刻的分布
Pbelief{1}=fun1( Nod{Dot(1)} , Target_Z{1} , Refer{1} , ar , ao );
figure,pcolor(Pbelief{1});

%根據分布得到濾波結果估計
Target_Esti{1}=fun2( Pbelief{1} , Refer{1} );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%以下生成狀態轉移概率矩陣
for i=1:99
    for j=1:99
        x=i/2;
        y=j/2;
%         P_P(i,j)=exp( -(x-25-V^0.5)^2/2/av^2 - (y-25-V^0.5)^2/2/av^2 );
        P_P(i,j)=exp( -(x-25-V/2^0.5)^2/2/av^2 - (y-25-V/2^0.5)^2/2/av^2 );
    end
end
P_P=P_P/sum(sum(P_P));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%以下為貝葉斯濾波過程
for t=2:T
    t
    %%%%%%%%%%%%%%%%%%
    %預測過程 包括卷積過程及概率矩陣中心變換
    Pbelief_P{t}=conv2( Pbelief{t-1} , P_P , 'same' );
    a=Refer{t}(1)-Refer{t-1}(1);    a=a*2;
    b=Refer{t}(2)-Refer{t-1}(2);    b=b*2;
    P=zeros(99);
    for i=1:99
        for j=1:99
            if( (i-a)>0 & (j-b)>0 )
                P(i-a,j-b)=Pbelief_P{t}(i,j);
            end
        end
    end
    Pbelief_P{t}=P;
    %%%%%%%%%%%%%%%%%%%
    %生成似然函數
    Pzx{t}=fun1( Nod{Dot(t)} , Target_Z{t} , Refer{t} , ar , ao );
    %%%%%%%%%%%%%%%%%%
    %更新過程
    Pbelief{t}=Pbelief_P{t}.*Pzx{t};
    Pbelief{t}=Pbelief{t}/sum(sum(Pbelief{t}));
    Target_Esti{t}=fun2( Pbelief{t} , Refer{t} );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%圖示仿真結果
for t=1:T
    x(t)=Target_Real{t}(1);
    y(t)=Target_Real{t}(2);    
    x1(t)=Target_Z{t}(1);
    y1(t)=Target_Z{t}(2);
    x2(t)=Target_Esti{t}(1);
    y2(t)=Target_Esti{t}(2);
end
figure,plot(x,y,x1,y1,x2,y2,x,y,'.',x1,y1,'.',x2,y2,'.')
legend('真實目標軌跡','觀測軌跡','濾波后軌跡')
axis([0 W 0 L])

for t=1:T
    D1(t)=( (x(t)-x1(t))^2 + (y(t)-y1(t))^2 )^0.5;
    D2(t)=( (x(t)-x2(t))^2 + (y(t)-y2(t))^2 )^0.5;
end
figure,plot(1:T,D1,1:T,D2)
legend('觀測誤差','濾波后誤差')
(sum(D1.^2/T))^0.5
(sum(D2.^2/T))^0.5
sum(D1)/T
sum(D2)/T