要求一:编写两个函数TOA_LLOP和TOA_CHAN得到位置的估计。
要求二:用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线对比图,横坐标为噪声方差,纵坐标为RMSE。
主函数
设计三个接收机的位置和移动台的位置
得到范围矩阵
初始化误差为0
多次循环
矩阵元素全部平方得到新矩阵
求均方根误差
函数toallop
取出矩阵的行数、列数
将矩阵元素平方在行求和得到新矩阵
行与对应行相减
利用最小二乘法求解
Toachan函数
取出矩阵的行数、列数
将矩阵元素平方在行求和得到新矩阵
乘以矩阵m*1的全一矩阵
得到toa协方差矩阵
在运用最大释然估计
%TOA:
% %% the simulation of TOA localization algorithm
clear all;
clc;
%接收机的位置坐标,简单实验就可采用少量样本,精准实验必须采用大量样本数量
BS1=[0,0];
BS2=[500,0];
BS3=[500,500];
%BS4=[0,500];
%BS5=[800,800];
MS=[250,250]; %移动台MS的估计位置
std_var=[1e-2,1e-1,1,1e1,1e2]; %范围矩阵
%A=[BS1;BS2;BS3;BS4]; %矩阵A包含基站的坐标
A=[BS1;BS2;BS3];
%A=[BS1;BS2;BS3;BS4;BS5];
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') %x轴取对数,X轴范围是1e-2到1e2,Y轴的范围是变动的
xlabel('测量噪声标准差(m) ');
legend('TOALLOP','TOACHAN');
ylabel('RMSE');
legend('TOA-LLOP','TOA-CHAN')
%TOALLOP:
function theta=TOALLOP(A,p,j)
% A is the coordinate of BSs
%A是BBS的坐标
% p is the range measurement
%P是范围测量
% j is the index of reference BS
%J是参考BS的索引
[m,~]=size(A); %size得到A的行列数赋值给[m,~],~表示占位,就是只要行m的值!
k=sum(A.^2,2);%矩阵A每个元素分别平方,得到新矩阵,在行求和,最为矩阵K
k1=k([1:j-1,j+1:m],:); %取出J行
A1=A([1:j-1,j+1:m],:); %取出J行
A2=A1-ones(m-1,1)*A(j,:); %得到D,就是j行与其余行对应值相减
p1=p([1:j-1,j+1:m],:); %取出J行
p2=p(j).^2*ones(m-1,1)-p1.^2-(k(j)*ones(m-1,1)-k1); %得到b,(Rn*Rn-R1*R1-Kn+K1)其中Kn为对应第n个x^2+y^2
theta=1/2*inv(A2'*A2)*A2'*p2; %利用最小二乘解,得
theta=theta';%转换为(x,y)形式
end
import random
import numpy as np
import time
import math
import cmath
import matplotlib.pyplot as plt#约定俗成的写法plt
def n2pingfangxiangjia(r1):#n*2矩阵每一排平方相加变为n*1矩阵
r=[]
for ii in range(4):
for jj in range(1):
temp=r1[ii][jj]*r1[ii][jj]+r1[ii][jj+1]*r1[ii][jj+1]
r.append(temp)
return r
def jiayi(a):#每一排后面加1元素
a=a.tolist()
for i in range(4):
a[i].append(1)
a=np.array(a)
return a
def zhongjian(a):#去元素对角线值
aa=[]
for i in range(4):
aa.append(a[i][i])
return aa
def TOA_LLOP(A,p,j):
m=4
k=n2pingfangxiangjia(A)
k1=k[1:4]
k1[1]=k1[1]-50000
A1=A[1:4]
A2=np.array(A1)
p1=p[1:4]
a=p[j-1]*p[j-1]*np.ones((m-1,1))
b=np.array(p1)*np.array(p1)
c=k[j-1]*np.ones((m-1,1))-k1
p2=a-b-c
p2=p2[0]
a=np.dot((A2.T),A2)
b=0.5*np.linalg.inv(a)
c=np.dot(b,A2.T)
theta=np.dot(c,p2)
theta=theta.T
return theta
def isNan_1(a):
return math.isnan(a)
def TOA_CHAN(A,p,sigima,j):
A=np.array(A)
m=A.shape[0]
k=np.sum(A **2,axis=1)
k=np.array([k]).T
#print(k)
A1=np.hstack([-2*A,np.ones((m,1))])
p1=p **2-k
p4=(2*p).reshape(m,)
B1=np.diag(p4)
p3=(np.ones((m,1)) * sigima).reshape(m,)
Q1=np.diag(p3)
cov1=np.dot(np.dot(B1,Q1),B1)
a=np.dot(np.dot(A1.T,np.linalg.inv(cov1)),A1)
a=np.linalg.inv(a)
theta1=np.dot(np.dot(np.dot(a,A1.T),np.linalg.inv(cov1)),p1)
cov_theta1=np.linalg.inv(np.dot(np.dot(A1.T,np.linalg.inv(cov1)),A1))
#print(cov_theta1)
#theta1=np.dot(np.dot(np.dot(np.linalg.inv(np.dot(np.dot(A.T,np.linalg.inv(cov1),A1 )),A1.T ),np.linalg.inv(cov1)),p1)
#cov_theta1=np.linalg.inv(np.dot(np.dot(A1.T,np.linalg.inv(cov1)),A1))
A2=np.array([[1,0],[0,1],[1,1]])
p2=np.array([[theta1[(0,0)] ** 2],[theta1[(1,0)]**2],[theta1[(2,0)]]])
B2=np.diag(np.array([[2*theta1[0,0]],[2*theta1[1,0]],[1]]).reshape(3,))
cov2=np.dot(np.dot(B2,cov_theta1),B2)
b=np.dot(np.dot(A2.T,np.linalg.inv(cov2)),A2)
b=np.linalg.inv(b)
theta2=np.dot(np.dot(np.dot(b,A2.T),np.linalg.inv(cov2)),p2)
theta=np.sqrt((np.sign(theta1[0:2,:])*theta2))
theta=theta.T[0]
if j==4:
for i in range(2):
if np.isnan(theta[i]):
theta[i]=0
return theta
a=time.time()
BS1=[0,0]
BS2=[500,0]
BS3=[500,500]
BS4=[0,500]
MS=[200,200]
std_var=[0.01,0.1,1,10,100]
A=[BS1,BS2,BS3,BS4]
number=10000
RMSE1=[]
RMSE2=[]
tmp=[]
for j in range(len(std_var)):
error1=0
error2=0
std_var1=std_var[j]
for i in range(number):
r1=A-np.ones((4,1))*MS
r2=[]
for ii in range(4):
for jj in range(1):
temp=r1[ii][jj]*r1[ii][jj]+r1[ii][jj+1]*r1[ii][jj+1]
temp=math.sqrt(temp)+std_var1*np.random.randn(4,1)
r2.append(temp[0].tolist())
r=r2
r=np.array(r)
theta1=TOA_LLOP(A,r,1) # LLOP
theta2=TOA_CHAN(A,r,std_var1*std_var1,j) # CHAN
error1=error1+np.linalg.norm(MS-theta1)*np.linalg.norm(MS-theta1)
error2=error2+np.linalg.norm(MS-theta2)*np.linalg.norm(MS-theta2)
RMSE1.append(math.sqrt(error1/number))
RMSE2.append(np.sqrt(error2/number))
b=time.time()
plt.axes(xscale="log")
plt.plot(std_var,RMSE1,'ro-', color='red', alpha=0.8, linewidth=1, label='用户1')
plt.plot(std_var,RMSE2,'bo-', color='green', alpha=0.8, linewidth=1, label='用户2')
plt.legend(loc="upper right")
plt.xlabel('The standard deviation of measurement noise (m')
plt.ylabel("RMSE")
#用来正常显示中文标签
plt.rcParams['font.sans-serif']=['SimHei']
plt.rcParams['axes.unicode_minus']=False
time_end=time.time()
plt.show()