【论文】三维坐标下TDOA之chan算法

function [X] = Chan3(BS,BSN,R)% 第一次WLS%k = x^2+y^2+z^2
for i =1:BSNk(i) = BS(1,i)^2 + BS(2,i)^2 + BS(3,i)^2;
end% h
for i = 1:BSN-1h(i) = 0.5*(R(i)^2 - k(i+1) + k(1));
end% Ga
for i = 1:BSN-1Ga(i,1) = -BS(1,i+1);%第i行的第一例数Ga(i,2) = -BS(2,i+1);Ga(i,3) = -BS(3,i+1);Ga(i,4) = -R(i);
end%Q为TDOA系统的协方差矩阵
Q = cov(R);%za,距离较远时
za1 = pinv(Ga'*pinv(Q)*Ga)*Ga'*pinv(Q)*h';% 第二次WLS%h2
X1 = BS(1,1);
Y1 = BS(2,1);
Z1 = BS(3,1);
h2 = [(za1(1,1) - X1)^2;(za1(2,1) - Y1)^2;(za1(3,1) - Z1)^2;za1(4,1)^2];%Ga2
Ga2 = [1,0,0;0,1,0;0,0,1;1,1,1];%B2
B2 = [za1(1,1)-X1,0,0,0;0,za1(2,1)-Y1,0,0;0,0,za1(3,1)-Z1,0;0,0,0,za1(4,1)];%za2
za2 = pinv( Ga2' * pinv(B2) * Ga' * pinv(Q) * Ga * pinv(B2) * Ga2) * (Ga2' * pinv(B2) * Ga' * pinv(Q) * Ga * pinv(B2)) * h2;%zp
zp(1,1) = abs(za2(1,1))^0.5+X1;
zp(2,1) = abs(za2(2,1))^0.5+Y1;
zp(3,1) = abs(za2(3,1))^0.5+Z1;X = zp;end

%基站数目
BSN = 7;%基站位置,每一列为一个基站位置
BS = [0, sqrt(3), 0.5*sqrt(3), -0.5*sqrt(3), -sqrt(3), -0.5*sqrt(3), 0.5*sqrt(3); 0,      0,         1.5,          1.5,        0,         -1.5,        -1.5;0,      0,           0,            0,        4,            0,           0];%当BS中z有一个有值时，三维定位的的Z轴精确度高
BS = BS(:,1:BSN);
BS = BS.*50;%MS 的实际位置
MS = [30,40,10];%无噪声情况下BS到MS的距离
for i = 1:BSNR0(i) =  sqrt((BS(1,i) - MS(1))^2 + (BS(2,i) - MS(2))^2 + (BS(3,i) - MS(3))^2);
end%噪声方差
Nosie = 1;%有噪声情况下BSi到MS的距离与BS1到MS的距离差，实际由TDOA*c求得
for i = 1:BSN-1R(i) = R0(i+1) - R0(1) +Nosie*randn(1);
endX = Chan3(BS,BSN,R)


X =28.968140.50268.4857

【论文】三维坐标下TDOA之chan算法相关推荐

基于TDOA的chan算法（定位算法）
Chan算法原理 TDOA(TDOA,the time differences of arrival,到达时间差),Chan算法是TDOA定位方法的一个很好用的方法. Chan算法是非递归双曲线方程组 ...
三维坐标偏转_三维坐标下的旋转
三维坐标的旋转有以下几种常见的表示形式:旋转向量,旋转矩阵,欧拉角,四元数,下面对这四种表示形式及其之间的转换进行介绍旋转向量通常为3x1的列向量,向量方向即为旋转轴,向量的模表示绕轴逆时针旋转的 ...
用C/C++实现输入IQ数据，计算时差（IQ数据互相关），完成TDOA定位（Chan算法）
文末贴源码链接需求:已知接收机IQ数据,根据IQ数据做互相关,求信号时差,最终通过TDOA(Chan算法)定位发射机坐标.输入输出坐标均为经纬度坐标,而在TDOA计算中,需要笛卡尔坐标系坐标,涉及坐 ...
matlab chan算法定位,MATLAB实现基于Chan氏算法的三维TDOA定位
% 功能:基于chan算法的TDOA三维定位 function [zp] = Chan_3(Noise,MS) %基站数目 BSN = 7; %基站位置,每一列为一个基站位置 BS = [0, 2*s ...
基于TDOA的Chan氏算法代码解释
基于TDOA的Chan氏算法(解释一下代码的问题) TDOA定位是啥自己找几个论文看看是啥就行,TDOA这玩意很适合超宽带定位,去找几篇博客或超宽带定位论文就能懂,这里不多说. 主要是对上传代码的解释 ...
TDOA定位 chan算法和Taylor算法比较仿真 matlab
最近在学习TDOA定位算法,需要比较chan(查恩算法)和Taylor(泰勒级数算法)的定位效果. 分别实现的效果图: 运动目标从零点开始沿x=y做匀速运动. 观测站坐标如下: 在不同的雷达测距误差下 ...
TDOA定位的Chan算法MATLAB源代码
TDOA定位的Chan算法MATLAB源代码 . function [POS_ref,POS1,POS2,POS3,POS4] = TDOA_chan(R,Pbs,Q) %************** ...
【老生谈算法】matlab实现TDOA定位的Chan算法源码——TDOA定位
TDOA定位的Chan算法MATLAB源代码 1.原文下载: 本算法原文如下,有需要的朋友可以点击进行下载序号原文(点击下载) 本项目原文 [老生谈算法]TDOA定位的Chan算法MATLAB源代 ...
Ubutntu下使用realsense d435i（三）：使用yolo v5测量目标物中心点三维坐标
01 参考本文下述使用参考的的工程均来自于下面的两个github yolo v5参考的代码:https://github.com/ultralytics/yolov5 本文参考的代码(仅获得深度): ...

【论文】三维坐标下TDOA之chan算法

【论文】三维坐标下TDOA之chan算法相关推荐

最新文章

热门文章