MATLAB代码|TOA测距定位|二维|任意个锚节点

MATLAB卡尔曼

已于 2024-10-11 19:15:51 修改

阅读量728

点赞数 15

分类专栏：定位与导航文章标签： matlab 数据库开发语言

于 2024-07-23 10:33:06 首次发布

本文链接：https://blog.csdn.net/callmeup/article/details/140619960

版权

定位与导航专栏收录该内容

35 篇文章 1 订阅

订阅专栏

TOA/TDOA

TOA (Time of Arrival)和TDOA (Time Difference of Arrival)是通过传播时间来确定位置的方法。
换算成的单程信号传输时间 $\tau$ 加上噪声 $n o i se$ 后，再乘上光速 $c$ ，即可得到距离 $r an g e$ ，如下：
$(\tau+noise)*c$
当已知点的坐标确定，已知点与待测点之间的距离也测得以后，可以根据最小二乘法计算出来待测点的位置坐标。

之前发布的相关程序有：

固定点下的二维坐标求解：https://blog.csdn.net/callmeup/article/details/136648868
高度上带有气压校准的三维坐标求解：https://blog.csdn.net/callmeup/article/details/136841197
带权重的求解方法：https://blog.csdn.net/callmeup/article/details/136839547
本文所述的程序通过设置已知点坐标、测距精度等参数，提供一个可以随意调节已知点（锚点）数量的最小二乘法。
可用于多点定位、自适应TOA、自适应TDOA算法的编程。

设置过程与运行结果

如下，设置待求点坐标、锚节点数量、测距误差：

point1 = [0.5,0.5]; %待求点坐标真值
n = 8; %定义锚节点数量
range_err = 0.1; %测距误差

绘制锚点、待测点真值、求出来的值的二维图像：
请添加图片描述
图中，共8个红色小圆圈，它们是已知的锚点，也就是定位使用的基站，数量符合前面设置的“num = 8;”。同理，蓝色圆圈是待测点的真值，是前面输入的值，红色“*”号是实际解算出来的值，和蓝点比较接近，它们俩离得越近说明定位效果越好。

程序结构

结构如下，由主程序注释、绘图、输出、定位函数组成：
在这里插入图片描述

命令行窗口也能显示出来准确的坐标：
在这里插入图片描述

程序源码

测试代码部分

% TOA/TDOA测距定位，二维任意（>3)个锚节点
% author:Evand
% 2024-7-22/Ver1
clear;clc;close all;
rng(0);
%% 主程序
point1 = [0.5,0.5]; %待求点坐标真值
n = 8; %定义锚节点数量
range_err = 0.1; %测距误差
baseP = [sin(1:n);cos(1:n)]';
R_calcu = sqrt(diag((point1-baseP)*(point1'-baseP')))+range_err*randn; %含噪声的距离
[p_out] = position(R_calcu,baseP);
%% 绘图
figure;
plot(point1(1),point1(2),'o');
hold on
plot(p_out(1),p_out(2),'*');
scatter(baseP(:,1),baseP(:,2),'or');
legend('待定位点（真实值）','待定位点位置解算（输出值）','锚点（已知点）');
%% output
fprintf('输入的坐标为(%f,%f)\n',point1(1),point1(2));
fprintf('输出的坐标为(%f,%f)\n',p_out(1),p_out(2));

函数代码部分

上面测试代码里面，positon是我写的函数：

function [p_out] = position(R_calcu,baseP)
[baseX_,baseY_] = deal(baseP(:,1),baseP(:,2));
len = size(baseP,1);
% 完整代码下载链接：https://gf.bilibili.com/item/detail/1105868012
end