定位算法——TDOA的Chan算法推导与Matlab实现

TDOA算法原理

TDOA(Time Difference of Arrival)——时间差到达算法，利用了几何数学中双曲线的特点—— 双曲线上的任意点到达两焦点的距离差是固定值。 这个距离差它天然可以抹去用户设备(UE)和基站的之间时钟误差。
请添加图片描述
P 1 C 1 = c ⋅ ( t 11 + Δ t ) P_1C_1 = c·(t_{11}+\Delta t) P1​C1​=c⋅(t11​+Δt)其中 Δ t \Delta t Δt是UE和基站之间的钟差(在UE与基站不完全同步的情况下)，这个钟差我们没法直接获得。
P 1 C 2 = c ⋅ ( t 12 + Δ t ) P_1C_2 = c·(t_{12}+\Delta t) P1​C2​=c⋅(t12​+Δt) 则 ∣ P 1 C 1 − P 1 C 2 ∣ = c ⋅ ( t 11 − t 12 ) 则|P_1C_1-P_1C_2 |=c·(t_{11}-t_{12}) 则∣P1​C1​−P1​C2​∣=c⋅(t11​−t12​)可见这里的钟差 Δ t \Delta t Δt被消除了，之后使用数学方法求出两个双曲线的焦点。但这同时也暗示着 基站的时钟需要同步才能被消除。 所以TDOA算法特性：UE和基站无需同步，基站之间需要同步，最少三个基站能测得焦点。

Chan算法介绍

在TDOA的解算方法上，有直接求解析解的Chan算法、Fang算法。也有迭代算法如Taylor算法(它是通过不断计算当前误差来调整参数，这个误差需要真实的位置标签来对比，但我们有真实标签后为什么还需要估计呢？这个是我对Taylor算法的疑惑，欢迎大家一起探讨👏)。基于解析解的方差理论上是没有误差的，它只受限于计算机的计算精度。

Chan算法公式推导

Chan算法公式推导
Chan算法公式推导
上述算法中， A x = r 0 C + D Ax=r_0C+D Ax=r0​C+D，先求 A x = C Ax=C Ax=C的解 x a x_a xa​，再求 A x = D Ax=D Ax=D的解 x b x_b xb​，再求 r 0 r_0 r0​，最后按照 x = r 0 x a + x b x=r_0x_a+x_b x=r0​xa​+xb​组合起来。

Chan算法实现2D


%%
clc; 
clear;
close all; 
format long;
figure;

%设置UE位置
for i=1:100
    ue_x = randi(100); 
    ue_y = randi(100);
    
    scatter(ue_x, ue_y, '*');
    hold on;
    
    %注意注意！！！基站的y不能全部相同，否则在第57行的矩阵A第二列元素全为0，Ax=C或Ax=Ds时求不出唯一解
    stations = [-40 0; 20 0; 40 50; 10 10];     %第四个基站是为了提出伪解
    
    hold on;
    r0_real = distance(ue_x, ue_y,stations(1,1), stations(1,2));
    r1_real = distance(ue_x, ue_y,stations(2,1), stations(2,2));
    r2_real = distance(ue_x, ue_y,stations(3,1), stations(3,2));
    r3_real = distance(ue_x, ue_y,stations(4,1), stations(4,2));
    %ri_real只是用来计算tds，实际上它会带有时钟误差，而这个误差我们不能直接得到
    tds = [r1_real-r0_real r2_real-r0_real, r3_real-r0_real];

    position = TDOA(stations, tds);
    scatter(position(1), position(2), 'o');
    hold on;
end
%%
function [position] = TDOA(stations, tds)
    
    x0 = stations(1,1);y0 = stations(1,2);
    x1 = stations(2,1);y1 = stations(2,2);
    x2 = stations(3,1);y2 = stations(3,2);
    x3 = stations(4,1);y3 = stations(4,2);
    
    r10 = tds(1);
    r20 = tds(2);
    r30 = tds(3);   %ue对3号基站和0号基站的距离差，真实的
    
    scatter(x0,y0,120,'d', 'filled'); text(x0,y0,'Anchor1');
    scatter(x1,y1,120,'d', 'filled'); text(x1,y1,'Anchor2');
    scatter(x2,y2,120,'d', 'filled'); text(x2,y2,'Anchor3');
    hold on;
    
    x10 = x1 - x0;
    x20 = x2 - x0;
    y10 = y1 - y0;
    y20 = y2 - y0;

    k0  = x0^2 + y0^2;
    k1  = x1^2 + y1^2;
    k2  = x2^2 + y2^2;

    A = [x10 y10; x20 y20];
    C = -[r10; r20];
    D = [(k1-k0-r10^2)/2; (k2-k0-r20^2)/2];

    %求解Ax = r0 * C + D

    a = A\C;
    b = A\D;

    %求解r0

    A_ = a(1)^2 + a(2)^2-1;
    B_ = a(1) * (b(1) - x0) + a(2) * (b(2) - y0);
    C_ = (x0 - b(1))^2 + (y0 - b(2))^2;
    if B_^2-A_*C_ < 0
        position = [Nan, Nan];
    else
        r0_1 = -(B_+sqrt(B_^2-A_*C_))/A_;
        r0_2 = -(B_-sqrt(B_^2-A_*C_))/A_;
        X1 = a * r0_1 + b;
        X2 = a * r0_2 + b;

        %剔除错误解：方法一：UE和基站时钟尽量同步。方法二：增加观测站(本例使用)
        if abs(r30-(distance(X1(1),X1(2),x3,y3)-distance(X1(1),X1(2),x0,y0))) < 1e-8
            position = X1;
        else
            position = X2;
        end
    end
end

%%
function dist = distance(x1,y1,x2,y2)
    dist = sqrt((x1-x2)^2 + (y1-y2)^2);    
end

上述代码需要注意三个地方！！！

  1. 算距离差时，不加上绝对值，这样可以排除掉一半的解(两双曲线相交有2至4个交交点)
  2. 计算 r 0 r_0 r0​时可能有伪解，需要增加观测站或牺牲一定精度来排除另外一个解。(本文是增加了一个基站)。
  3. 二维定位时，不允许所有的基站在 y y y轴的数值相等。

上述代码得到的结果
在这里插入图片描述
上图中*表示UE的真实位置，o表示UE的计算位置，可以看到每个UE的位置都被正确解算了。

Chan算法实现3D


%%
clc; 
clear;
close all;
format long;
 
% tmp = unifrnd(0,255,4,2);
% x1 = tmp(1,1); y1 = tmp(1,2);  % Anchor1
% x2 = tmp(2,1); y2 = tmp(2,2);  % Anchor2
% x3 = tmp(3,1); y3 = tmp(3,2);  % Anchor3

figure;
%随机生成100个UE位置，并对其进行TDOA计算
correct_sum = 0;
uncorrect_sum = 0;
for i=1:100
    ue_x = randi(60)+randn();
    ue_y = randi(60)+randn();
    ue_z = randi(60)+randn();
    
    scatter3(ue_x, ue_y, ue_z, 120, '*');
    hold on;

    %注意注意！！！基站的z不能全部相同，否则在第67行的矩阵A第三列元素全为0，Ax=C或Ax=Ds时求不出唯一解
    stations = [-40 0 5; 20 0 15; 40 50 5; 0 0 5; 10 10 10];  

    r0_real = distance(ue_x, ue_y, ue_z, stations(1,1), stations(1,2), stations(1,3));
    r1_real = distance(ue_x, ue_y, ue_z, stations(2,1), stations(2,2), stations(2,3));
    r2_real = distance(ue_x, ue_y, ue_z, stations(3,1), stations(3,2), stations(3,3));
    r3_real = distance(ue_x, ue_y, ue_z, stations(4,1), stations(4,2), stations(4,3));
    r4_real = distance(ue_x, ue_y, ue_z, stations(5,1), stations(5,2), stations(5,3));
    tds = [r1_real-r0_real r2_real-r0_real r3_real-r0_real r4_real-r0_real];

    position = TDOA(stations, tds);

    if distance(position(1), position(2), position(3), ue_x, ue_y, ue_z) < 1e-8
        correct_sum = correct_sum + 1;
    else
        uncorrect_sum = uncorrect_sum + 1;
    end
    scatter3(position(1), position(2), position(3), 'o', 'r');
    hold on;
    
    

end
%%
function [position] = TDOA(stations, tds)
    
    x0 = stations(1,1);y0 = stations(1,2);z0 = stations(1,3);
    x1 = stations(2,1);y1 = stations(2,2);z1 = stations(2,3);
    x2 = stations(3,1);y2 = stations(3,2);z2 = stations(3,3);
    x3 = stations(4,1);y3 = stations(4,2);z3 = stations(4,3);
    x4 = stations(5,1);y4 = stations(5,2);z4 = stations(5,3);
    
    r10 = tds(1);
    r20 = tds(2);
    r30 = tds(3);
    r40 = tds(4);
    
    
    scatter3(x0,y0,z0,120,'d', 'filled'); text(x0,y0,z0,'Station1');hold on;
    scatter3(x1,y1,z1,120,'d', 'filled'); text(x1,y1,z1,'Station2');hold on;
    scatter3(x2,y2,z2,120,'d', 'filled'); text(x2,y2,z2,'Station3');hold on;
    scatter3(x3,y3,z3,120,'d', 'filled'); text(x3,y3,z3,'Station4');hold on;
    hold on;

    % r21 represents the TDOA between anchor1 and anchor2
    % r31 represents the TDOA between anchor1 and anchor3
    
    x10 = x1 - x0;
    x20 = x2 - x0;
    x30 = x3 - x0;
    
    y10 = y1 - y0;
    y20 = y2 - y0;
    y30 = y3 - y0;
    
    z10 = z1 - z0;
    z20 = z2 - z0;
    z30 = z3 - z0;

    k0  = x0^2 + y0^2 + z0^2;
    k1  = x1^2 + y1^2 + z1^2;
    k2  = x2^2 + y2^2 + z2^2;
    k3  = x3^2 + y3^2 + z3^2;

    A = [x10 y10 z10; x20 y20 z20; x30 y30 z30];
    C = -[r10; r20; r30];
    D = [(k1-k0-r10^2)/2; (k2-k0-r20^2)/2; (k3-k0-r30^2)/2];

    %求解Ax = r0 * C + D

    a = A\C;
    b = A\D;

    %求解r0

    A_ = a(1)^2 + a(2)^2 + a(3)^2 -1;
    B_ = a(1) * (b(1) - x0) + a(2) * (b(2) - y0) + a(3) * (b(3) - z0);
    C_ = (x0 - b(1))^2 + (y0 - b(2))^2 + (z0 - b(3))^2;
    
    r0_1 = -(B_+sqrt(B_^2-A_*C_))/A_;
    r0_2 = -(B_-sqrt(B_^2-A_*C_))/A_;
    X1 = a * r0_1 + b;
    X2 = a * r0_2 + b;

    %剔除错误解：方法一：UE和基站时钟尽量同步。方法二：增加观测站(本例使用)
    if abs(r40-(distance(X1(1),X1(2),X1(3),x4,y4,z4)-distance(X1(1),X1(2),X1(3),x0,y0,z0))) < 1e-8
        position = X1;
    else
        position = X2;
    end

end

%%
function dist = distance(x1,y1,z1,x2,y2,z2)
    dist = sqrt((x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2);    
end

这份代码是随机生成了100个三维点，然后使用Chan算法解算位置。
上述代码也需要注意三个地方！！！

  1. 算距离差时，不加上绝对值，这样可以排除掉一半的解(两双曲线相交有2至4个交交点)
  2. 计算 r 0 r_0 r0​时可能有伪解，需要增加观测站或牺牲一定精度来排除另外一个解。(本文是增加了一个基站)。
  3. 二维定位时，不允许所有的基站在 z z z轴的数值相等。

上述代码的运行结果
在这里插入图片描述
经过测试，所有的解算误差都小于 1 0 − 8 10^{-8} 10−8

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/443766.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

武汉灰京文化：5G与云计算技术下的手游行业，技术创新将带来怎样的变革？

武汉灰京文化：5G与云计算技术下的手游行业，技术创新将带来怎样的变革？

随着技术的不断进步&#xff0c;手游行业将迎来一场革命性的变革。5G网络的普及和云计算技术的飞速发展&#xff0c;将为手游带来更加流畅、高清晰度的游戏体验&#xff0c;让玩家们尽情享受更真实、更沉浸式的虚拟现实和增强现实游戏。同时&#xff0c;人工智能技术的运用也将…
阅读更多...
【Linux】cpp-httplib库

【Linux】cpp-httplib库

目录 升级gcc版本 下载cpp-httplib的zip安装包&#xff0c;上传到服务器 ​编辑 简单使用 首先打开gittee,搜索cpp-httplib,选择其中一个即可 也可以点下方链接 cpp-httplib库&#xff1a;cpp-httplib: cpp-httplib (gitee.com) 注意&#xff1a;cpp-httplib在使用的时候需…
阅读更多...
BUU [网鼎杯 2020 半决赛]AliceWebsite

BUU [网鼎杯 2020 半决赛]AliceWebsite

BUU [网鼎杯 2020 半决赛]AliceWebsite 开题&#xff1a; hint附件是源码。在index.php中有一个毫无过滤的本地文件包含 <?php $action (isset($_GET[action]) ? $_GET[action] : home.php); if (file_exists($action)) {include $action; } else {echo "File not…
阅读更多...
人工智能|机器学习——DBSCAN聚类算法（密度聚类）

人工智能|机器学习——DBSCAN聚类算法（密度聚类）

1.算法简介 DBSCAN(Density-Based Spatial Clustering of Applications with Noise)是一种基于密度的聚类算法&#xff0c;簇集的划定完全由样本的聚集程度决定。聚集程度不足以构成簇落的那些样本视为噪声点&#xff0c;因此DBSCAN聚类的方式也可以用于异常点的检测。 2.算法原…
阅读更多...
使用python获取电脑的公网 IP

使用python获取电脑的公网 IP

python 代码&#xff1a; from requests import getip get(https://api.myip.la).text print(My public IP address is: {}.format(ip))输出结果&#xff1a;
阅读更多...
XSS-Labs靶场1---11关

XSS-Labs靶场1---11关

一、XSS环境搭建&#xff1a; [ 靶场环境篇 ] XSS-labs 靶场环境搭建(特别详细)_xss靶场搭建-CSDN博客 &#xff08;该博主总结的较为详细&#xff0c;若侵权必删&#xff09; 常用的xss攻击语句&#xff1a; 输入检测确定标签没有过滤后&#xff0c;为了显示存在漏洞&#…
阅读更多...
python并发编程-多路复用IO

python并发编程-多路复用IO

多路复用IO(IO multiplexing) O multiplexing这个词可能有点陌生&#xff0c;但是如果我说select/epoll&#xff0c;大概就都能明白了。有些地方也称这种IO方式为事件驱动IO (event driven IO)。我们都知道&#xff0c;select/epoll的好处就在于单个process就可以同时处理多个…
阅读更多...
【蓝桥杯-单片机】基础模块LED和按键

【蓝桥杯-单片机】基础模块LED和按键

文章目录 【蓝桥杯-单片机】Led、按键等基础模块01 前置准备&#xff08;1&#xff09;新建工程&#xff08;4&#xff09;编写程序 02 基础模块&#xff1a;LED&#xff08;0&#xff09;LED原理图&#xff08;1&#xff09;对P1整体赋值&#xff0c;控制所有的LED灯&#xff…
阅读更多...
【C++】函数模板和类模板

【C++】函数模板和类模板

目录 1.泛型编程 2.函数模板 2.1函数模板的定义格式 2.2函数模板的实例化 2.3函数模板参数的匹配原则 3.类模板 3.1类模板的定义格式 3.2类模板的实例化 3.3模板的分离编译 1.泛型编程 泛型编程&#xff1a;编写与类型无关的通用代码&#xff0c;是代码复用的一种手段…
阅读更多...
Linux——文件重定向

Linux——文件重定向

目录 前言 一、重定向 二、重定向的运用 三、dup2 四、命令行中的重定向 五、为什么要有标准错误 前言 在之前我们学习了文件标识符&#xff0c;直到close可以使用文件标识符进行关闭&#xff0c;但是当我们关闭1号&#xff08;stdout&#xff09;时&#xff0c;无法往显…
阅读更多...
java 哨兵线性搜索

java 哨兵线性搜索

顾名思义&#xff0c;哨兵线性搜索是线性搜索的一种&#xff0c;与传统线性搜索相比&#xff0c;比较次数减少了。在传统的线性搜索中&#xff0c;仅进行N次比较&#xff0c;而在哨兵线性搜索中&#xff0c;哨兵值用于避免任何越界比较&#xff0c;但没有专门针对正在搜索的元素…
阅读更多...
springMVC自定义类型转换

springMVC自定义类型转换

目录 &#x1f34b;&#x1f34a;自定义的转换类 &#x1f34b;&#x1f34a;xml文件中添加配置 &#x1f34b;&#x1f34a;测试 SpringMVC 底层已经封装了很多的类型转换器&#xff0c;也就是为什么我们页面上传的字符串可以使用 Integer接收或者可以直接转换为数组的原因…
阅读更多...
学习 考证 帆软 FCP-FineBI V6.0 考试经验

学习 考证 帆软 FCP-FineBI V6.0 考试经验

学习背景&#xff1a; 自2024年1月起&#xff0c;大部分时间就在家里度过了&#xff0c;想着还是需要充实一下自己&#xff0c;我是一个充满热情的个体。由于之前公司也和帆软结缘&#xff0c;无论是 Fine-Report 和 Fine-BI 都有接触3年之久&#xff0c;但是主要做为管理者并…
阅读更多...
高级语言讲义2010计专（仅高级语言部分）

高级语言讲义2010计专（仅高级语言部分）

1.编写一程序&#xff0c;对输入的正整数&#xff0c;求他的约数和。 如&#xff1a;18的约数和为1236939 #include <stdio.h>int getsum(int n){int i,sum0;for(i1;i<n;i)if(n%i0)sumi;return sum; } int main(){int sum getsum(18);printf("%d",sum); …
阅读更多...
JavaWeb--Mybatis

JavaWeb--Mybatis

一&#xff1a;Mybatis概述 1.Mybatis概念 MyBatis 是一款优秀的 持久层框架 &#xff0c;用于简化 JDBC 开发&#xff1b; MyBatis 本是 Apache 的一个开源项目 iBatis, 2010 年这个项目由 apache software foundation 迁移到了 google code&#xff0c;并且改名为 MyB…
阅读更多...
《Effective Modern C++》- 极精简版 15-21条

《Effective Modern C++》- 极精简版 15-21条

本文章属于专栏《业界Cpp进阶建议整理》 继续上篇《Effective Modern C》- 极精简版 5-14条。本文列出《Effective Modern C》的15-21条的个人理解的极精简版本。 Item15、尽量使用constexpr constexpr形容对象 constexpr对象都是const&#xff0c;但是const对象不一定是conste…
阅读更多...
全网最最最详细centos7如何安装docker教程

全网最最最详细centos7如何安装docker教程

在CentOS 7上安装Docker主要包括以下步骤&#xff1a; 1. 卸载旧版本的Docker 首先&#xff0c;需要确保系统上没有安装旧版本的Docker。可以通过以下命令来卸载它们&#xff1a; sudo yum remove docker \docker-client \docker-client-latest \docker-common \docker-late…
阅读更多...
C++篇 语 句

C++篇 语 句

到目前为止&#xff0c;我们只见过两种语句&#xff1a; return 语句和表达式语句。根据语句对执行顺 序的影响&#xff0c;C 语言其余语句大多属于以下 3 大类。 选择语句&#xff1a; if 语句和 switch 语句。循环语句&#xff1a; while 语句&#xff0c; do...while 语句和…
阅读更多...
SSH移植到BusyBox

SSH移植到BusyBox

手动编译SSH安装挺麻烦的&#xff0c;本文主要是我大量借鉴和实践总结出来的流程&#xff0c;一步一按照做不会有太大问题。 移植平台&#xff1a;IMX6UL(迅为开发板) 根文件系统&#xff1a;BusyBox 所有操作都建议不要在root账户下运行&#xff0c;并且make install的安装路…
阅读更多...
【FFmpeg】ffmpeg 命令行参数 ⑤ ( 使用 ffmpeg 命令提取 音视频 数据 | 保留封装格式 | 保留编码格式 | 重新编码 )

【FFmpeg】ffmpeg 命令行参数 ⑤ ( 使用 ffmpeg 命令提取 音视频 数据 | 保留封装格式 | 保留编码格式 | 重新编码 )

文章目录 一、使用 ffmpeg 命令提取 音视频 数据1、提取音频数据 - 保留封装格式2、提取视频数据 - 保留封装格式3、提取视频数据 - 保留编码格式4、提取视频数据 - 重新编码5、提取音频数据 - 保留编码格式6、提取音频数据 - 重新编码 一、使用 ffmpeg 命令提取 音视频 数据 1…
阅读更多...
最新文章

Copyright @ 2022~2023