function?[MXMYSS]=ExtendedKalmanFilter（D1D2D3A1A2A3Flag1FLAG2S0P0SigmaRSigmaAOA）
%%?TDOA/AOA定位的擴(kuò)展卡爾曼濾波定位算法
%%?此程序?yàn)镚reenSim團(tuán)隊(duì)原創(chuàng)作品，轉(zhuǎn)載請(qǐng)注明
%%?歡迎訪問GreenSim團(tuán)隊(duì)的主頁http://blog.sina.com.cn/greensim
%%?欲與程序原作者進(jìn)行技術(shù)交流，請(qǐng)發(fā)郵件Email:aihuacheng@gmail.com
%%?輸入?yún)?shù)列表
%??D1????????基站1和移動(dòng)臺(tái)之間的距離
%??D2????????基站2和移動(dòng)臺(tái)之間的距離
%??D3????????基站3和移動(dòng)臺(tái)之間的距離
%??A1????????基站1測(cè)得的角度值
%??A2????????基站2測(cè)得的角度值
%??A3????????基站3測(cè)得的角度值
%??Falg1?????1×1矩陣，取值123，表明是以哪一個(gè)基站作為基準(zhǔn)站計(jì)算TDOA數(shù)據(jù)的
%??FLAG2?????N×3矩陣，取值0和1，每一行表示該時(shí)刻各基站的AOA是否可選擇，
%????????????1表示選擇AOA數(shù)據(jù)，F(xiàn)LAG2并非人為給定，而是由LOS/NLOS檢測(cè)模塊確定
%??S0????????初始狀態(tài)向量，4×1矩陣
%??P0????????預(yù)測(cè)誤差矩陣的初始值，4×4的矩陣
%??SigmaR????無偏/有偏卡爾曼輸出距離值的方差，由事先統(tǒng)計(jì)得到
%??SigmaAOA??選擇AOA數(shù)據(jù)的方差，生成AOA數(shù)據(jù)的規(guī)律已知，因此可以確定
%%??輸出參數(shù)列表
%??MX
%??MY
%%?第一步：計(jì)算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è)固定的矩陣
%構(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:）;%當(dāng)前各信道環(huán)境的LOS/NLOS判據(jù)
????R=FunR（SigmaRSigmaAOAFlag2）;%調(diào)用產(chǎn)生R矩陣的子函數(shù)
????S1=Phi*S0;%由狀態(tài)方程得到的預(yù)測(cè)值
????H=FunH（S1Flag1Flag2）;%調(diào)用產(chǎn)生H矩陣的子函數(shù)
????P1=Phi*P0*（Phi‘）+Q;%計(jì)算上述預(yù)測(cè)值的協(xié)方差矩陣
????K=P1*（H‘）*inv（H*P1*（H‘）+R）;%計(jì)算濾波增益（加權(quán)系數(shù)）
????Z=FunZ（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）;%調(diào)用構(gòu)造觀察向量的子函數(shù)
????hS1=FunhS1（S1Flag1Flag2）;%調(diào)用構(gòu)造觀測(cè)值的估計(jì)值向量的子函數(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&&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（SigmaRSigmaAOAFlag2）
%%?產(chǎn)生R矩陣的子函數(shù)
m=sum（Flag2）;
B=[-110;
???-101];
R11=B*[SigmaR00;0SigmaR0;00SigmaR]*（B‘）;
R12=zeros（2m）;
R21=zeros（m2）;
if?m==0
????R22=[];
elseif?m==1
????R22=SigmaAOA;
elseif?m==2
????R22=[SigmaAOA???????0;
????????????????0SigmaAOA];

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件????????5031??2018-03-27?15:45??基于卡爾曼濾波的定位算法MATLAB程序\ExtendedKalmanFilter.m
?????文件????????4743??2018-03-27?16:42??基于卡爾曼濾波的定位算法MATLAB程序\Untitled2.m
?????文件?????????192??2018-03-27?16:37??基于卡爾曼濾波的定位算法MATLAB程序\Untitled4.m
?????文件????????2642??2018-03-30?20:22??基于卡爾曼濾波的定位算法MATLAB程序\test_ekf.m
?????目錄???????????0??2018-03-30?20:22??基于卡爾曼濾波的定位算法MATLAB程序\