一、实现要求

要求一：编写两个函数TOA_LLOP和TOA_CHAN得到位置的估计。

要求二：用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线对比图，横坐标为噪声方差，纵坐标为RMSE。

二、仿真方案设计

TOA原理：测量待定位节点MS（x，y）与发送端（xi，yi）的信号之间的到达时间，然后转换为距离，从而进行定位。三个基站到MS的距离分别为r1，r2，r3，以各自基站为圆心测量距离为半径，绘制三个圆，其交点即为 MS 的位置。当三个基站都是 LOS 基站时，一般可以根据最小二乘(LS)算法计算 MS的估计位置。假设 MS 位的置坐标为 (x,y)，N 个 BS的位置坐标为 (xi,yi))，根据其几何意义，则它们之间满足的关系是


我们要求得坐标 (x,y)，即求得 X。利用最小二乘法可得

三、代码实现

1.LLOP代码（matlab）

function theta=TOALLOP(A,p,j)
[m,~]=size(A);  
k=sum(A.^2,2);
k1=k([1:j-1,j+1:m],:); 
A1=A([1:j-1,j+1:m],:); 
A2=A1-ones(m-1,1)*A(j,:); 
p1=p([1:j-1,j+1:m],:); 
p2=p(j).^2*ones(m-1,1)-p1.^2-(k(j)*ones(m-1,1)-k1); 
theta=1/2*inv(A2'*A2)*A2'*p2; theta=theta';

2.CHAN代码（matlab）

function theta=TOACHAN(A,p,sigma) 
[m,~]=size(A);
k=sum(A.^2,2); 
A1=[-2*A,ones(m,1)]; 
p1=p.^2-k; 
B1=diag(2*p);
Q1=diag(ones(m,1)*sigma); 
cov1=B1*Q1*B1; 
theta1=inv(A1'*inv(cov1)*A1)*A1'*inv(cov1)*p1; 
cov_theta1=inv(A1'*inv(cov1)*A1); 
A2=[1,0;0,1;1,1]; p2=[theta1(1,1)^2;theta1(2,1)^2;theta1(3,1)]; 
B2=diag([2*theta1(1,1);
2*theta1(2,1);1]); 
cov2=B2*cov_theta1*B2; 
theta2=inv(A2'*inv(cov2)*A2)*A2'*inv(cov2)*p2; 
theta=sign(theta1(1:2,:)).*theta2.^(1/2); 
theta=theta';

3.主函数代码（matlab）

 %主函数
clear all; 
clc; 
BS1=[0,0];
BS2=[500,0];
BS3=[500,500];
BS4=[0,500]; 
MS=[100,100]; %移动台MS的估计位置
std_var=[1e-2,1e-1,1,1e1,1e2]; %范围矩阵
A=[BS1;BS2;BS3;BS4];   
number=10000; 
for j=1:length(std_var) %从1循环到std_var的长度
    error1=0;%初始误差为0
    error2=0; %初始误差为0
    std_var1=std_var(j);
 for i=1:number   %多次循环
    r1=A-ones(4,1)*MS;  %矩阵A减去4*1的全一矩阵乘以MS
    %r1=A-ones(3,1)*MS;
    %r1=A-ones(5,1)*MS; 
    r2=sum(r1.^2,2); %矩阵r1每个元素分别平方，得到新矩阵，在行求和，最为矩阵r2
    r=r2.^(1/2)+std_var1*randn(4,1); %从移动到位置MS发射信号到达基站i的TOA测量值
    %r=r2.^(1/2)+std_var1*randn(3,1); 
    %r=r2.^(1/2)+std_var1*randn(5,1); 
    theta1=TOALLOP(A,r,1); % 调用TOALLOP函数
    theta2=TOACHAN(A,r,std_var1^2); % 调用TOACHAN函数
    error1=error1+norm(MS-theta1)^2; %norm是返回MS-theta1的最大奇异值，即max(svd(MS-theta1))，
    error2=error2+norm(MS-theta2)^2; %移动台MS估计位置与计算的到的距离的平方
 end 
   RMSE1(j)=(error1/number)^(1/2); %求TOALLOP均方根误差
   RMSE2(j)=(error2/number)^(1/2);%求TOACHAN均方根误差
end
% plot
semilogx(std_var,RMSE1,'-O',std_var,RMSE2,'-s') 
xlabel('测量噪声标准差（m） '); 
legend('TOALLOP','TOACHAN');
ylabel('RMSE'); 
legend('TOA-LLOP','TOA-CHAN')

四、结果图

移动台估计位置为(200，200)

移动台估计位置为(300，200)

移动台估计位置为(300，300)

从上面移动台不同位置所估出来的RMSE值来看，CHAN算法的性能要优于LLOP算法，能对位置估计更加准确。此外，噪声对位置的定位估计影响也比较大，随着噪声的增加，位置的估计也就越不准确。

五、python实现LLOP算法

1.引入库

import numpy as np
import random
from numpy import *
from scipy.optimize import leastsq
from scipy.stats import norm
import matplotlib.pyplot as plt
import matplotlib as mpl

2.LLOP代码（python）

def TOALLOP(A,p,j):
    m=A.shape[0]
    #t1=A**2
    #k=np.mat([sum(t1[0])],[sum(t1[1])],[sum(t1[2])],[sum(t1[3])])
    k = (np.multiply(A,A)).sum(axis=1)
    k1=k[j]
    A1=A[j]
    A2 = A1 - np.ones((m,1))*A[j]
    p1=p[j]
    M = np.dot(np.multiply(p[j],p[j]),np.ones((m,1)))
    N = np.multiply(p1,p1)
    p2=np.dot(np.multiply(p[j],p[j]),np.ones((m,1)))-np.multiply(p1,p1)-(np.dot(k[j],np.ones((m-1,1)))-k1)
    theta = 1/2*np.matrix(A2).I
    theta = theta.T
    return theta

3.主函数

BS1=np.array([0,0])
BS2=np.array([500,0])
BS3=np.array([500,500])
BS4=np.array([0,500])
MS=np.array([200,200])
std_var=np.array([1e-2,1e-1,1,1e1,1e2])
A=np.vstack((BS1,BS2,BS3,BS4))
number=10000
for j in range(len(std_var)):
    error1=0
    error2=0
    std_var1=std_var[j]
    RMSE1 = np.zeros(shape=(len(std_var),1))
    RMSE2 = np.zeros(shape=(len(std_var),1))
    for i in range(0,number):
        r1=A-np.multiply(np.ones((4,1)),MS)
        r2 = (np.multiply(r1,r1)).sum(axis=1)
        r=np.square(r2)+std_var1*np.random.randn(1,4)
        theta1=TOALLOP(A,r,0)
        #theta2=TOALLOP(A,r,std_var1**2)
        theta2=TOALLOP(A,r,0)
        error1 = error1 + np.linalg.norm(MS-theta1)**2
        error2 = error2 + np.linalg.norm(MS-theta2)**2
    RMSE1[j] = (error1/number)**(1/2)
    RMSE2[j] = (error2/number)**(1/2)
    # print(RMSE1)
# print(RMSE2)
# 为了显示中文
mpl.rcParams['font.sans-serif'] = [u'SimHei']
mpl.rcParams['axes.unicode_minus'] = False
plt.semilogx(std_var,RMSE1,'-0',std_var,RMSE2,'-s')
plt.xlabel('测量噪声标准差（m） ')
#plt.legend('TOALLOP','TOACHAN')
plt.ylabel('RMSE')
#plt.legend('TOA-LLOP','TOA-CHAN')

感觉效果不是太好可能是方法没用对，仅供参考

六、参考文章

https://blog.csdn.net/qq_23947237/article/details/82738191