基于python与matlab的TOA定位算法性能仿真

仿真要求

要求一：编写两个函数TOA_LLOP和TOA_CHAN得到位置的估计。
要求二：用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线对比图，横座标为噪声方差，纵座标为RMSE。

仿真方案的设计

主函数
设计三个接收机的位置和移动台的位置
得到范围矩阵
初始化误差为0
多次循环
矩阵元素全部平方得到新矩阵
求均方根误差
函数toallop
取出矩阵的行数、列数
将矩阵元素平方在行求和得到新矩阵
行与对应行相减
利用最小二乘法求解
Toachan函数
取出矩阵的行数、列数
将矩阵元素平方在行求和得到新矩阵
乘以矩阵m*1的全一矩阵
得到toa协方差矩阵
在运用最大释然估计

matlab仿真代码

%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

python仿真代码

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()

仿真结果