TDOA定位Taylor算法

基于TDOA的经典定位算法分为两类，一类是可以求出解析解的算法，如Fang算法、Chan算法的；另一类是迭代算法，如Taylor算法。

Taylor级数展开法是一种迭代算法，在Taylor级数展开的基础上，利用初始迭代值进行WLS估计，然后求解位置估计误差的局部最小二乘解，并对标签的位置进行更新。Taylor算法的前提是需要标签位置的初始估计值，而算法的主要思想是通过不断迭代来修正待定位标签位置的估计值，最后逐渐逼近标签真实的位置坐标。

标签与基站之间的约束关系可以通过某一函数fi(x,y,xi,yi){f_i}\left( {x,y,{x_i},{y_i}} \right)fi(x,y,xi,yi)来表达，该函数的测量值可以用MiM_iMi来表示，Mi=Mi0+ei{M_i} = M_i^0 + {e_i}Mi=Mi0+ei ,其中，Mi0M_i^0Mi0为函数的真实值，eie_iei表示测量误差。用η\etaη来代表误差门限值，那么，应在满足条件：Δx+Δy<η{\Delta x + \Delta y} < \etaΔx+Δy<η时停止迭代计算。

假设标签坐标的初始值为(x0,y0)({x_0},{y_0})(x0,y0)，真实值为(x,y)(x,y)(x,y)，且x=x0+Δxx = {x_0} + \Delta xx=x0+Δx，y=y0+Δyy = {y_0} + \Delta yy=y0+Δy，那么函数fi(x,y,xi,yi){f_i}\left( {x,y,{x_i},{y_i}} \right)fi(x,y,xi,yi)在(x0,y0)({x_0},{y_0})(x0,y0)处的Taylor级数展开结果如下：

fi(x,y,xi,yi)=fi(x0,y0,xi,yi)+(Δx∂∂x+Δy∂∂y)fi(x0,y0,xi,yi)+12!(Δx∂∂x+Δy∂∂y)2fi(x0,y0,xi,yi)+⋯+1n!(Δx∂∂x+Δy∂∂y)nfi(x0,y0,xi,yi)+⋯f_{i}\left(x, y, x_{i}, y_{i}\right)=f_{i}\left(x_{0}, y_{0}, x_{i}, y_{i}\right)+\left(\Delta x \frac{\partial}{\partial x}+\Delta y \frac{\partial}{\partial y}\right) f_{i}\left(x_{0}, y_{0}, x_{i}, y_{i}\right)\\ +\frac{1}{2 !}\left(\Delta x \frac{\partial}{\partial x}+\Delta y \frac{\partial}{\partial y}\right)^{2} f_{i}\left(x_{0}, y_{0}, x_{i}, y_{i}\right)+\cdots\\ +\frac{1}{n !}\left(\Delta x \frac{\partial}{\partial x}+\Delta y \frac{\partial}{\partial y}\right)^{n} f_{i}\left(x_{0}, y_{0}, x_{i}, y_{i}\right)+\cdotsfi(x,y,xi,yi)=fi(x0,y0,xi,yi)+(Δx∂x∂+Δy∂y∂)fi(x0,y0,xi,yi)+2!1(Δx∂x∂+Δy∂y∂)2fi(x0,y0,xi,yi)+⋯+n!1(Δx∂x∂+Δy∂y∂)nfi(x0,y0,xi,yi)+⋯

由基于TDOA的定位方法，可以得到以下的方程组：
Ri=(x−xi)2+(y−yi)2Ri1=(x−xi)2+(y−yi)2−(x−x1)2+(y−y1)2\begin{aligned} &R_{i}=\sqrt{\left(x-x_{i}\right)^{2}+\left(y-y_{i}\right)^{2}}\\ &R_{i 1}=\sqrt{\left(x-x_{i}\right)^{2}+\left(y-y_{i}\right)^{2}}-\sqrt{\left(x-x_{1}\right)^{2}+\left(y-y_{1}\right)^{2}}\\ \end{aligned} Ri=(x−xi)2+(y−yi)2Ri1=(x−xi)2+(y−yi)2−(x−x1)2+(y−y1)2

其中，(x,y)(x,y)(x,y)表示标签的坐标，Ri1=Ri−R1{R_{i1}} = {R_i} - {R_1}Ri1=Ri−R1表示标签到第iii个基站与到第1个基站的距离差值。

由上式构造函数fi(x,y,xi,yi){f_i}\left( {x,y,{x_i},{y_i}} \right)fi(x,y,xi,yi)：

fi(x,y,xi,yi)=c(ti−tj)=Ri−Rj=(xi−x)2+(yi−y)2−(xj−x)2+(yj−y)2\begin{aligned} &f_{i}\left(x, y, x_{i}, y_{i}\right)=c\left(t_{i}-t_{j}\right)=R_{i}-R_{j}\\ &=\sqrt{\left(x_{i}-x\right)^{2}+\left(y_{i}-y\right)^{2}}-\sqrt{\left(x_{j}-x\right)^{2}+\left(y_{j}-y\right)^{2}}\\ \end{aligned} fi(x,y,xi,yi)=c(ti−tj)=Ri−Rj=(xi−x)2+(yi−y)2−(xj−x)2+(yj−y)2

将fi(x,y,xi,yi){f_i}\left( {x,y,{x_i},{y_i}} \right)fi(x,y,xi,yi)在(x0,y0)({x_0},{y_0})(x0,y0)处进行泰勒展开，并忽略二阶以上的分量，有：

fi(x,y,xi,yi)=fi(x0,y0,xi,yi)+(Δx∂∂x+Δy∂∂y)fi(x0,y0,xi,yi){f_i}\left( {x,y,{x_i},{y_i}} \right) = {f_i}\left( {{x_0},{y_0},{x_i},{y_i}} \right) + \left( {\Delta x\frac{\partial }{{\partial x}} + \Delta y\frac{\partial }{{\partial y}}} \right){f_i}\left( {{x_0},{y_0},{x_i},{y_i}} \right)fi(x,y,xi,yi)=fi(x0,y0,xi,yi)+(Δx∂x∂+Δy∂y∂)fi(x0,y0,xi,yi)

转化为矩阵的形式，有：

ψ=hi−Giδ\psi = {{\bf{h}}_i} - {{\bf{G}}_i}{\bf{\delta }}ψ=hi−Giδ

其中，ψ\psiψ为误差矢量，另有

hi=[R2,1−(R2−R1)R3,1−(R3−R1)⋮RL,1−(RL−R1)]Gi=[x1−xR1−x2−xR2y1−yR1−y2−yR2x1−xR1−x3−xR3y1−yR1−y3−yR3⋮⋮x1−xR1−xL−xRLy1−yR1−yL−yRL]δ=[ΔxΔy]\begin{aligned} &\mathbf{h}_{i}=\left[\begin{array}{c}R_{2,1}-\left(R_{2}-R_{1}\right) \\ R_{3,1}-\left(R_{3}-R_{1}\right) \\ \vdots \\ R_{L, 1}-\left(R_{L}-R_{1}\right)\end{array}\right]\\ &\mathbf{G}_{i}=\left[\begin{array}{cc}\frac{x_{1}-x}{R_{1}}-\frac{x_{2}-x}{R_{2}} & \frac{y_{1}-y}{R_{1}}-\frac{y_{2}-y}{R_{2}} \\ \frac{x_{1}-x}{R_{1}}-\frac{x_{3}-x}{R_{3}} & \frac{y_{1}-y}{R_{1}}-\frac{y_{3}-y}{R_{3}} \\ \vdots & \vdots \\ \frac{x_{1}-x}{R_{1}}-\frac{x_{L}-x}{R_{L}} & \frac{y_{1}-y}{R_{1}}-\frac{y_{L}-y}{R_{L}}\end{array}\right]\\ &\delta=\left[\begin{array}{c}\Delta x \\ \Delta y\end{array}\right]\\ \end{aligned} hi=⎣⎢⎢⎢⎡R2,1−(R2−R1)R3,1−(R3−R1)⋮RL,1−(RL−R1)⎦⎥⎥⎥⎤Gi=⎣⎢⎢⎢⎡R1x1−x−R2x2−xR1x1−x−R3x3−x⋮R1x1−x−RLxL−xR1y1−y−R2y2−yR1y1−y−R3y3−y⋮R1y1−y−RLyL−y⎦⎥⎥⎥⎤δ=[ΔxΔy]

Ri(i=1,2,3,⋯,L){R_i}(i = 1,2,3, \cdots ,L)Ri(i=1,2,3,⋯,L)表示在一次迭代计算中标签和第i个基站之间的距离。

上式的加权最小二乘解为：

δ=[ΔxΔy]=(GiTQ−1Gi)−1GiTQ−1hi\delta=\left[\begin{array}{c}\Delta x \\ \Delta y\end{array}\right]=\left(\mathbf{G}_{i}^{T} \mathbf{Q}^{-1} \mathbf{G}_{i}\right)^{-1} \mathbf{G}_{i}^{T} \mathbf{Q}^{-1} \mathbf{h}_{i}δ=[ΔxΔy]=(GiTQ−1Gi)−1GiTQ−1hi

其中，Q{\bf{Q}}Q表示TDOA测量值的协方差矩阵。在下一次递归计算中，令x′=x0+Δxx' = {x_0} + \Delta xx′=x0+Δx，y′=y0+Δyy' = {y_0} + \Delta yy′=y0+Δy，更新标签的坐标值进行迭代计算。重复以上过程，直到误差满足设定的门限值时停止迭代计算，即：∣Δx∣+∣Δy∣<η\left| {\Delta x} \right| + \left| {\Delta y} \right| < \eta∣Δx∣+∣Δy∣<η。

References
[1] 宋洋. 超宽带室内定位技术研究[D].西安科技大学,2019.

代码实现（matlab）

X_estimate = 50;
Y_estimate = 50;
Xb = [200 200];
X = [200 0; -100 173; -100 -173];
Noise = 0.1*randn(3,1);
Real_ms = [20 20];BSN = size(X,1);
x = X_estimate;
y = Y_estimate;MS = [x,y];
iEP = MS;%%
Rb = sqrt((Real_ms(1) - Xb(1))^2+(Real_ms(2) - Xb(2))^2);
RD = zeros(BSN,1);
for i = 1:BSN                RD(i) = -Rb+sqrt((Real_ms(1)- X(i,1))^2+(Real_ms(2) - X(i,2))^2)+Noise(i);
end
%%
% TDOA协方差矩阵Q
Q = 0.01*eye(BSN);
delta = [1 1];
kk = 0;
% Taylor级数展开法
while ((abs(delta(1)) + abs(delta(2))) > 0.01)   R1 = sqrt((iEP(1) - Xb(1))^2 + (iEP(2) - Xb(2))^2);   R = zeros(1,BSN);kk = kk+1;for i = 1: BSN R(i) = sqrt((iEP(1) - X(i,1))^2 + (iEP(2) - X(i,2))^2);end% hihi = zeros(BSN,1);for i = 1: BSNhi(i) = RD(i) - (R(i) - R1);end% GiGi = zeros(BSN,2);for i = 1: BSNGi(i, 1) = (Xb(1)-iEP(1))/R1 - (X(i,1) - iEP(1))/R(i);Gi(i, 2) = (Xb(2)-iEP(2))/R1 - (X(i,2) - iEP(2))/R(i);end% deltadelta = inv(Gi'*inv(Q)*Gi)*Gi'*inv(Q)*hi;   if (abs(delta(1))+abs(delta(2))) > 0.01   EP = iEP + delta';   iEP = EP;   % 更新迭代值end
end% 输出
z_out = iEP;   % 标签坐标估计值
mse = sqrt((z_out(1)-Real_ms(1))^2+(z_out(2)-Real_ms(2))^2);   % 均方误差

配套pdf下载
https://download.csdn.net/download/gongshouxiayin/13658167

TDOA定位Taylor算法相关推荐

TDOA定位 chan算法和Taylor算法比较仿真 matlab
最近在学习TDOA定位算法,需要比较chan(查恩算法)和Taylor(泰勒级数算法)的定位效果. 分别实现的效果图: 运动目标从零点开始沿x=y做匀速运动. 观测站坐标如下: 在不同的雷达测距误差下 ...
【定位问题】基于matlab TDOA+taylor算法移动基站无源定位【含Matlab源码 2098期】
⛄一.chan+taylor算法移动基站无源定位简介 1 引言随着无人机的普及,低空空域的安全问题受到人们的极大关注．针对该问题,本研究对"非合作型"无人机采用一种基于时差法的无 ...
TDOA定位的Chan算法MATLAB源代码
TDOA定位的Chan算法MATLAB源代码 . function [POS_ref,POS1,POS2,POS3,POS4] = TDOA_chan(R,Pbs,Q) %************** ...
在matlab下实现TDOA定位算法性能仿真
一.题目 TDOA定位算法性能仿真二.仿真要求要求一:编写两个函数TDOA_CHAN和TDOA_Taylor得到位置的估计. 要求二:用RMSE实现两种算法的性能比较, 得到两种算法的RMSE曲线 ...
基于TDOA的chan算法（定位算法）
Chan算法原理 TDOA(TDOA,the time differences of arrival,到达时间差),Chan算法是TDOA定位方法的一个很好用的方法. Chan算法是非递归双曲线方程组 ...
用C/C++实现输入IQ数据，计算时差（IQ数据互相关），完成TDOA定位（Chan算法）
文末贴源码链接需求:已知接收机IQ数据,根据IQ数据做互相关,求信号时差,最终通过TDOA(Chan算法)定位发射机坐标.输入输出坐标均为经纬度坐标,而在TDOA计算中,需要笛卡尔坐标系坐标,涉及坐 ...
【老生谈算法】matlab实现TDOA定位的Chan算法源码——TDOA定位
TDOA定位的Chan算法MATLAB源代码 1.原文下载: 本算法原文如下,有需要的朋友可以点击进行下载序号原文(点击下载) 本项目原文 [老生谈算法]TDOA定位的Chan算法MATLAB源代 ...
TDOA定位方法的Fang算法的程序实现（二维）
TDOA定位方法的Fang算法的程序实现(二维) 摘要: 蜂窝无线定位服务(LCS)是一种具有广阔市场前景的移动增值业务,其基本原理是利用现有蜂窝网络,通过对各种位置特征参数,包括到达时间(TOA), ...
【综合算法】不考虑误差的TDOA定位
不考虑误差的TDOA定位 TDOA是一种无线定位技术,是一种利用时间差进行定位的方法. 不同于TOA,TDOA(到达时间差)是通过检测信号到达两个基站的时间差,而不是到达的绝对时间来确定移动台的位置, ...

TDOA定位Taylor算法

TDOA定位Taylor算法相关推荐

最新文章

热门文章