TOA定位算法性能仿真

一、题目

TOA定位算法性能仿真

二、仿真要求

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

三、主要仿真代码

3.1、函数TOA_LLOP和TOA_CHAN主要代码

函数TOA_LLOP 主要代码:

%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

函数TOA_CHAN

%TOACHAN:
function theta=TOACHAN(A,p,sigma) 
% A is the coordinate of BSs 
%A是BBS的坐标
% p is the range measurement 
%P是范围测量
% sigma is the the variance of TOA measurement 
%sigma是TOA测量的方差
[m,~]=size(A); %size得到A的行列数赋值给[m,~],~表示占位,就是只要行m的值! 
k=sum(A.^2,2); %矩阵A每个元素分别平方,得到新矩阵,在行求和,最为矩阵K
A1=[-2*A,ones(m,1)]; %-2乘以矩阵A与一个m*1的全一矩阵,组成新矩阵A1,也就是Gn
p1=p.^2-k; %得到h
B1=diag(2*p);%得到B,基站与移动台的真实值
Q1=diag(ones(m,1)*sigma); %TOA协方差矩阵
cov1=B1*Q1*B1; %由于B1中有2,此处就可以省略4
theta1=inv(A1'*inv(cov1)*A1)*A1'*inv(cov1)*p1; %得到Za,因为B中包含MS与个基站之间的距离,Q1是未知量,需进一步近似
cov_theta1=inv(A1'*inv(cov1)*A1); %上式采用扰动算法得
A2=[1,0;0,1;1,1]; %得到Ga‘
p2=[theta1(1,1)^2;
theta1(2,1)^2;
theta1(3,1)]; %得到h‘
B2=diag([2*theta1(1,1);
2*theta1(2,1);1]); %得到B’
cov2=B2*cov_theta1*B2; %误差向量的协方差矩阵
theta2=inv(A2'*inv(cov2)*A2)*A2'*inv(cov2)*p2; %运用最大似然估计得到
theta=sign(theta1(1:2,:)).*theta2.^(1/2); %得到MS位置的估计值坐标,以及符号
theta=theta';%转换为(x,y)形式
end

3.2.RMSE函数代码

%TOA: 
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')

3.3、全部代码


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

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

function theta=TOACHAN(A,p,sigma) 
% A is the coordinate of BSs 
%A是BBS的坐标
% p is the range measurement 
%P是范围测量
% sigma is the the variance of TOA measurement 
%sigma是TOA测量的方差
[m,~]=size(A); %size得到A的行列数赋值给[m,~],~表示占位,就是只要行m的值! 
k=sum(A.^2,2); %矩阵A每个元素分别平方,得到新矩阵,在行求和,最为矩阵K
A1=[-2*A,ones(m,1)]; %-2乘以矩阵A与一个m*1的全一矩阵,组成新矩阵A1,也就是Gn
p1=p.^2-k; %得到h
B1=diag(2*p);%得到B,基站与移动台的真实值
Q1=diag(ones(m,1)*sigma); %TOA协方差矩阵
cov1=B1*Q1*B1; %由于B1中有2,此处就可以省略4
theta1=inv(A1'*inv(cov1)*A1)*A1'*inv(cov1)*p1; %得到Za,因为B中包含MS与个基站之间的距离,Q1是未知量,需进一步近似
cov_theta1=inv(A1'*inv(cov1)*A1); %上式采用扰动算法得
A2=[1,0;0,1;1,1]; %得到Ga‘
p2=[theta1(1,1)^2;
theta1(2,1)^2;
theta1(3,1)]; %得到h‘
B2=diag([2*theta1(1,1);
2*theta1(2,1);1]); %得到B’
cov2=B2*cov_theta1*B2; %误差向量的协方差矩阵
theta2=inv(A2'*inv(cov2)*A2)*A2'*inv(cov2)*p2; %运用最大似然估计得到
theta=sign(theta1(1:2,:)).*theta2.^(1/2); %得到MS位置的估计值坐标,以及符号
theta=theta';%转换为(x,y)形式
end

四、仿真结果

在这里插入图片描述大家觉得有用,记得点赞收藏加转发。阿陈给大家磕头了,砰砰砰。
在这里插入图片描述

展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 深蓝海洋 设计师: CSDN官方博客
应支付0元
点击重新获取
扫码支付

支付成功即可阅读