function?[MXMYSS]=ExtendedKalmanFilter（D1D2D3A1A2A3Flag1FLAG2S0P0SigmaRSigmaAOA）
%%?TDOA/AOA定位的擴展卡爾曼濾波定位算法
%??GreenSim團隊——專業(yè)級算法設(shè)計&代寫程序
%??歡迎訪問GreenSim團隊主頁→http://blog.sina.com.cn/greensim
%%?輸入?yún)?shù)列表
%??D1????????基站1和移動臺之間的距離
%??D2????????基站2和移動臺之間的距離
%??D3????????基站3和移動臺之間的距離
%??A1????????基站1測得的角度值
%??A2????????基站2測得的角度值
%??A3????????基站3測得的角度值
%??Falg1?????1×1矩陣，取值123，表明是以哪一個基站作為基準站計算TDOA數(shù)據(jù)的
%??FLAG2?????N×3矩陣，取值0和1，每一行表示該時刻各基站的AOA是否可選擇，
%????????????1表示選擇AOA數(shù)據(jù)，F(xiàn)LAG2并非人為給定，而是由LOS/NLOS檢測模塊確定
%??S0????????初始狀態(tài)向量，4×1矩陣
%??P0????????預(yù)測誤差矩陣的初始值，4×4的矩陣
%??SigmaR????無偏/有偏卡爾曼輸出距離值的方差，由事先統(tǒng)計得到
%??SigmaAOA??選擇AOA數(shù)據(jù)的方差，生成AOA數(shù)據(jù)的規(guī)律已知，因此可以確定
%%??輸出參數(shù)列表
%??MX
%??MY
%%?第一步：計算TDOA數(shù)據(jù)
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輸入有誤，它只能取123‘）;
end
%%?第二步：構(gòu)造兩個固定的矩陣
%構(gòu)造狀態(tài)轉(zhuǎn)移矩陣Φ
Phi=[1????00.025????0;
?????0????1????00.025;
?????0????0????1????0;
?????0????0????0????1];
%構(gòu)造W的協(xié)方差矩陣Q
SigmaU=0.00001;%噪聲方差取很小的值
Q=[0?????0?????0?????0;
???0?????0?????0?????0;
???0?????0SigmaU?????0;
???0?????0?????0SigmaU];
%%?第三步：輸出數(shù)據(jù)初始化
N=length（D1）;
MX=zeros（1N）;
MY=zeros（1N）;
MX（1）=S0（1）;
MY（1）=S0（2）;
SS=zeros（4N）;
SS（:1）=S0;
%%?第四步：以下是迭代過程
for?i=2:N
????Flag2=FLAG2（i:）;%當前各信道環(huán)境的LOS/NLOS判據(jù)
????R=FunR（SigmaRSigmaAOAFlag2）;%調(diào)用產(chǎn)生R矩陣的子函數(shù)
????S1=Phi*S0;%由狀態(tài)方程得到的預(yù)測值
????H=FunH（S1Flag1Flag2）;%調(diào)用產(chǎn)生H矩陣的子函數(shù)
????P1=Phi*P0*（Phi‘）+Q;%計算上述預(yù)測值的協(xié)方差矩陣
????K=P1*（H‘）*inv（H*P1*（H‘）+R）;%計算濾波增益（加權(quán)系數(shù)）
????Z=FunZ（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）;%調(diào)用構(gòu)造觀察向量的子函數(shù)
????hS1=FunhS1（S1Flag1Flag2）;%調(diào)用構(gòu)造觀測值的估計值向量的子函數(shù)
????S2=S1+K*（Z-hS1）;%加權(quán)得到濾波輸出值
????%更新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（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）
%調(diào)用構(gòu)造觀察向量的子函數(shù)
m=sum（Flag2）;
Z=zeros（2+m1）;
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&&F