在matlab下实现TDOA定位算法性能仿真
2024-06-17 03:36:48
一、题目
TDOA定位算法性能仿真
二、仿真要求
要求一：编写两个函数TDOA_CHAN和TDOA_Taylor得到位置的估计。
要求二：用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线对比图，横坐标为噪声方差，纵坐标为RMSE。
三、主要仿真代码
3.1、函数TDOA_CHAN和主要代码
函数TDOA_CHAN主要代码：
%TDOA-CHAN:
function theta=TDOACHAN(A,p,sigma)
% A is the coordinate of BSs
%A是BSS的坐标
% p is the range measurement
%P是范围测量
% sigma is the the variance of TDOA measurement
%sigma是TDOA测量的方差
[m,~]=size(A); %size得到A的行列数赋值给[m,~]，~表示占位，就是只要行m的值！
k=sum(A.^2,2); %矩阵A每个元素分别平方，得到新矩阵，在行求和，最为矩阵K
G1=[A(2:end,:)-ones(m-1,1)*A(1,:),p]; %得到Xm1,Ym1,Rm1,的值，m取值[2,i],构建矩阵Ga
h1=1/2*(p.^2-k(2:end,:)+ones(m-1,1)*k(1,:)); %构建矩阵h
Q=diag(ones(m-1,1)*sigma); %构建TDOA的协方差矩阵
% initial estimate
theta0=inv(G1'*inv(Q)*G1)*G1'*inv(Q)*h1; %通过一次WLS算法进行求解，
s=A(2:end,:)-ones(m-1,1)*theta0(1:2,:)';
d=sum(s.^2,2);%矩阵s每个元素分别平方，得到新矩阵，在行求和，最为矩阵d
B1=diag(d.^(1/2));
cov1=B1*Q*B1;
% first wls
theta1=inv(G1'*inv(cov1)*G1)*G1'*inv(cov1)*h1; %进行第一次WLS计算
cov_theta1=inv(G1'*inv(cov1)*G1); %得到theta1的协方差矩阵
% second wls
G2=[1,0;0,1;1,1]; %构建G'
h2=[(theta1(1,1)-A(1,1))^2;(theta1(2,1)-A(1,2))^2;theta1(3,1)^2]; %构建h'
B2=diag([theta1(1,1)-A(1,1),theta1(2,1)-A(1,2),theta1(3,1)]); %构建b'
cov2=4*B2*cov_theta1*B2; %得到误差矢量的协方差矩阵。
theta2=inv(G2'*inv(cov2)*G2)*G2'*inv(cov2)*h2; %运用最大似然估计得到
theta=theta2.^(1/2)+[A(1,1);A(1,2)]; %得到MS位置的估计值坐标，以及符号
theta=theta';%转换为（x,y）形式
end
函数TDOA_Taylor主要代码：
%TDOA-Talayor:
function theta=TDOATaylor(A,p,sigma)
% A is the coordinate of BSs% p is the range measurement
% sigma is the the variance of TOA measurement
% initial estimate
theta0=TDOACHAN(A,p,sigma); %调用TDOACHAN得到一个初始的估计位置
delta=norm(theta0); %得到范数
while norm(delta)>1e-2 %得到足够小的值[m,~]=size(A); %size得到A的行列数赋值给[m,~]，~表示占位，就是只要行m的值！d=sum((A-ones(m,1)*theta0).^2,2); R=d.^(1/2); G1=ones(m-1,1)*(A(1,1)-theta0(1,1))/R(1,1)-(A(2:m,1)-theta0(1,1))./R(2:m,:); G2=ones(m-1,1)*(A(1,2)-theta0(1,2))/R(1,1)-(A(2:m,2)-theta0(1,2))./R(2:m,:); G=[G1,G2]; %构建Gth=p-(R(2:m,:)-ones(m-1,1)*R(1,:)); %构建HtQ=diag(ones(m-1,1)*sigma); %TDOA测量值的协方差矩阵delta=inv(G'*inv(Q)*G)*G'*inv(Q)*h; %加权最小二乘解theta0=theta0+delta'; %累加
end
3.2、函数RMSE主要代码
函数RMSE主要代码：
%TDOA：
% %% the simulation of TDOA localization algorithm
clear all;
clc;
%定义四个参与基站的坐标位置
BS1=[0,0];
BS2=[500,0];
BS3=[500,500];
BS4=[0,500];
%BS5=[600,500];
%移动台MS的初始估计位置
MS=[550,150];
std_var=[1e-2,5e-2,1e-1,5e-1,1]; %范围
%A=[BS1;BS2;BS3;BS4]; %矩阵A包含4个初始坐标
A=[BS1;BS2;BS3;BS4];
number=10000;
for j=1:length(std_var) %循环error1=0;%初始误差置为0error2=0; %初始误差置为0std_var1=std_var(j);%令std_var1等于当前数组的值for i=1:number %多次循环%r1=A-ones(4,1)*MS; r1=A-ones(4,1)*MS;r2=(sum(r1.^2,2)).^(1/2); %r=r2(2:end,:)-ones(3,1)*r2(1,:)+std_var1*randn(3,1); %表示从[2,i]开始MS与基站i和基站1的距离差r=r2(2:end,:)-ones(3,1)*r2(1,:)+std_var1*randn(3,1);sigma=std_var1^2; theta1=TDOACHAN(A,r,sigma); % 调用TDOACHAN函数theta2=TDOATaylor(A,r,sigma); %调用TDOATalor函数error1=error1+norm(MS-theta1)^2; %移动台MS估计位置与计算的到的距离的平方error2=error2+norm(MS-theta2)^2; %移动台MS估计位置与计算的到的距离的平方end RMSE1(j)=(error1/number)^(1/2); %均方根误差RMSE2(j)=(error2/number)^(1/2); %均方根误差
end
3.3、全部代码
%TDOA：
% %% the simulation of TDOA localization algorithm
clear all;
clc;
%定义四个参与基站的坐标位置
BS1=[0,0];
BS2=[500,0];
BS3=[500,500];
BS4=[0,500];
%BS5=[600,500];
%移动台MS的初始估计位置
MS=[550,150];
std_var=[1e-2,5e-2,1e-1,5e-1,1]; %范围
%A=[BS1;BS2;BS3;BS4]; %矩阵A包含4个初始坐标
A=[BS1;BS2;BS3;BS4];
number=10000;
for j=1:length(std_var) %循环error1=0;%初始误差置为0error2=0; %初始误差置为0std_var1=std_var(j);%令std_var1等于当前数组的值for i=1:number %多次循环%r1=A-ones(4,1)*MS; r1=A-ones(4,1)*MS;r2=(sum(r1.^2,2)).^(1/2); %r=r2(2:end,:)-ones(3,1)*r2(1,:)+std_var1*randn(3,1); %表示从[2,i]开始MS与基站i和基站1的距离差r=r2(2:end,:)-ones(3,1)*r2(1,:)+std_var1*randn(3,1);sigma=std_var1^2; theta1=TDOACHAN(A,r,sigma); % 调用TDOACHAN函数theta2=TDOATaylor(A,r,sigma); %调用TDOATalor函数error1=error1+norm(MS-theta1)^2; %移动台MS估计位置与计算的到的距离的平方error2=error2+norm(MS-theta2)^2; %移动台MS估计位置与计算的到的距离的平方end RMSE1(j)=(error1/number)^(1/2); %均方根误差RMSE2(j)=(error2/number)^(1/2); %均方根误差
end
% plot
semilogx(std_var,RMSE1,'-O',std_var,RMSE2,'-s')% x轴取对数，X轴范围是1e-2到1,Y轴的范围是变动的
xlabel('The standard deviation of measurement noise (m)');
ylabel('RMSE');
legend('TDOA-CHAN','TDOA-Taylor');%TDOA-CHAN:
function theta=TDOACHAN(A,p,sigma)
% A is the coordinate of BSs
%A是BSS的坐标
% p is the range measurement
%P是范围测量
% sigma is the the variance of TDOA measurement
%sigma是TDOA测量的方差
[m,~]=size(A); %size得到A的行列数赋值给[m,~]，~表示占位，就是只要行m的值！
k=sum(A.^2,2); %矩阵A每个元素分别平方，得到新矩阵，在行求和，最为矩阵K
G1=[A(2:end,:)-ones(m-1,1)*A(1,:),p]; %得到Xm1,Ym1,Rm1,的值，m取值[2,i],构建矩阵Ga
h1=1/2*(p.^2-k(2:end,:)+ones(m-1,1)*k(1,:)); %构建矩阵h
Q=diag(ones(m-1,1)*sigma); %构建TDOA的协方差矩阵
% initial estimate
theta0=inv(G1'*inv(Q)*G1)*G1'*inv(Q)*h1; %通过一次WLS算法进行求解，
s=A(2:end,:)-ones(m-1,1)*theta0(1:2,:)';
d=sum(s.^2,2);%矩阵s每个元素分别平方，得到新矩阵，在行求和，最为矩阵d
B1=diag(d.^(1/2));
cov1=B1*Q*B1;
% first wls
theta1=inv(G1'*inv(cov1)*G1)*G1'*inv(cov1)*h1; %进行第一次WLS计算
cov_theta1=inv(G1'*inv(cov1)*G1); %得到theta1的协方差矩阵
% second wls
G2=[1,0;0,1;1,1]; %构建G'
h2=[(theta1(1,1)-A(1,1))^2;(theta1(2,1)-A(1,2))^2;theta1(3,1)^2]; %构建h'
B2=diag([theta1(1,1)-A(1,1),theta1(2,1)-A(1,2),theta1(3,1)]); %构建b'
cov2=4*B2*cov_theta1*B2; %得到误差矢量的协方差矩阵。
theta2=inv(G2'*inv(cov2)*G2)*G2'*inv(cov2)*h2; %运用最大似然估计得到
theta=theta2.^(1/2)+[A(1,1);A(1,2)]; %得到MS位置的估计值坐标，以及符号
theta=theta';%转换为（x,y）形式
end
%TDOA-Talayor:
function theta=TDOATaylor(A,p,sigma)
% A is the coordinate of BSs% p is the range measurement
% sigma is the the variance of TOA measurement
% initial estimate
theta0=TDOACHAN(A,p,sigma); %调用TDOACHAN得到一个初始的估计位置
delta=norm(theta0); %得到范数
while norm(delta)>1e-2 %得到足够小的值[m,~]=size(A); %size得到A的行列数赋值给[m,~]，~表示占位，就是只要行m的值！d=sum((A-ones(m,1)*theta0).^2,2); R=d.^(1/2); G1=ones(m-1,1)*(A(1,1)-theta0(1,1))/R(1,1)-(A(2:m,1)-theta0(1,1))./R(2:m,:); G2=ones(m-1,1)*(A(1,2)-theta0(1,2))/R(1,1)-(A(2:m,2)-theta0(1,2))./R(2:m,:); G=[G1,G2]; %构建Gth=p-(R(2:m,:)-ones(m-1,1)*R(1,:)); %构建HtQ=diag(ones(m-1,1)*sigma); %TDOA测量值的协方差矩阵delta=inv(G'*inv(Q)*G)*G'*inv(Q)*h; %加权最小二乘解theta0=theta0+delta'; %累加
end
theta=theta0;
end
四、仿真结果
在matlab下实现TDOA定位算法性能仿真相关推荐
- 基于python与matlab的TOA定位算法性能仿真
基于python与matlab的TOA定位算法性能仿真 仿真要求 仿真方案的设计 matlab仿真代码 python仿真代码 仿真结果 仿真要求 要求一:编写两个函数TOA_LLOP和TOA_CHAN ...
- TOA定位算法性能仿真
一.题目 TOA定位算法性能仿真 二.仿真要求 要求一:编写两个函数TOA_LLOP和TOA_CHAN得到位置的估计. 要求二:用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线对比图,横 ...
- Matlab-TOA定位算法性能仿真 TOA-LLOP和TOA-CHAN得到位置的估计 两种算法的性能比较 两种算法的RMSE曲线对比图
本博客环境为Matlab2018 ,软件版本不同可能会有些出入,需要稍作修改. TOA定位算法性能的仿真 要求一:编写两个函数TOA_LLOP和TOA_CHAN得到位置的估计. 要求二:用RMSE实现 ...
- Matlab下 IIR 滤波器实现（Simulink仿真和C语言实现）
Matlab下 IIR 滤波器实现(Simulink仿真和C语言实现)_Terrys0518的专栏-CSDN博客
- 【 笔记 】定位算法性能分析
目录 1 CRLB Computation 2 Mean and Variance Analysis PERFORMANCE ANALYSIS FOR LOCALIZATION ALGORITHMS ...
- 【目标定位】基于matlab粒子滤波的定位算法【含Matlab源码 2161期】
一.基于粒子滤波污染源定位简介 粒子滤波定位算法是目前最精准定位可移动物体的位置,由于水域的流动,工业固体废物污染源很可能随着水流移动位置,基于粒子滤波算法将污染物定位分为预测.测量以及重新采样可大大 ...
- 液压减振器 matlab,汽车减振器的结构分析与性能仿真研究
内容简介: 毕业设计 汽车减振器的结构分析与性能仿真研究 摘要: 减振器是悬架系统中的重要部件之一,它影响着整车的舒适性.操作稳定性.安全性等重要性能.双筒液压减振器作为减振器中主流产品,改善其设计方 ...
- 强干扰下弱信号matlab,北斗四阵元抗干扰算法性能研究
引言随着北斗导航系统大量运用在国防建设和国民建设等领域,其重要性也日益凸显.同时,北斗导航系统的工作环境也面临着严峻挑战,由于北斗导航系统到达地面的有用信号非常微弱,比接收机的热噪声还要低[2],尤其 ...
- matlab理想低通滤波器代码_自己动手，解开Matlab下AMD锐龙处理器性能封印
性能除了需要花钱的硬件级提升方法外,还有系统管理,驱动以及应用优化等不花钱的方法.早年间硬件玩家们流行通过刷BIOS软改,覆盖驱动强行开启专属功能等各种魔改方式提升硬件性能.而对特定的应用进行优化的话 ...
最新文章
- 中国电子学会青少年编程能力等级测试图形化四级编程题：计算生肖
- java超线程_超线程多核心下Java多线程编程技术分析
- eclipse创建springboot项目_idea创建基于gradle构建的spring boot项目
- 【收藏】如何避免everything每次都重建索引
- 大数据Notebook调研信息汇总(持续更新中)
- Docker Compose配置springboot微服务项目
- 蘑菇街2019实习生题目总结
- 诗与远方：无题（五十六）
- 面试题 数组长度可否赋值
- raid -- 分区-- 格式化 --挂载
- lucene.net 3.0.3、结合盘古分词进行搜索的小例子(转)
- es mapping 设置
- 可编辑！热门动态表情包！
- C++根据旋转轴和角度求旋转矩阵
- 翁恺老师Java进阶城堡游戏源码
- 论文速递 EMNLP2022 | 接受论文抢先看！！！（内含下载列表）
- 合成大西瓜unity总结
- 应用层 DNS域名解析服务器 文件传送协议FTP 简单邮件传送协议SMTP 万维网 HTTP超文本协议
- Linux终端一直输出login,Linux tty pty console区别
- 【PMP备考经验分享】从选择培训机构到5A通关，你需要做什么？
热门文章
- mysql新增加密字段_mysql字段加密
- linux文件末尾 m,Linux7-删除nginx配置文件末尾的^M字符
- Shell解析curl返回的json数据
- 亲密关系沟通-【价值感】-确保价值感的沟通方法
- 4创建ui显示不出来_4道小学生经典推理题，家长们一道也做不出来，太烧脑了...
- ❤️测试人的曲折职场路：从毕业的5K到20K，四年我换了3份工作…
- python+selenium自动化测试-Windows环境搭建
- 神威计算机主板型号,十代酷睿高端型Z490主板曝光 总线规格依旧是PCIe 3.0且USB接口并不支持USB 3.2...
- php伪静态规则生成,SEO工具箱：PHP自动生成PHPCMS伪静态规则.htaccess
- 学计算机辐射,离散数学对计算机专业系统知识辐射作用.doc