matlab实现TOA定位算法
文章目录一、实现要求二、仿真方案设计三、代码实现1.LLOP代码(matlab)2.CHAN代码(matlab)3.主函数代码(matlab)四、结果图五、python实现LLOP算法1.引入库2.LLOP代码(python)3.主函数六、参考文章一、实现要求要求一:编写两个函数TOA_LLOP和TOA_CHAN得到位置的估计。要求二:用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线
文章目录
一、实现要求
要求一:编写两个函数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
![Logo](https://devpress.csdnimg.cn/79de2bf0b7994defa4242ef90d5513fa.jpg)
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)