TDOA算法中Chan算法的代码实现

TDOA（Time Difference of Arrival）算法是一种基于时间差的定位算法，其中Chan算法是一种常用的TDOA算法。下面是Chan算法的代码实现：

import numpy as np

def chan_algorithm(anchor_positions, distances):
    """
    :param anchor_positions: array of shape (n, 2) containing the positions of the n anchors
    :param distances: array of shape (n,) containing the distances from the unknown point to each anchor
    :return: estimated position of the unknown point
    """
    n = anchor_positions.shape[0]
    A = np.zeros((n - 1, 2))
    b = np.zeros((n - 1,))
    for i in range(1, n):
        A[i - 1, :] = anchor_positions[i, :] - anchor_positions[0, :]
        b[i - 1] = (distances[0]**2 - distances[i]**2
                    + np.sum(anchor_positions[i, :]**2)
                    - np.sum(anchor_positions[0, :]**2)) / 2
    x = np.linalg.lstsq(A, b, rcond=None)[0]
    return anchor_positions[0, :] + x

其中，`anchor_positions`是一个形状为`(n, 2)`的数组，包含n个锚点的位置坐标；`distances`是一个形状为`(n,)`的数组，包含从未知点到每个锚点的距离。函数返回估计的未知点位置。

该算法的实现过程如下：
. 构造一个(n-1)×2的矩阵A和一个(n-1,)的向量b，其中n是锚点的数量。
2. 对于每个锚点i（i从1到n-1），计算A[i-1,:]和b[i-1]的值。
3. 使用最小二乘法求解线性方程组Ax=b，其中A是矩阵A，x是未知点的坐标，b是向量b。
4. 返回估计的未知点位置。