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

仿真結果