function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA)
%% TDOA/AOA定位的擴展卡爾曼濾波定位算法

%% 輸入參數列表
%  D1        基站1和移動臺之間的距離
%  D2        基站2和移動臺之間的距離
%  D3        基站3和移動臺之間的距離
%  A1        基站1測得的角度值
%  A2        基站2測得的角度值
%  A3        基站3測得的角度值
%  Falg1     1×1矩陣，取值1,2,3，表明是以哪一個基站作為基準站計算TDOA數據的
%  FLAG2     N×3矩陣，取值0和1，每一行表示該時刻各基站的AOA是否可選擇，
%            1表示選擇AOA數據，FLAG2并非人為給定，而是由LOS/NLOS檢測模塊確定
%  S0        初始狀態向量，4×1矩陣
%  P0        預測誤差矩陣的初始值，4×4的矩陣
%  SigmaR    無偏/有偏卡爾曼輸出距離值的方差，由事先統計得到
%  SigmaAOA  選擇AOA數據的方差，生成AOA數據的規律已知，因此可以確定
%%  輸出參數列表
%  MX
%  MY

%% 第一步：計算TDOA數據
if Flag1==1
    TDOA1=D2-D1;
    TDOA2=D3-D1;
elseif Flag1==2
    TDOA1=D1-D2;
    TDOA2=D3-D2;
elseif Flag1==3
    TDOA1=D1-D3;
    TDOA2=D2-D3;
else
    error('Flag1輸入有誤，它只能取1,2,3');
end

%% 第二步：構造兩個固定的矩陣
%構造狀態轉移矩陣Φ
Phi=[1,    0,0.025,    0;
     0,    1,    0,0.025;
     0,    0,    1,    0;
     0,    0,    0,    1];
%構造W的協方差矩陣Q
SigmaU=0.00001;%噪聲方差取很小的值
Q=[0,     0,     0,     0;
   0,     0,     0,     0;
   0,     0,SigmaU,     0;
   0,     0,     0,SigmaU];

%% 第三步：輸出數據初始化
N=length(D1);
MX=zeros(1,N);
MY=zeros(1,N);
MX(1)=S0(1);
MY(1)=S0(2);
SS=zeros(4,N);
SS(:,1)=S0;

%% 第四步：以下是迭代過程
for i=2:N
    Flag2=FLAG2(i,:);%當前各信道環境的LOS/NLOS判據
    R=FunR(SigmaR,SigmaAOA,Flag2);%調用產生R矩陣的子函數
    S1=Phi*S0;%由狀態方程得到的預測值
    H=FunH(S1,Flag1,Flag2);%調用產生H矩陣的子函數
    P1=Phi*P0*(Phi')+Q;%計算上述預測值的協方差矩陣
    K=P1*(H')*inv(H*P1*(H')+R);%計算濾波增益（加權系數）
    Z=FunZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i);%調用構造觀察向量的子函數
    hS1=FunhS1(S1,Flag1,Flag2);%調用構造觀測值的估計值向量的子函數
    S2=S1+K*(Z-hS1);%加權得到濾波輸出值
    %更新S0和P0
    P2=P1-K*H*P1;
    S0=S2;
    P0=P2;
    %記錄濾波輸出值
    MX(i)=S2(1);
    MY(i)=S2(2);
    SS(:,i)=S2;
end


function Z=FunZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i)
%調用構造觀察向量的子函數
m=sum(Flag2);
Z=zeros(2+m,1);
Z(1)=TDOA1(i);
Z(2)=TDOA2(i);
if Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==0
    %空語句
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==0
    Z(3)=A1(i);
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==0
    Z(3)=A2(i);
elseif Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==1
    Z(3)=A3(i);
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==0
    Z(3)=A1(i);
    Z(4)=A2(i);
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==1
    Z(3)=A1(i);
    Z(4)=A3(i);
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==1
    Z(3)=A2(i);
    Z(4)=A3(i);
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==1
    Z(3)=A1(i);
    Z(4)=A2(i);
    Z(5)=A3(i);
else
    error('Flag2格式不正確，它的元素只能取0或者1');
end


function R=FunR(SigmaR,SigmaAOA,Flag2)
%% 產生R矩陣的子函數
m=sum(Flag2);
B=[-1,1,0;
   -1,0,1];
R11=B*[SigmaR,0,0;0,SigmaR,0;0,0,SigmaR]*(B');
R12=zeros(2,m);
R21=zeros(m,2);
if m==0
    R22=[];
elseif m==1
    R22=SigmaAOA;
elseif m==2
    R22=[SigmaAOA,       0;
                0,SigmaAOA];
elseif m==3
    R22=[SigmaAOA,       0,       0;
                0,SigmaAOA,       0;
                0,       0,SigmaAOA];
else
    error('Flag2格式不正確，它的元素只能取0或者1');
end
R=[R11,R12;
   R21,R22];


function hS1=FunhS1(S1,Flag1,Flag2)
%% 構造觀測值的估計值向量的子函數
%提取估計的移動臺坐標
x=S1(1);
y=S1(2);
%三個基站的橫縱坐標
x1=0;
y1=0;
x2=5;
y2=8.66;
x3=10;
y3=0;
%計算移動臺到三個基站的距離（估計值）
d1=((x-x1)^2+(y-y1)^2)^0.5;
d2=((x-x2)^2+(y-y2)^2)^0.5;
d3=((x-x3)^2+(y-y3)^2)^0.5;
M=2+sum(Flag2);
hS1=zeros(M,1);
if Flag1==1%以第一個基站為基準計算TDOA數據
    hS1(1)=d2-d1;
    hS1(2)=d3-d1;
elseif Flag1==2%以第二個基站為基準計算TDOA數據
    hS1(1)=d1-d2;
    hS1(2)=d3-d2;
elseif Flag1==3%以第三個基站為基準計算TDOA數據
    hS1(1)=d1-d3;
    hS1(2)=d2-d3;
else
    error('Flag1格式不正確，它只能取1,2,3');
end

h1=atan2(y-y1,x-x1);
h2=atan2(y-y2,x-x2);
h3=atan2(y-y3,x-x3);
if Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==0
    %空語句
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==0
    hS1(3)=h1;
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==0
    hS1(3)=h2;
elseif Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==1
    hS1(3)=h3;
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==0
    hS1(3:4)=[h1;h2];
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==1
    hS1(3:4)=[h1;h3];
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==1
    hS1(3:4)=[h2;h3];
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==1
    hS1(3:5)=[h1;h2;h3];
else
    error('Flag2格式不正確，它的元素只能取0或者1');
end