Chan算法原理
TDOA(TDOA，the time differences of arrival，到达时间差)，Chan算法是TDOA定位方法的一个很好用的方法。
Chan算法是非递归双曲线方程组解法，具有解析表达式解，主要特点是：在测量误差服从理想高斯分布时，它的定位精度高、计算量小，并且可以通过增加已确定点的数量来提高算法精度。
该算法的推导前提是基于测量误差为零均值高斯随机变量，对于实际环境中误差较大的测量值，比如在有非视距误差的环境下，该算法的性能会有显著下降。二维情况下，可分为只有三个点参与定位和三个点以上参与定位。
已知坐标(x1,y1),(x2,y2),(x3,y3)(x_1, y_1), (x_2, y_2), (x_3, y_3)(x1,y1),(x2,y2),(x3,y3)，假设第未知点的坐标是(x,y)(x, y)(x,y)根据几何关系定义一下关系表达式：
ri=(xi−x)2+(yi−y)2r_i = \sqrt{(x_i-x)^2+(y_i-y)^2}ri=(xi−x)2+(yi−y)2 ………(1)\dots\dots\dots(1)………(1)
ri,1=ri−r1=(xi−x)2+(yi−y)2−(x1−x)2+(y1−y)2r_{i,1}=r_i-r_1=\sqrt{(x_i-x)^2+(y_i-y)^2}-\sqrt{(x_1-x)^2+(y_1-y)^2}ri,1=ri−r1=(xi−x)2+(yi−y)2−(x1−x)2+(y1−y)2………(2)\dots\dots\dots(2)………(2)
根据(1)另有如下关系：先令Ki=xi2+yi2K_i=x_i^2+y_i^2Ki=xi2+yi2
ri2=xi2+yi2=Ki−2xix−2yiy+x2+y2r_i^2=x_i^2+y_i^2=K_i-2x_ix-2y_iy+x^2+y^2ri2=xi2+yi2=Ki−2xix−2yiy+x2+y2………(3)\dots\dots\dots(3)………(3)
根据(2)另外有如下关系：
ri2=(ri,1+ri)2r_i^2=(r_{i,1}+r_i)^2ri2=(ri,1+ri)2………(4)\dots\dots\dots(4)………(4)
将(3)代入(4)可推出如下关系（关键）：
ri,12+2ri,1r1=xi2+yi2−2xix−2yiy+2x1x+2y1y−(x12+y12)=ri2−r12r_{i,1}^2+2r_{i,1}r_1=x_i^2+y_i^2-2x_ix-2y_iy+2x_1x+2y_1y-(x_1^2+y_1^2)=r_i^2-r_1^2ri,12+2ri,1r1=xi2+yi2−2xix−2yiy+2x1x+2y1y−(x12+y12)=ri2−r12………(5)\dots\dots\dots(5)………(5)
进一步，令 xi,1=xi−x1x_{i,1}=x_i-x_1xi,1=xi−x1，有
ri,12+2ri,1r1=(Ki−K1)−2xi,1x−2yi,1yr_{i,1}^2+2r_{i,1}r_1=(K_i-K_1)-2x_{i,1}x-2y_{i,1}yri,12+2ri,1r1=(Ki−K1)−2xi,1x−2yi,1y
这里（5）是关键一步：消除了未知数的平方项，仅保留一系列的线性方程。当i=1,2,3i = 1, 2, 3i=1,2,3时，有如下表达式：
通过消元法进行求解：
接下来就是如何求解(x,y)(x,y)(x,y)
首先，令
有如下非常简洁的形式：
其中只有r1r_1r1是未知的。因此要想求出未知坐标(x,y)(x,y)(x,y)，由式（9）可知，首先要求出r1r_1r1。
接下来，将式(9)代入到式(3)，ri2=xi2+yi2=Ki−2xix−2yiy+x2+y2,i=1r_i^2=x_i^2+y_i^2=K_i-2x_ix-2y_iy+x^2+y^2, i = 1ri2=xi2+yi2=Ki−2xix−2yiy+x2+y2,i=1可以化为如下形式：
(q12+q22−1)r12−2[q1(x1−p1)+q2(y1−p2)]r1+[(x1−p1)2+(y1−p2)2]=0………(10)(q_1^2+q_2^2-1)r_1^2-2[q_1(x_1-p_1)+q_2(y_1-p_2)]r_1+[(x_1-p_1)^2+(y_1-p_2)^2]=0\dots\dots\dots(10)(q12+q22−1)r12−2[q1(x1−p1)+q2(y1−p2)]r1+[(x1−p1)2+(y1−p2)2]=0………(10)
→ a∗r2+b∗r+c=0，r=r1a*r^2+b*r+c=0，r = r_1a∗r2+b∗r+c=0，r=r1
式子（10）是r1r_1r1的一元二次方程。求解式(10)可得rrr的两个根，根据先验信息可以舍去一个无效的根，将有效的rrr代会到式（9）中可求出未知点的坐标。
代码
clear all;clc;%% 初始化数值c = 3120.77; % 标准值t1 = 0.22 * 0.0001; % 传感器1的时间延迟 t2 = 0.58 * 0.0001; % 传感器2的时间延迟x1 = 0;y1 = 0.5; % 传感器1x2 = 0;y2 = 0; % 传感器2x3 = 0.5; y3 = 0; % 传感器3s1 = c * t1; % 时延距离 s1 = r21s2 = c * t2; % 试验距离 s2 = r31%% 求解未知点k1 = x1^2 + y1^2; % 中间值k2 = x2^2 + y2^2;k3 = x3^2 + y3^2;p1_molecule = (y2 - y1)*s2^2 - (y3 - y1)*s1^2 + (y3 - y1)*(k2 - k1) - (y2 - y1)*(k3 - k1);p1_denominator = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);p1 = p1_molecule / (p1_denominator*2);q1 = ((y2 - y1)*s2 - (y3 - y1)*s1)/((x2 - x1)*(y3 - y1)-(x3 - x1)*(y2 - y1));p2_molecule = (x2 - x1)*s2^2 - (x3 - x1)*s1^2 + (x3 - x1)*(k2 - k1) - (x2 - x1)*(k3 - k1);p2_denominator = (x3 - x1)*(y2 - y1) - (x2 - x1)*(y3 - y1);p2 = p2_molecule / (p2_denominator * 2);q2 = ((x2 - x1)*s2 - (x3 - x1)*s1)/((x3 - x1)*(y2 - y1)-(x2 - x1)*(y3 - y1));%% 求取方程a = q1^2 + q2^2 -1;b = -2*((x1 - p1)*q1 + (y1 - p2)*q2);c = (x1 - p1)^2 + (y1 - p2)^2;r1 = (-b + sqrt(b^2 - 4*a*c))/(2*a);r2 = (-b - sqrt(b^2 - 4*a*c))/(2*a);if r1 > 0x = p1 + q1*r1;y = p2 + q2*r1;elsex = p1 + q1*r2;y = p2 + q2*r2;enddisp('未知点坐标: ')disp(x), disp(y)
