matlab实现的三维UWBTDOA AOA联合定位算法仿真程序

matlab实现的三维UWBTDOA AOA联合定位算法仿真程序，写的很详细

%% 三维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