当前位置：首页 > news >正文

二维环境下TDOA的MATLAB仿真代码（4个锚节点）

news 2024/9/25 21:43:45

文章目录

TDOA介绍
程序源码
运行结果
代码详解
- - 程序目的
  - 代码结构
  - 定位函数 `triposition_weight`
  - 总结

TDOA介绍

TDOA 是一种定位技术，常用于无线通信和信号处理，通过测量信号到达不同接收器的时间差来定位信号源。

程序源码

源代码如下：

% TDOA测距定位，二维平面， 4个锚节点的情况
% author:Evand（VX：matlabfilter，除前期达成一致外，讲解需付费）
% 2024年9月25日/Ver1
clear;clc;close all;
rng(0);
%% 主程序
c = 3e8; %信号传输速度，即光速
range_err = 1e-9; %时钟与时间计算误差
point1 = [1,1];
baseP = [0,1;0,0;2,2;2,0];
R_real = sqrt(diag((point1-baseP)*(point1'-baseP'))); %含噪声的距离
TDOA = R_real/c+range_err*randn; %含噪声的传播时间
R_calcu = TDOA*c;
weight = eye(3); %权重设置，如果不用权重，这里不动即可
[p_out] = triposition_weight(R_calcu,baseP,weight);%% 绘图
figure;
plot(point1(1),point1(2),'o');
hold on
plot(p_out(1),p_out(2),'o');
scatter(baseP(:,1),baseP(:,2),'*r');
xlim([-1,3]);ylim([-1,3]);
legend('待定位点（真实值）','待定位点位置解算','锚点（已知点）')fprintf('待定位点的真实坐标为：(%f,%f)\n',point1(1),point1(2));
fprintf('解得的位置坐标为：(%f,%f)\n',p_out(1),p_out(2));%% 定位函数
function [p_out] = triposition_weight(R_calcu,baseP,varargin)
% p = [5,5;10,9;15,12;20,22;25,3]; %real location
if size(varargin,1) == 0W = 0.5*eye(3);
elseW = cell2mat(varargin);
end
[baseX_,baseY_] = deal(baseP(:,1),baseP(:,2));
% baseY_ = baseP(:,2);
% baseZ_ = baseP(:,3);
H = [baseX_(2)-baseX_(1),baseY_(2)-baseY_(1);baseX_(3)-baseX_(1),baseY_(3)-baseY_(1);baseX_(4)-baseX_(1),baseY_(4)-baseY_(1)];% HX=aa = 0.5*[baseX_(2).^2+baseY_(2).^2-R_calcu(2).^2-baseX_(1).^2-baseY_(1).^2+R_calcu(1).^2;baseX_(3).^2+baseY_(3).^2-R_calcu(3).^2-baseX_(1).^2-baseY_(1).^2+R_calcu(1).^2;baseX_(4).^2+baseY_(4).^2-R_calcu(4).^2-baseX_(1).^2-baseY_(1).^2+R_calcu(1).^2];%     p_out(i,:) = (pinv(H)*a)'; %伪逆求法
%         (H'*H)^(-1)*H'*a;X(:,1) = (H'*W*H)^(-1)*H'*W*a; %左逆求法p_out = X';
end

运行结果

绘图显示锚点位置、待定位真实值、计算得到的位置：
在这里插入图片描述
同时，命令行会给出待定位点的真实坐标、解得的位置坐标：

代码详解

程序目的

该程序实现了基于时差定位（TDOA）的方法，来估算在二维平面上一个未知点（待定位点）的位置。程序使用了四个已知的锚节点，并考虑了信号传播的误差和噪声。

代码结构

初始化
```
clear; clc; close all;
rng(0);
```
- 清除工作区、命令窗口和关闭所有图形窗口。
- 设置随机数生成器的种子为0，以确保结果可重现。

主程序

c = 3e8; % 信号传输速度，即光速
range_err = 1e-9; % 时钟与时间计算误差
point1 = [1,1]; % 待定位点的真实坐标
baseP = [0,1; 0,0; 2,2; 2,0]; % 四个锚节点的坐标

定义信号传播速度 c 和时钟误差 range_err。
定义待定位点的真实坐标 point1 以及四个锚节点的坐标 baseP。

计算距离和传播时间

R_real = sqrt(diag((point1-baseP)*(point1'-baseP'))); % 含噪声的距离
TDOA = R_real/c + range_err*randn; % 含噪声的传播时间
R_calcu = TDOA*c; % 计算的距离

计算待定位点到各个锚节点的真实距离 R_real。
通过加入噪声计算传播时间 TDOA，并将其转换为计算的距离 R_calcu。

权重设置
```
weight = eye(3); % 权重设置
[p_out] = triposition_weight(R_calcu, baseP, weight);
```
- 设置权重矩阵 weight（默认为单位矩阵）。
- 调用函数 triposition_weight 进行位置解算，返回估算的待定位点位置 p_out。

绘图

figure;
plot(point1(1), point1(2), 'o');
hold on;
plot(p_out(1), p_out(2), 'o');
scatter(baseP(:,1), baseP(:,2), '*r');
xlim([-1, 3]); ylim([-1, 3]);
legend('待定位点（真实值）', '待定位点位置解算', '锚点（已知点）');

绘制真实待定位点、估算位置和锚节点的位置。
设置坐标轴范围和图例。

输出结果

fprintf('待定位点的真实坐标为：(%f,%f)\n', point1(1), point1(2));
fprintf('解得的位置坐标为：(%f,%f)\n', p_out(1), p_out(2));

打印真实坐标和解算得到的坐标。

定位函数 `triposition_weight`

函数定义
```
function [p_out] = triposition_weight(R_calcu, baseP, varargin)
```
- 定义一个函数 triposition_weight，接受计算得到的距离、锚节点坐标和可选权重参数。

权重处理

if size(varargin, 1) == 0W = 0.5 * eye(3);
elseW = cell2mat(varargin);
end

根据输入的权重设置，默认使用0.5倍的单位矩阵。

构造矩阵 H 和向量 a

[baseX_, baseY_] = deal(baseP(:,1), baseP(:,2));
H = [ baseX_(2)-baseX_(1), baseY_(2)-baseY_(1);baseX_(3)-baseX_(1), baseY_(3)-baseY_(1);baseX_(4)-baseX_(1), baseY_(4)-baseY_(1)];

从锚节点坐标中提取x和y坐标，构造矩阵 H。

计算 a 向量

a = 0.5 * [ baseX_(2).^2 + baseY_(2).^2 - R_calcu(2).^2 - baseX_(1).^2 - baseY_(1).^2 + R_calcu(1).^2;baseX_(3).^2 + baseY_(3).^2 - R_calcu(3).^2 - baseX_(1).^2 - baseY_(1).^2 + R_calcu(1).^2;baseX_(4).^2 + baseY_(4).^2 - R_calcu(4).^2 - baseX_(1).^2 - baseY_(1).^2 + R_calcu(1).^2];