function?[MXMYSS]=ExtendedKalmanFilter（D1D2D3A1A2A3Flag1FLAG2S0P0SigmaRSigmaAOA）
%%?TDOA/AOA定位的擴展卡爾曼濾波定位算法
%%?此程序為GreenSim團隊原創作品，轉載請注明
%%?歡迎訪問GreenSim團隊的主頁http://blog.sina.com.cn/greensim
%%?欲與程序原作者進行技術交流，請發郵件Email:aihuacheng@gmail.com
%%?輸入參數列表
%??D1????????基站1和移動臺之間的距離
%??D2????????基站2和移動臺之間的距離
%??D3????????基站3和移動臺之間的距離
%??A1????????基站1測得的角度值
%??A2????????基站2測得的角度值
%??A3????????基站3測得的角度值
%??Falg1?????1×1矩陣，取值123，表明是以哪一個基站作為基準站計算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輸入有誤，它只能取123‘）;
end
%%?第二步：構造兩個固定的矩陣
%構造狀態轉移矩陣Φ
Phi=[1????00.025????0;
?????0????1????00.025;
?????0????0????1????0;
?????0????0????0????1];
%構造W的協方差矩陣Q
SigmaU=0.00001;%噪聲方差取很小的值
Q=[0?????0?????0?????0;
???0?????0?????0?????0;
???0?????0SigmaU?????0;
???0?????0?????0SigmaU];
%%?第三步：輸出數據初始化
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:）;%當前各信道環境的LOS/NLOS判據
????R=FunR（SigmaRSigmaAOAFlag2）;%調用產生R矩陣的子函數
????S1=Phi*S0;%由狀態方程得到的預測值
????H=FunH（S1Flag1Flag2）;%調用產生H矩陣的子函數
????P1=Phi*P0*（Phi‘）+Q;%計算上述預測值的協方差矩陣
????K=P1*（H‘）*inv（H*P1*（H‘）+R）;%計算濾波增益（加權系數）
????Z=FunZ（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）;%調用構造觀察向量的子函數
????hS1=FunhS1（S1Flag1Flag2）;%調用構造觀測值的估計值向量的子函數
????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（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）
%調用構造觀察向量的子函數
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）
%%?產生R矩陣的子函數
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];

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????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程序\