%%?三維TDOA/AOA定位的擴展卡爾曼濾波仿真主程序
clc
clear
close?all
%%?設置觀測站分布
N=4;%觀測站個數
%主站位于坐標原點
x1=0;
y1=0;
z1=0;
x2=1000;
y2=0;
z2=0;
x3=0;
y3=1000;
z3=0;
x4=1000;
y4=1000;
z4=0;
XI=[x1x2x3x4];
YI=[y1y2y3y4];
ZI=[z1z2z3z4];
%%?產生觀測數據
%假設被定位對象勻速地從A點（100300300）運動到B點（9007000），單位：米
Px=[100:0.8:900]‘;
Py=[300:0.4:700]‘;
Pz=[300:-0.3:0]‘;
%假設采樣時間間隔為1s
T=1;
%根據觀測誤差水平，產生各個觀測站的觀測量
M=length（Px）;%離散點數
Alpha=zeros（MN）;
Beta=zeros（MN）;
Distance=zeros（MN）;
Sigma_Alpha_Theta=1;%以角度形式表示的角度噪聲標準差
Sigma_Alpha=（（Sigma_Alpha_Theta）/180）*pi;%轉化為弧度表示的角度噪聲方差
Sigma_Beta_Theta=1.5;%以角度形式表示的角度噪聲標準差
Sigma_Beta=（（Sigma_Beta_Theta）/180）*pi;%轉化為弧度表示的角度噪聲方差
Sigma_Distance=2;%距離測量的標準差
for?n=1:N
????Alpha（:n）=atan2（Py-YI（n）Px-XI（n））+Sigma_Alpha*randn（M1）;
????Beta（:n）=atan2（Pz-ZI（n）sqrt（（Px-XI（n））.^2+（Py-YI（n））.^2））+Sigma_Beta*randn（M1）;
????Distance（:n）=sqrt（（Px-XI（n））.^2+（Py-YI（n））.^2+（Pz-ZI（n））.^2）+Sigma_Distance*randn（M1）;
end
Rho=Distance（:2:end）-Distance（:1）*ones（1N-1）;

%%?使用擴展卡爾曼濾波進行定位
SigmaU=0.005;%驅動噪聲
S0=[1202703200.50.1-0.4]‘;%初始狀態向量
P0=0.01*ones（66）;%預測誤差矩陣的初始值
[MXMYMZ]=EKF_TDOA_AOA_3D（AlphaBetaRhoXIYIZISigmaUSigma_AlphaSigma_BetaSigma_DistanceTS0P0）;

%%?繪圖
figure
plot3（PxPyPz‘b‘）;
xlabel（‘X軸‘）;
ylabel（‘Y軸‘）;
zlabel（‘Z軸‘）;
grid?on
axis（[0?1000?0?1000?0?300]）;
hold?on
plot3（MXMYMZ‘r‘）;
title（‘跟蹤軌跡‘）;
%
figure
plot（MX-Px）;
xlabel（‘迭代次數‘）;
ylabel（‘誤差（米）‘）;
title（‘X軸收斂曲線‘）;
grid?on
%
figure
plot（MY-Py）;
xlabel（‘迭代次數‘）;
ylabel（‘誤差（米）‘）;
title（‘Y軸收斂曲線‘）;
grid?on
%
figure
plot（MZ-Pz）;
xlabel（‘迭代次數‘）;
ylabel（‘誤差（米）‘）;
title（‘Z軸收斂曲線‘）;
grid?on
%
figure
ERR=sqrt（（MX-Px）.^2+（MY-Py）.^2+（MZ-Pz）.^2）;
plot（ERR）;
xlabel（‘迭代次數‘）;
ylabel（‘誤差（米）‘）;
title（‘誤差收斂曲線‘）;
grid?on

function?[XAllXAlldxy]=TDOA_AOA_3D（X0ThetaPhiTauxbybDeltaK）
%?GreenSim團隊原創作品，轉載請注明
%?Email:greensim@163.com
%?GreenSim團隊主頁：http://blog.sina.com.cn/greensim
%?[color=red]歡迎訪問GreenSim——算法仿真團隊→[url=http://blog.sina.com.cn/greensim]http://blog.sina.com.cn/greensim[/url][/color]
N=length（Theta）;
H=zeros（3*N-12*N+2）;
Rho=zeros（3*N-11）;
AllX=zeros（K2*N+2）;
Alldxy=zeros（K1）;
dxy=Inf;
X=X0;
counter=1;
while?dxy>Delta
????xm=X（2*N+1）;
????ym=X（2*N+2）;
????x1=X（1）;
????y1=X（2）;
????for?i=1:N
????????xi=X（2*i-1）;
????????yi=X（2*i）;
????????if?xi==xm
????????????xi=xm+0.000001;
????????end
????????if?yi==ym
????????????yi=ym+0.000001;
????????end
????????if?xi==xb
????????????xi=xb+0.000001;
????????end
????????if?yi==yb
????????????yi=yb+0.000001;
????????end
????????H（i2*i-1）=-（yi-ym）/（xi-xm）^2/（1+（yi-ym）^2/（xi-xm）^2）;
????????H（i2*i）=1/（xi-xm）/（1+（yi-ym）^2/（xi-xm）^2）;
????????H（i2*N+1）=（yi-ym）/（xi-xm）^2/（1+（yi-ym）^2/（xi-xm）^2）;
????????H（i2*N+2）=-1/（xi-xm）/（1+（yi-ym）^2/（xi-xm）^2）;
????????Rho（i1）=atan（（yi-ym）/（xi-xm））;
????????H（N+i2*i-1）=-（yi-yb）/（xi-xb）^2/（1+（yi-yb）^2/（xi-xb）^2）;
????????H（N+i2*i）=1/（xi-xb）/（1+（yi-yb）^2/（xi-xb）^2）;
??

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????4633??2011-12-19?19:28??sanwei?tdoaand?aoa.m

-----------?---------??----------?-----??----

?????????????????4633????????????????????1