本项目作者：苏晴
无线定位技术
基于距离的无线定位技术（TOA）
为了简化问题，我们考虑二维环境，扩展到三维环境中可以使用以下相同的步骤。在图1中，M个接收机在二维环境中随意分布。假设\((x,y)\)是目标的位置，\((x_i,y_i)\)是第\(i\)个接收机的位置，\(d_i\)是第\(i\)个接收机的到达时间测量数据[1]。到达时间可以由扩展卡尔曼滤波或者其他方法得到更加准确的结果。
视线路径丰富环境下
目前在实践中，尤其在大城市或者多山区域，目标发出的信号通常不能直接到达接收者，他们总是比直接的花费更长的路径。所以通过结合非视距传播的影响，存在如下公式
在这里\({K_i} = {x_i}^2 + {y_i}^2,{r_i} = c{d_i}\)是目标和第i个接收者的测量路径，并且c是信号传播速度。并且通过定义一个新的变量\(R = {x^2} + {y^2}\),我们通过线性表达式重新写上式
令\({Z_a} = {[x,y,R]^T}\),式(2)过矩阵形式表示如下
式中
当视距传播存在于目标和所有的接收机之间时，(3)式转化成等式，在这种情况下，最大似然定位方法由下式给出
其中
Q是测量噪声的协方差矩阵，并且\({r_1}^0,...,{r_M}^0\)表示目标和i个接收机的真实的距离，在估计中，我们需要使用矩阵B。但是如(6)式，对角线的值是不知道的。所以，我们先使用测量值代替真实值，用来估计移动设备初始的位置，然后利用移动设备的初始解计算相应的矩阵B，然后利用新的矩阵B进一步得到精确的结果。这个过程是迭代的直至结果收敛。仿真显示大多数情况下这个过程可以很快的收敛在几次迭代以后。将该视线路径的解表示为\(( x',y')\)。
非视线传播严重环境下
当非视线传播变得严重时，在等式(3)中不保持等式他的准确性开始下降。所以在非视线传播环境中，我们不得不考虑(3)式中的不等式情况。如果使用最大似然估计时，这就意味着我们应该找到(3)给出的条件下的最大似然估计值，而不是在全空间中搜索他。因此存在一个数学问题，如下
其中，\(h,{G_a},{Z_a}\)和\(\psi\)像式(4)和式(6)有相同的定义。对于估计值\(\psi\)，我们使用通过坐标\((x',y')\)计算得出的\({r_1}',...,{r_M}'\)，而不是将真实的值作为(6)中的B的对角线。非常明显可得，(2.7)式是一个线性约束最小二乘问题，是一类二次规划问题，有很多算法被开发解决这类问题。在这里，用MATLAB的quadprog函数进行求解，当求出(7)式中\({Z_a}\)的满足要求的解时，\({Z_a}\)的协方差矩阵可由下式计算
因为在\({Z_a}\)的估计过程中，我们使用了变量\(x,y,R\)的独立性假设，尽管变量R依赖于变量\(x,y\)，我们应该将结果修改如下。令\(x,y,R\)的估计误差分别为\({e_1},{e_2},{e_3}\)。在这里，\({[M]_ {i,j}}\)表示矩阵M的第\((i,j)\)项，那么向量\({Z_a}\)中的项成为
其中，\({x^0},{y^0},{R^0}\)表示为\(x,y,R\)的真实值。令另一个误差矢量
其中
并且，\({Z_p} = \left[ \begin{array}{l} {x^2}\\ {y^2} \end{array} \right]\)，将式(9)代入式(10)中，可以得到
显然地，上述近似仅在误差很小的情况下才有效。\(\psi '\)的协方差矩阵是
作为一个近似，矩阵\(B'\)中的\({x^0},{y^0}\)可以由\({Z_a}\)中的前两个元素\(x,y\)代替。相似地，\({Z_p}\)的估计可以由下式给出
所以，最终位置\(Z{\rm{ = (}}x,y{{\rm{)}}^{\rm{T}}}\)是
这里x的符号应与\({[{Z_a}]_1}\)重合，同理y也是。
基于到达角度的无线定位技术（AOA）
在这一节中，假设：（1）AOA测量噪声是独立的0均值和已知方差的随机变量的高斯随机变量；（2）在AOA测量中存在视线路径。当存在的基站数量大于等于3时，我们有一系列多种因素决定的非线性定位方程。根据几何关系可知
通过矩阵变换可得
可写为
式中：
设估计位置对应的误差协方差矩阵为Q，则定位方案由下式给出
基于到达角度的定位方法虽然比较简单方便，但是对于接收天线的方向性要求极高，定位精度受定位环境的影响很大。
混合定位技术(TOA/AOA)
在TOA/AOA混合定位技术中，将上节提到的AOA定位方法扩展到章节1中的TOA定位方法[2]。假设第i个基站RD坐标为\(({x_i},{y_i})\)，与移动设备MD\((x,y)\)的距离为\({r_i}\)，角度为\({\theta _ i}\)，则参数之间满足
可写为(18)式的形式。其中
其中
式(25)中，\({n_d},{n_\theta }\)分别是距离与角度的测量误差，设Q为联合方差矩阵，为一对角矩阵：
\({\sigma _{{\rm{di}}}}^{\rm{2}},{\sigma _{\theta {\rm{i}}}}^{\rm{2}}i = 1,2,...,M\)分别是各基站TOA与AOA的测量方差。由式(26)构造的误差矢量$\Psi $协方差矩阵表示为
其中B矩阵定义为：
B矩阵中真实值\({{\rm{r}}_{\rm{i}}}^0\)暂由测量值代替，使用最小二乘法对移动设备MD的位置进行第一次估计：
第一次目标位置估计完成后，可以对B矩阵进行更新，利用式(29)可以得到新的位置估计。在进行误差矢量\(\Psi\)协方差矩阵计算的过程中，假设矩阵\({Z_a}\)元素间相互独立，但是事实上并非如此，对于这个问题可以根据下式进行改进，首先计算\({Z_a}\)的协方差矩阵为
根据\({Z_a}\)矩阵元素之间的关系，构造新的误差矢量：
其中
协方差矩阵为
其中，\({x^0},{y^0}\)可由第二次目标估计的结果代替。\({Z_a}'\)第三次目标估计的结果是
混合定位方法充分发挥各自技术参数的优势，混合定位算法可以比单纯的TOA定位算法取得更精确的结果，在一定的程度上可以提升定位精度，改善定位系统的性能。
基于散射体位置信息的泰勒算法
在本课题中，我们考虑了一个简化的MIMO信道模型，在发射器和接收器之前有N个可分辨的传播路径，每个散射体只有一个反射信号。我们假设多径信号路径参数测得的如AOD, AOA和DOA相对于一个共同的方位方向[3]。
如图3所示，令\(({x_b},{y_b}),({x_m},{y_m}),({x_i},{y_i})\)表示基站（RD），移动设备（MD）和第i个散射体的真实位置。\(({x_m},{y_m})\)和\(({x_i},{y_i})\)是不知道的，但是可以被估计的。令\({r_1}\)和\({r_2}\)分别是构成第i条路径的线段的长度，最后，令\({\theta _i}\)和\({\phi _i}\)分别表示从基站到移动基站的第i条路径的到达角和出发角。从图3中，可以直接获得\({\theta _i}\)和\({\phi _i}\)的函数，以移动基站和散射体的位置为参量
\(i\)的取值为1,..,N。相似地，将c 定义为信号的传播速度，到达时间可由下式计算
此方法的目的是从不确定的测量参数\(\mathop {{\theta _i}}\limits^ \wedge ,\mathop {{\phi _i}}\limits^ \wedge ,\mathop {{t_i}}\limits^ \wedge\) 和确切的位置 \(({x_b},{y_b})\)确定未知位置\(({x_m},{y_m})\)。统计学上，测量值包含噪声，如下式
其中，对于\(\mathop {{t_i}}\limits^ \wedge ,{\mathop \theta \limits^ \wedge _i},\mathop {{\phi _i}}\limits^ \wedge\)，i取值为1到N。
在这一部分，推导了一个迭代的最小二乘的定位估计器，去解决移动基站定位的非线性TOA/AOA/AOD等式。令这个向量\(X{\rm{ = [}}{{\rm{x}}_ {\rm{m}}}{\rm{,}}{{\rm{y}}_{\rm{m}}}{\rm{,}}{{\rm{x}}_{\rm{1}}}{\rm{,}}{{\rm{y}}_{\rm{1}}}{\rm{,}}...{\rm{,}}{{\rm{x}}_ {\rm{N}}}{\rm{,}}{{\rm{y}}_{\rm{N}}}{\rm{]}}\)表示移动设备和散射体位置的先验值。定义F是一个3N的列向量
(38)式中的对于(2N+2)的向量X的估计模型，当存在加性高斯白噪声时，可以写为矩阵形式
其中，向量\(M{\rm{ = [}}\mathop {{{\rm{t}}_{\rm{i}}}}\limits^ \wedge ...\mathop {{\theta _{\rm{i}}}{\rm{.}}}\limits^ \wedge ..\mathop {{\phi _{\rm{i}}}}\limits^ \wedge {{\rm{] }}^{\rm{T}}}\)代表3N的测量数据。测量误差\(n = {[{n_{{t_i}}}...{n_{{\theta _i}}}...{n_{{\phi _i}}}]^T}\)是一个多元随机变量，并且有着以下形式的3N*3N的协方差矩阵
其中，\({Q_t}\)是到达时间TOA的协方差矩阵，\({Q_{\rm{d}}}\)和\({Q_a}\)分别是AOD（出发角度）和AOA（到达角度）的协方差矩阵。
假设X是未知的非随机变量，并且n是零均值高斯分布，最大似然估计X可由下式给出
如果\(F(X) = HX\)是一个以连续矩阵H为系数的线性矩阵，移动定位的估计位置可以由下式定位方法给出
为了确定一个简单的估计量，非线性函数\(F(X)\)必须要线性化。最直接的线性化方法是使用泰勒级数展开方法。假设\(\delta X\)很小，我们可以写为下式
F对X的梯度G定义为3N×(2N+2)的矩阵，其中第i行是标量\({F_i}\)关于X的梯度
如果\({X^{n - 1}}\)是X的近似值，并且\(\delta X\)是一个小扰动，则由式(43)可得下式
在等式（41）中表示这种关系，对于\(\delta {X^{(n - 1)}}\)的估计\(\delta X\)由(2.46)式给出，并且M由\([M{\rm{ - }}F({X^{(n - 1)}})]\)代替，H使用G来代替
提供了一个新的估计模型\({X^{(n)}} = {X^{(n - 1)}} + \delta {X^{(n - 1)}}\).
该程序可以从假设接近X的初始值\({X^{(0)}}\)开始迭代应用，然后计算一系列\({X^{(n)}}\)的估计值。这个过程不断重复，直至\(\delta X\)变得比阈值还小，表明收敛。无线MIMO通信系统中，接发端的天线阵列的优势在于可以利用多径信号的冗余性，并且得益于先进的阵列信号处理技术，更多的到达角、出发角和到达延迟等参数可以获取。该方法最大限度地减少了估计多径传输导致的误差，并通过求解一组代数位置公示，目标位置得到了估计。
代码实现的难点是计算式(44)中的雅克比矩阵，尝试过X对F(X)每一行求梯度，但是效果不理想。
多径信息辅助定位
通常利用非视距路径构造LPMD，图3表示了非视线路径(\(RD{\rm{ - }}{S_i}{\rm{ - }}MD\))，其中\({S_{j,m}}\)是未知散射点。图中\({d_{j,m}},{\theta _{j,m}}\)和\({r_ {j,m}},{\phi _{j,m}}\)分别是第j个RD和MD的第m条路径测量的TOA和AOA值，其中，\(j = 1,...,N\)，N是基站RD的数量，\(m = 2,...,M\)，即\(M{\rm{ - }}1\)是NLOS路径的数目，\(m = 1\)是代表LOS路径。在图3中，实际的散射点\({S_{j,m}}\)是在沿着参考设备\(R{D_j}\)且到达角度为\({\theta _{j,m}}\)的直线上，如果给定测量数据的集合\({d_ {j,m}},{\theta _{j,m}}\)，\({r_{j,m}},{\phi _{j,m}}\)，可以注意到\({S_{j,m}}\)的位置不是唯一的，其中一个可能的散射点\(S{'_ {j,m}}\)如图所示，散射体到可能的移动设备的路径长度为\(r{'_{{S_{j,m}}}}\)，\(MD'\)可以位于以\(S{'_{j,m}}\)为圆心，\(r{'_{{S_ {j,m}}}}\)为半径的圆周上的任意位置，但是如果给定到达角度\({\phi _ {j,m}}\)，便可以确定圆周上唯一的移动设备的位置\(MD'\)，通过上述过程，所有的可能的移动设备的位置都可以通过形成以散射体为圆心的圆来识别，位于相应的散射体圆周上的所有可能的移动设备的位置组合在一起，就形成了一条直线，这条直线被称为第 m条信号路径的可能移动设备的位置线（LPMD）。
基站\(R{D_j}\)和移动设备MD之间的另一条信号路径可以构造其他LPMD，两条LPMD的交点可以确定移动设备的位置。
多径辅助定位的方法可以参照文献[4]进行，流程图如下
衡量定位性能的基本指标
均方根误差（Root Mean Square Error，RMSE）
均方根误差是一种常用的衡量定位算法性能的指标，它反映了解算的目标位置与真实位置的偏离情况，其计算方式如下：
其中，M代表实验的蒙特卡洛方法的次数，\(\mathop {{x_i}}\limits^\^ \(代表第i次蒙特卡洛方法中解算的目标位置，\)\)表示第i次蒙特卡洛方法中真实的目标位置。
累积分布函数（Cumulative Distribution Function, CDF）
累积分布函数就是分布函数，即概率密度函数的积分，可以描述一个随机变量X的概率分布。通常计算RMSE的CDF来描述方法的性能。
对于所有实数x，累积分布函数定义如下：
其中x表示门限值，表示所有小于等于x的值出现概率的和。在蒙特卡洛实验仿真中，CDF主要统计特定的定位误差门限下某一定位误差占总体的定位误差的比例，当定位误差低于特定的定位误差门限的比例越高，表明该定位系统的定位精度越高。
克拉美罗下界（Cramér-Rao-Lower-Bound, CRLB）
克拉美罗下界表示参数无偏估计的理论下界，是定位系统衡量定位参数的一个重要指标，通常把均方误差与CRLB相比较，衡量算法的性能，同时，CRLB还可以估计服从高斯分布的参数。CRLB的核心表达为，任何无偏估计量的方差至少与 Fisher 信息矩阵（FIM）的倒数一样大，FIM表示随机变量的一个样本所能提供的关于状态参数在某种意义下的平均信息量。达到此界限的无偏估计器被称为提供最低可能的最小均方误差的有效估计器。
MATLAB仿真
实验数据的获取
可以参照文献[4]中的实验数据，考虑一下四种场景（场景A、场景B、场景C、场景D），如图所示。同样地，可以自己利用硬件获取实验数据。
1、 场景A-所有的参考设备都在移动设备的视线范围内；
2、 场景B-两个参考设备都在移动设备的视线范围内；
3、 场景C-一个参考设备都在移动设备的视线范围内；
4、 场景D-所有的参考设备都在移动设备的非视线范围内；
为了与现有的定位方案进行比较，实际的测量数据将使用均值为零和已知方差的高斯随机变量噪声产生噪声影响，估计位置的定位的均方根误差由下式计算
每种算法进行10000次的独立运算，通过其累积概率分布（CDF）曲线进行定位准确性的比较。
仿真举例：场景C定位仿真
3RD TOA LS：
clear all
clc
warning off
RD1 = [25,9];%
RD2 = [18,4];
RD3 = [3,14];%
h = zeros(3,1);
Ga = zeros(3,3);
f = zeros(1,3);
G = zeros(3,3);
a = ones(3,3);
b = zeros(3,1);
Za = zeros(3,1);
Aeq = [];
beq = [];
lb = zeros(3,1);
ub = [];
a = -1.*a;
c = 3*10^8;
pack0 = 0;%目标发出数据包时的时间
n = 0:0.1:11;
error_estimated = zeros(1,10000);
targ = [16, 8];%目标的实际位置
S11 = [21.14 12];
S21 = [13 6.5];
S31 = [13 11.9];
S11x = [20 12];%基站1第1个多界散射点，下同
S12x = [13 9.8];
S31x = [8 16.5];
S32x = [18 12.5];
S33x = [13 9.5];
t1 = sqrt((S11-targ)*(S11-targ)')/c+sqrt((RD1-S11)*(RD1-S11)')/c;
t2 = sqrt((RD2-targ)*(RD2-targ)')/c;
t3 = sqrt((S33x-targ)*(S33x-targ)')/c+sqrt((RD3-S31x)*(RD3-S31x)')/c+sqrt((S32x-S31x)*(S32x-S31x)')/c+sqrt((S32x-S33x)*(S32x-S33x)')/c;%信号由目标到基站的时间
t11 = sqrt((S12x-targ)*(S12x-targ)')/c+sqrt((RD1-S11x)*(RD1-S11x)')/c+sqrt((S12x-S11x)*(S12x-S11x)')/c;
t21 = sqrt((S21-targ)*(S21-targ)')/c+sqrt((RD2-S21)*(RD2-S21)')/c;
t31 = sqrt((S31-targ)*(S31-targ)')/c+sqrt((RD3-S31)*(RD3-S31)')/c;
t_flight = [0 t2 0];
pack_arr = pack0 + t_flight;%基站接收数据包时的时间；因目标与基站时钟有偏差，应加上;在这里假设基站和目标时钟一致
k1 = RD1(1)^2+RD1(2)^2;
k2 = RD2(1)^2+RD2(2)^2;
k3 = RD3(1)^2+RD3(2)^2;
Ga(1,:)=[-2*RD1(1) -2*RD1(2) 1]; %排列方程之后，常数矩阵B
Ga(2,:)=[-2*RD2(1) -2*RD2(2) 1];
Ga(3,:)=[-2*RD3(1) -2*RD3(2) 1];
Q = diag([(1/c)^2 (1/c)^2 (1/c)^2],0);
for i = 1:10000
distance_measure = pack_arr*3*10^8 + normrnd(0,1,1,3);%距离然后加上偏差
h(1,:)= distance_measure(1)^2 - k1; %排列方程之后，数据位置系数A设置
h(2,:)= distance_measure(2)^2 - k2;
h(3,:)= distance_measure(3)^2 - k3;
for k = 1:1
B = diag([distance_measure(1),distance_measure(2),distance_measure(3)],0);
Phi = 4*9*10^(16)*B*Q*B;%
Za = pinv(Ga'*pinv(Phi)*Ga)*Ga'*pinv(Phi)*h; %利用矩阵乘法把系数矩阵转到方程右边求解数据位置
distance_measure(1) = sqrt((RD1(1) - Za(1))^2+(RD1(2) - Za(2))^2);
distance_measure(2) = sqrt((RD2(1) - Za(1))^2+(RD2(2) - Za(2))^2);
distance_measure(3) = sqrt((RD3(1) - Za(1))^2+(RD3(2) - Za(2))^2);
h(1,:)= distance_measure(1)^2 - k1; %排列方程之后，数据位置系数A设置
h(2,:)= distance_measure(2)^2 - k2;
h(3,:)= distance_measure(3)^2 - k3;
end
B = diag([distance_measure(1),distance_measure(2),distance_measure(3)],0);
Phi = 4*9*10^(16)*B*Q*B;%
G = pinv(Phi);
[X,fval] = quadprog(G,f,a,b,Aeq,beq,lb,ub,[]);
Za = pinv(Ga'*Ga)*Ga'*(h-X);
covZa = pinv(Ga'*pinv(Phi)*Ga);
h1 = [Za(1,1)^2 Za(2,1)^2 Za(3,1)]';
B1 = diag([Za(1),Za(2),0.5],0);
Phi1 = 4*B1*covZa*B1;
Ga1 = [1 0;0 1;1 1];
Zp = pinv(Ga1'*pinv(Phi1)*Ga1)*Ga1'*pinv(Phi1)*h1;
if Za(1,1)>0
if Zp(1,1)>0
x = sqrt(Zp(1,1));
else
x = sqrt(abs(Zp(1,1)));
end
else
if Zp(1,1)>0
x= -sqrt(Zp(1,1));
else
x = -sqrt(abs(Zp(1,1)));
end
end
if Za(2,1)>0
if Zp(2,1)>0
y = sqrt(Zp(2,1));
else
y = sqrt(abs(Zp(2,1)));
end
else
if Zp(2,1)>0
y = -sqrt(Zp(2,1));
else
y = -sqrt(abs(Zp(2,1)));
end
end
targ_estimated_x = x;
targ_estimated_y = y;
error_estimated(i) = sqrt((targ_estimated_x - targ(1))^2+(targ_estimated_y - targ(2))^2);
end
hold on
figure(1)
for m = 1:110
count = 0;
for i = 1:10000
if(error_estimated(i) < n(m))
count = count+1;
end
end
xi(m) = n(m);
yi(m) = count/10000;
end
xx = 0:1:11;
plot(xi,yi,'x')
axis([0 11 0 1])
set(gca,'xtick',xx);
ylabel('probability of location error less than the ordinate','FontSize',9)
xlabel('RMS error in metres','FontSize',9)
title('Distance std variation of 1m and angle std variation of 2degree for MD location(16,8)','FontSize',10);
3RD TOA/AOA LS：
clear all
clc
RD1 = [25,9];
RD2 = [18,4];
RD3 = [3,14];
S11 = [21.14 12];
S21 = [13 6.5];
S31 = [13 11.9];
S11x = [20 12];%基站1第1个多界散射点，下同
S12x = [13 9.8];
S31x = [8 16.5];
S32x = [18 12.5];
S33x = [13 9.5];
A = zeros(4,1);
B = zeros(4,3);
f = zeros(6,1);
G = zeros(6,6);
a = ones(6,6);
b = zeros(6,1);
Za = zeros(3,1);
c = 3*10^8;
% delta_t = 2;
pack0 = 0;%目标发出数据包时的时间
n = 0:0.1:11;
error_estimated = zeros(1,10000);
targ = [16, 8];%目标的实际位置
t1 = sqrt((S11-targ)*(S11-targ)')/c+sqrt((RD1-S11)*(RD1-S11)')/c;
t2 = sqrt((RD2-targ)*(RD2-targ)')/c;
t3 = sqrt((S33x-targ)*(S33x-targ)')/c+sqrt((RD3-S31x)*(RD3-S31x)')/c+sqrt((S32x-S31x)*(S32x-S31x)')/c+sqrt((S32x-S33x)*(S32x-S33x)')/c;%信号由目标到基站的时间
t_flight = [t1 t2 t3];
pack_arr = pack0 + t_flight;%基站接收数据包时的时间；因目标与基站时钟有偏差，应加上;在这里假设基站和目标时钟一致
k1 = RD1(1)^2+RD1(2)^2;
k2 = RD2(1)^2+RD2(2)^2;
k3 = RD3(1)^2+RD3(2)^2;
B(1,:)=[-2*RD1(1) -2*RD1(2) 1]; %排列方程之后，常数矩阵B
B(2,:)=[-2*RD2(1) -2*RD2(2) 1];
B(3,:)=[-2*RD3(1) -2*RD3(2) 1];
Res = [targ(1) targ(2) targ(1)^2+targ(2)^2]';
for i = 1:10000
distance_measure(1) = pack_arr(1)*3*10^8 + randn(1)*1;%距离然后加上偏差
distance_measure(2) = pack_arr(2)*3*10^8 + randn(1)*1;
distance_measure(3) = pack_arr(3)*3*10^8 + randn(1)*1;
A(1,:)= -(k1 - distance_measure(1)^2); %排列方程之后，数据位置系数A设置
A(2,:)= -(k2 - distance_measure(2)^2);
A(3,:)= -(k3 - distance_measure(3)^2);
beta1 = atan2((S11(2)-RD1(2)),(S11(1)-RD1(1)))*180/pi + randn(1)*2;%真实值加噪声为测量值
beta2 = atan2((targ(2)-RD2(2)),(targ(1)-RD2(1)))*180/pi + randn(1)*2;
beta3 = atan2((S31x(2)-RD3(2)),(S31x(1)-RD3(1)))*180/pi + randn(1)*2;
A(4,:) = RD2(1)*tand(beta2)-RD2(2);%改distance_measure(1)*sind(randn(1)*2)
% A(5,:) = RD2(1)*tand(beta2)-RD2(2);
% A(6,:) = RD3(1)*tand(beta3)-RD3(2);
B(4,:) = [tand(beta2) -1 0];%改[sind(beta1) -cosd(beta1) 0]
% B(5,:) = [tand(beta2) -1 0];
% B(6,:) = [tand(beta3) -1 0];
C = diag([distance_measure(1),distance_measure(2),distance_measure(3),0.5],0);
Q = diag([1 1 1 4],0);
Phi = 4*C*Q*C;%9*10^(16)*
Res = pinv(B'*pinv(Phi)*B)*B'*pinv(Phi)*A;%第一次位置估计
distance_measure(1) = sqrt((RD1(1) - Res(1))^2+(RD1(2) - Res(2))^2);
distance_measure(2) = sqrt((RD2(1) - Res(1))^2+(RD2(2) - Res(2))^2);
distance_measure(3) = sqrt((RD3(1) - Res(1))^2+(RD3(2) - Res(2))^2);
beta2 = atan2((Res(2)-RD2(2)),(Res(1)-RD2(1)))*180/pi + randn(1)*2;
A(1,:)= -(k1 - distance_measure(1)^2); %排列方程之后，数据位置系数A设置
A(2,:)= -(k2 - distance_measure(2)^2);
A(3,:)= -(k3 - distance_measure(3)^2);
A(4,:) = RD2(1)*tand(beta2)-RD2(2);
B(4,:) = [tand(beta2) -1 0];
C = diag([distance_measure(1),distance_measure(2),distance_measure(3),0.5],0);
Phi = 4*C*Q*C;%9*10^(16)*
Res = pinv(B'*pinv(Phi)*B)*B'*pinv(Phi)*A;%第二次位置估计
h1 = [Res(1,1)^2 Res(2,1)^2 Res(3,1)]';
covZ = pinv(B'*pinv(Phi)*B);
C1 = diag([Res(1),Res(2),0.5],0);
Phi1 = 4*C1*covZ*C1;
Ga1 = [1 0;0 1;1 1];
Zp = (Ga1'*Phi1^(-1)*Ga1)^(-1)*Ga1'*Phi1^(-1)*h1;
x=sqrt(Zp(1,1));
y=sqrt(Zp(2,1));
targ_estimated_x = x;
targ_estimated_y = y;
error_estimated(i) = sqrt((targ_estimated_x - 16)^2+(targ_estimated_y - 8)^2);
end
hold on
figure(1)
for m = 1:110
count = 0;
for i = 1:10000
if(error_estimated(i) < n(m))
count = count+1;
end
end
xi(m) = n(m);
yi(m) = count/10000;
end
hold on
xx = 0:1:11;
plot(xi,yi,'+')
axis([0 11 0 1])
set(gca,'xtick',xx);
ylabel('probability of location error less than the ordinate','FontSize',9)
xlabel('RMS error in metres','FontSize',9)
title('Distance std variation of 1m and angle std variation of 2degree for MD location(16,8)','FontSize',10);
1RD Taylor NLOS LS：
%目前初始的散射体位置设置是随机的;求解雅克比矩阵的方法有待研究
clear all
clc
RD1 = [25,9];
RD2 = [18,4];
RD3 = [3,14];
RD1_targ = [21.14,12];
RD2_targ = [17,8];
RD3_targ = [9.5,13];
S1 = [21.14,12];
S2 = [13,9];
S3 = [7.6,16.5];
x = zeros(8,1);
M = zeros(9,1);
Res1 = zeros(1,2);
c = 3*10^8;
pack0 = 0;%目标发出数据包时的时间
n = 0:0.1:11;
% deltaX = 0.5;
error_estimated = zeros(1,10000);
targ = [16, 8];%目标的实际位置
ts11 = (sqrt((S1-targ)*(S1-targ)')+sqrt((RD1-S1)*(RD1-S1)'))/c;
thetas11 = atan2(S1(2)-RD1(2),S1(1)-RD1(1))*180/pi;
phis11 = atan2(S1(2)-targ(2),S1(1)-targ(1))*180/pi;
sigmat = 1/c;
Q = diag([sigmat^2 4 4],0);
for i = 1:10000
k = 0;
FX1 = [ts11;thetas11;phis11];
converg = false;
while ~converg
x = [16.2 8.2 randi([165,240])/10 randi([110,130])/10]';%randi([180,220])/10 12 16.3 he 10.5 是有曲线的[16.3 12.3 20.5 10.5 17 8 9.5 13]
deltaX = [100 100 10 10]';
%本次测得的TOA\AOA\AOD数据
a = normrnd(0,1,1,1);
b = normrnd(0,2,1,2);
noise = [a b(1) b(2)]';
M = FX1 + noise;%本次的测量值
while (abs(deltaX(1,1))+abs(deltaX(2,1))) >= 3
k = k+1;
%上次估计的TOA\AOA\AOD数据
t1 = (sqrt((x(3:4,:)'-x(1:2,:)')*(x(3:4,:)'-x(1:2,:)')')+sqrt((x(3:4,:)'-RD1)*(x(3:4,:)'-RD1)'))/c;
theta1 = atan2(x(4)-RD1(2),x(3)-RD1(1))*180/pi;
phi1 = atan2(x(4)-x(2),x(3)-x(1))*180/pi;
FX = [t1;theta1;phi1];%上次的FX
G = pinv(x*x')*x*FX';
G = G';
% G1 = gradient([FX(1,1);FX(2,1);FX(3,1)],x(1));
% G2 = gradient([FX(1,1);FX(2,1);FX(3,1)],x(2));
% G3 = gradient([FX(1,1);FX(2,1);FX(3,1)],x(3));
% G4 = gradient([FX(1,1);FX(2,1);FX(3,1)],x(4));
%
% G = [G1 G2 G3 G4];
deltaX = pinv(G'*pinv(Q)*G)*G'*pinv(Q)*[M-FX];
x = x + deltaX;
if k > 7
k = 0;
converg = false;
break;
else
converg = true;
end
end
end
targ_estimated_x = x(1,1);
targ_estimated_y = x(2,1);
error_estimated(i) = sqrt((targ_estimated_x - 16)^2+(targ_estimated_y - 8)^2);
end
fclose(fid);
hold on
figure(1)
for m = 1:110
count = 0;
for i = 1:10000
if(error_estimated(i) < n(m))
count = count+1;
end
end
xi(m) = n(m);
yi(m) = count/10000;
end
xx = 0:1:11;
plot(xi,yi,'v')
axis([0 11 0 1])
set(gca,'xtick',xx);
ylabel('probability of location error less than the ordinate','FontSize',9)
xlabel('RMS error in metres','FontSize',9)
title('Distance std variation of 1m and angle std variation of 2degree for MD location(16,8)','FontSize',10);
1RD LPMD NLOS LS
clear all
clc
RD1 = [25,9];
RD2 = [18,4];
RD3 = [3,14];
RD = [25 9
18 4
3 14];
A = zeros(2,1);
B = zeros(2,2);
deltajm_o = zeros(3,2);
deltajm = zeros(3,2);
C = zeros(1,2);
sigma_theta = 2;
sigma_Phi = 2;
n = 0:0.1:11;
error_estimated = zeros(1,10000);
RD1_targ = [20.5,10.5];
RD2_targ = [17,6];
RD3_targ = [9.5,13];
S11 = [21.14 12];
S21 = [13 6.5];
S31 = [13 11.9];
S11x = [20 12];%基站1第1个多界散射点，下同
S12x = [13 9.8];
S31x = [8 16.5];
S32x = [18 12.5];
S33x = [13 9.5];
targ = [16, 8];%目标的实际位置
for p = 1:10000
%LOS路径--->改：第一条路径（m=1）/主要路径
ts1 = sqrt((S11-targ)*(S11-targ)')+sqrt((RD1-S11)*(RD1-S11)')+normrnd(0,1,1,1);
ts2 = sqrt((RD2-targ)*(RD2-targ)')+normrnd(0,1,1,1);
ts3 = sqrt((S33x-targ)*(S33x-targ)')+sqrt((RD3-S31x)*(RD3-S31x)')+sqrt((S32x-S31x)*(S32x-S31x)')+sqrt((S32x-S33x)*(S32x-S33x)')+normrnd(0,1,1,1);
taus1 = sqrt((S11-targ)*(S11-targ)')+sqrt((RD1-S11)*(RD1-S11)')+normrnd(0,1,1,1);
taus2 = sqrt((RD2-targ)*(RD2-targ)')+normrnd(0,1,1,1);
taus3 = sqrt((S33x-targ)*(S33x-targ)')+sqrt((RD3-S31x)*(RD3-S31x)')+sqrt((S32x-S31x)*(S32x-S31x)')+sqrt((S32x-S33x)*(S32x-S33x)')+normrnd(0,1,1,1);
thetas1 = atan2(S11(2)-RD1(2),S11(1)-RD1(1))*180/pi+normrnd(0,2,1,1);
thetas2 = atan2(RD2_targ(2)-RD2(2),RD2_targ(1)-RD2(1))*180/pi+normrnd(0,2,1,1);
thetas3 = atan2(S31x(2)-RD3(2),S31x(1)-RD3(1))*180/pi+normrnd(0,2,1,1);
phis1 = atan2(S11(2)-targ(2),S11(1)-targ(1))*180/pi+normrnd(0,2,1,1);
phis2 = atan2(RD2_targ(2)-targ(2),RD2_targ(1)-targ(1))*180/pi+normrnd(0,2,1,1);
phis3 = atan2(S33x(2)-targ(2),S33x(1)-targ(1))*180/pi+normrnd(0,2,1,1);
%one-bound path--->改：第二条路径（m=2）/次要路径
ts11 = sqrt((S12x-targ)*(S12x-targ)')+sqrt((RD1-S11x)*(RD1-S11x)')+sqrt((S12x-S11x)*(S12x-S11x)')+normrnd(0,1,1,1);
ts21 = sqrt((S21-targ)*(S21-targ)')+sqrt((RD2-S21)*(RD2-S21)')+normrnd(0,1,1,1);
ts31 = sqrt((S31-targ)*(S31-targ)')+sqrt((RD3-S31)*(RD3-S31)')+normrnd(0,1,1,1);
taus11 = sqrt((S12x-targ)*(S12x-targ)')+sqrt((RD1-S11x)*(RD1-S11x)')+sqrt((S12x-S11x)*(S12x-S11x)')+normrnd(0,1,1,1);
taus21 = sqrt((S21-targ)*(S21-targ)')+sqrt((RD2-S21)*(RD2-S21)')+normrnd(0,1,1,1);
taus31 = sqrt((S31-targ)*(S31-targ)')+sqrt((RD3-S31)*(RD3-S31)')+normrnd(0,1,1,1);
thetas11 = atan2(S11x(2)-RD1(2),S11x(1)-RD1(1))*180/pi+normrnd(0,2,1,1);
thetas21 = atan2(S21(2)-RD2(2),S21(1)-RD2(1))*180/pi+normrnd(0,2,1,1);
thetas31 = atan2(S31(2)-RD3(2),S31(1)-RD3(1))*180/pi+normrnd(0,2,1,1);
phis11 = atan2(S12x(2)-targ(2),S12x(1)-targ(1))*180/pi+normrnd(0,2,1,1);
phis21 = atan2(S21(2)-targ(2),S21(1)-targ(1))*180/pi+normrnd(0,2,1,1);
phis31 = atan2(S31(2)-targ(2),S31(1)-targ(1))*180/pi+normrnd(0,2,1,1);
t1 = ts1+taus1+ts11+taus11;%RD1的所有路径的时间和
t2 = ts2+taus2+ts21+taus21;%RD2的所有路径的时间和
t3 = ts3+taus3+ts31+taus31;%RD3的所有路径的时间和
w11o = 2*1/6*(t1+t2+t3)/(ts1+taus1);
w12o = 2*1/6*(t1+t2+t3)/(ts11+taus11);
w21o = 2*1/6*(t1+t2+t3)/(ts2+taus2);
w22o = 2*1/6*(t1+t2+t3)/(ts21+taus21);
w31o = 2*1/6*(t1+t2+t3)/(ts3+taus3);
w32o = 2*1/6*(t1+t2+t3)/(ts31+taus31);
w11 = w11o/(w11o+w12o+w21o+w22o+w31o+w32o);
w12 = w12o/(w11o+w12o+w21o+w22o+w31o+w32o);
w21 = w21o/(w11o+w12o+w21o+w22o+w31o+w32o);
w22 = w22o/(w11o+w12o+w21o+w22o+w31o+w32o);
w31 = w31o/(w11o+w12o+w21o+w22o+w31o+w32o);
w32 = w32o/(w11o+w12o+w21o+w22o+w31o+w32o);
tjm = [ts1 ts11
ts2 ts21
ts3 ts31];
taujm = [taus1 taus11
taus2 taus21
taus3 taus31];
theta = [thetas1 thetas11
thetas2 thetas21
thetas3 thetas31];
phi = [phis1 phis11
phis2 phis21
phis3 phis31];
wjm = [w11 w12
w21 w22
w31 w32];
%RD1
z111x = RD1(1) - taujm(1,1)*cosd(phi(1,1));%RD1的第1条路径的第1个端点的x,下同
z111y = RD1(2) - taujm(1,1)*sind(phi(1,1));%RD1的第1条路径的第1个端点的y,下同
z112x = RD1(1) + tjm(1,1)*cosd(theta(1,1));
z112y = RD1(2) + tjm(1,1)*sind(theta(1,1));
z113x = (z111x + z112x)/2;
z113y = (z111y + z112y)/2;
z11 = [z111x z111y z112x z112y z113x z113y];
z121x = RD1(1) - taujm(1,2)*cosd(phi(1,2));%RD1的第2条路径的第1个端点的x,下同
z121y = RD1(2) - taujm(1,2)*sind(phi(1,2));%RD1的第2条路径的第1个端点的y,下同
z122x = RD1(1) + tjm(1,2)*cosd(theta(1,2));
z122y = RD1(2) + tjm(1,2)*sind(theta(1,2));
z123x = (z121x + z122x)/2;
z123y = (z121y + z122y)/2;
z12 = [z121x z121y z122x z122y z123x z123y];
%RD2
z211x = RD2(1) - taujm(2,1)*cosd(phi(2,1));%RD2的第1条路径的第1个端点的x,下同
z211y = RD2(2) - taujm(2,1)*sind(phi(2,1));%RD2的第1条路径的第1个端点的y,下同
z212x = RD2(1) + tjm(2,1)*cosd(theta(2,1));
z212y = RD2(2) + tjm(2,1)*sind(theta(2,1));
z213x = (z211x + z212x)/2;
z213y = (z211y + z212y)/2;
z21 = [z211x z211y z212x z212y z213x z213y];
z221x = RD2(1) - taujm(2,2)*cosd(phi(2,2));%RD2的第2条路径的第1个端点的x,下同
z221y = RD2(2) - taujm(2,2)*sind(phi(2,2));%RD2的第2条路径的第1个端点的y,下同
z222x = RD2(1) + tjm(2,2)*cosd(theta(2,2));
z222y = RD2(2) + tjm(2,2)*sind(theta(2,2));
z223x = (z221x + z222x)/2;
z223y = (z221y + z222y)/2;
z22 = [z221x z221y z222x z222y z223x z223y];
% z22 = z21;
% z22 = [0 0 0 0 0 0];
%RD3
z311x = RD3(1) - taujm(3,1)*cosd(phi(3,1));%RD3的第1条路径的第1个端点的x,下同
z311y = RD3(2) - taujm(3,1)*sind(phi(3,1));%RD3的第1条路径的第1个端点的y,下同
z312x = RD3(1) + tjm(3,1)*cosd(theta(3,1));
z312y = RD3(2) + tjm(3,1)*sind(theta(3,1));
z313x = (z311x + z312x)/2;
z313y = (z311y + z312y)/2;
z31 = [z311x z311y z312x z312y z313x z313y];
z321x = RD3(1) - taujm(3,2)*cosd(phi(3,2));%RD3的第2条路径的第1个端点的x,下同
z321y = RD3(2) - taujm(3,2)*sind(phi(3,2));%RD3的第2条路径的第1个端点的y,下同
z322x = RD3(1) + tjm(3,2)*cosd(theta(3,2));
z322y = RD3(2) + tjm(3,2)*sind(theta(3,2));
z323x = (z321x + z322x)/2;
z323y = (z321y + z322y)/2;
z32 = [z321x z321y z322x z322y z323x z323y];
zjmp = cell(3,2);
zjmp = {z11 z12
z21 z22
z31 z32};
[r,c] = size(wjm);
zsumx = 0;
zsumy = 0;
count = 0;
for i = 1:r
for j = 1:c
if wjm(i,j)>0.1
count = count+1;
zjm = cell2mat(zjmp(i,j));
zsumx = zsumx+zjm(5);
zsumy = zsumy+zjm(6);
end
end
end
C(1,1) = zsumx/count;%(r*c*3)zsumx/count
C(1,2) = zsumy/count;%zsumy/count
for i = 1:r
for j = 1:c
zjm = cell2mat(zjmp(i,j));
deltajm_o(i,j) = sqrt((C-zjm(:,5:6))*(C-zjm(:,5:6))');
end
end
% deltajm_o(2,2) = 0;
deltajm_sum = sum(sum(deltajm_o));
for i = 1:r
for j = 1:c
deltajm(i,j) = deltajm_o(i,j)/deltajm_sum;
end
end
for i = 1:r
if deltajm(i,1)<0.2&&deltajm(i,2)<0.2
q = i;
theta11 = theta(i,1);
Phi11 = phi(i,1);
theta12 = theta(i,2);
Phi12 = phi(i,2);
d11 = tjm(i,1);
r11 = taujm(i,1);
d12 = tjm(i,2);
r12 = taujm(i,2);
end
end
if (abs(abs(theta11)-abs(Phi11))<=174)||(abs(abs(theta11)-abs(Phi11))<=186)
else
theta11 = theta(q,2);
Phi11 = phi(q,2);
theta12 = theta(q,1);
Phi12 = phi(q,1);
d11 = tjm(q,2);
r11 = taujm(q,2);
d12 = tjm(q,1);
r12 = taujm(q,1);
end
noise = abs(theta11)+abs(Phi11)-180;
%根据误差权重尽量消除角度误差
if theta11 >= 0
theta11 = theta11 - noise*(sigma_theta/(sigma_theta+sigma_Phi));
else
theta11 = theta11 + noise*(sigma_theta/(sigma_theta+sigma_Phi));
end
muN12 = d12*sind(theta12) + r12*sind(Phi12);
muD12 = d12*cosd(theta12) + r12*cosd(Phi12);
A(1,:) = RD(q,1)*sind(theta11) - RD(q,2)*cosd(theta11);
A(2,:) = muN12*RD(q,1) - muD12*RD(q,2)+d12*r12*sind(Phi12-theta12);
B(1,:) = [sind(theta11) -cosd(theta11)];%改[sind(beta1) -cosd(beta1) 0]
B(2,:) = [muN12 -muD12];
Res = pinv(B'*B)*B'*A;%第一次位置估计
targ_estimated_x = Res(1);
targ_estimated_y = Res(2);
error_estimated(p) = sqrt((targ_estimated_x - 16)^2+(targ_estimated_y - 8)^2);
end
hold on
figure(1)
for m = 1:110
count = 0;
for i = 1:10000
if(error_estimated(i) < n(m))
count = count+1;
end
end
xi(m) = n(m);
yi(m) = count/10000;
end
xx = 0:1:11;
plot(xi,yi,'-.')%,'linewidth',1
axis([0 11 0 1])
set(gca,'xtick',xx);
ylabel('probability of location error less than the ordinate','FontSize',9)
xlabel('RMS error in metres','FontSize',9)
title('Distance std variation of 1m and angle std variation of 2degree for MD location(16,8)','FontSize',10);
参考文献
[1] Xin Wang, Zongxin Wang and B. O'Dea, "A TOA-based location algorithm reducing the errors due to non-line-of-sight ( NLOS) propagation," in IEEE Transactions on Vehicular Technology, vol. 52, no. 1, pp. 112-116, Jan. 2003, doi: 10.1109/TVT.2002.807158.
[2] 邓平,刘林,范平志.一种改进的TOA/AOA混合定位算法[J].电路与系统学报,2003(04):54-57.
[3] J. Li, J. Conan and S. Pierre, "Mobile Station Location Estimation for MIMO Communication Systems," 2006 3rd International Symposium on Wireless Communication Systems, 2006, pp. 561-564, doi: 10.1109/ISWCS.2006.4362361.
[4] C. K. Seow and S. Y. Tan, "Non-Line-of-Sight Localization in Multipath Environments," in IEEE Transactions on Mobile Computing, vol. 7, no. 5, pp. 647-660, May 2008, doi: 10.1109/TMC.2007.70780.