CHAN时延估计算法（二维/三维定位实现）

一、算法核心原理：把"双曲线"掰直

在无线通信或声纳定位中，我们很难直接测出移动台（MS，比如你的手机）到基站（BS）的绝对距离。但我们可以轻松算出时间差（TDOA），也就是声音或电磁波从 MS 传到 BS1 比传到 BS2 晚了多久。

这个"时间差 × 光速"，就画出了一个以两个基站为焦点的双曲线。

CHAN算法（Chan's Method）的高明之处在于：它不直接去硬解双曲线的交点，而是通过两步加权最小二乘法（WLS），巧妙地把这些非线性的"双曲线方程"强行掰直，变成我们熟悉的线性方程 y=Ax+by = Ax + by=Ax+b 来解。

二、算法执行"两步走"战略

假设我们要解算 MS 的坐标 (x,y)(x, y)(x,y)，已知有 NNN 个基站的坐标 (xi,yi)(x_i, y_i)(xi,yi)。

第一步：初步定位（消除二次项）

目标：利用 BS1（作为主站）和其他基站的时间差，建立方程组。
数学魔法 ：将方程 Ri−R1=c⋅τiR_i - R_1 = c \cdot \tau_iRi−R1=c⋅τi 两边平方，利用平方差公式，原本复杂的根号项（代表距离）就被"消掉"了。
结果：得到一个关于 x,yx, yx,y 的线性方程组。这一步算出来的坐标是初步估计值。

第二步：加权修正（压榨残差的剩余价值）

痛点：第一步算出来的初步坐标是有误差的，因为平方项把误差放大了。
解法：我们把第一步算出的误差看作一个新的未知数。再次利用第一步的方程，建立一个关于"误差"的新方程组。
核心：引入加权矩阵（Weight Matrix）。因为距离越远，测量误差通常越大，加权可以让远距离基站说的话"轻一点"，近处的"重一点"。
结果：得到一个极其精准的最终坐标。

三、MATLAB 代码实战

它包含了一个核心算法函数和一个演示如何使用它的脚本。

3.1 核心算法函数 (`chan_algorithm.m`)

matlab 复制代码

function [ms_pos, cov_ms] = chan_algorithm(bs, rdoa, sigma)
% CHAN算法实现二维/三维定位
% 输入:
%   bs    - 基站坐标矩阵 [N x 2] 或 [N x 3]，N为基站个数
%   rdoa  - 到达时间差向量 [N-1 x 1]，(R_i - R_1)
%   sigma - 测量误差标准差 (标量或向量)
% 输出:
%   ms_pos - 移动台最终估计坐标
%   cov_ms - 估计结果的协方差矩阵 (反映定位精度)

    % ========== 数据准备 ==========
    num_bs = size(bs, 1);
    dim = size(bs, 2); % 判断是2D还是3D定位
    
    if num_bs < dim + 1
        error('基站数量不足！二维至少需要3个，三维至少需要4个。');
    end

    % --- 第一步：第一次加权最小二乘法 (WLS) ---
    
    % 构建矩阵 h，其中包含常数项和坐标乘积项
    h = zeros(num_bs - 1, 1);
    Ga = zeros(num_bs - 1, dim);
    
    % 提取主站（基站1）的坐标
    x1 = bs(1, :);
    
    for i = 2:num_bs
        % R_i^2 - R_1^2 = (K_i - K_1) - 2*(x1 - xi)*x_ms - 2*(y1 - yi)*y_ms
        K(i-1) = bs(i, :) * bs(i, :)' - rdoa(i-1)^2;
        h(i-1) = K(i-1) - (x1 * x1.'); 
        
        Ga(i-1, :) = bs(1, :) - bs(i, :);
    end

    % 构建权重矩阵 B (假设误差服从高斯分布)
    % B 是对角阵，对角元素是 R_i^2 的方差
    B = diag(4 * sigma^2 .* (rdoa.^2));
    
    % 第一次WLS解算：z_a = inv(Ga' * inv(B) * Ga) * Ga' * inv(B) * h
    za = (Ga' * (B \ Ga)) \ (Ga' * (B \ h));
    
    % 提取第一次的粗略估计坐标
    ms_init = [za(1:dim); bs(1, :)'];


    % --- 第二步：第二次WLS修正 (利用第一步的误差) ---
    
    % 构建新的矩阵 h'
    hp = zeros(num_bs - 1, 1);
    Gp = zeros(num_bs - 1, dim);
    
    for i = 2:num_bs
        ri_hat = norm(ms_init - bs(i, :)');
        hp(i-1) = (rdoa(i-1))^2 - za(dim+1) + 2*rdoa(i-1)*ri_hat;
        Gp(i-1, :) = (ms_init - bs(i, :)')' / ri_hat;
    end

    % 构建新的权重矩阵 B'
    Bp = diag(4 * sigma^2 .* (rdoa.^2));

    % 第二次WLS解算
    zp = (Gp' * (Bp \ Gp)) \ (Gp' * (Bp \ hp));

    % 最终坐标 = 初始坐标 + 修正量
    ms_pos = ms_init + zp;
    
end

参考代码 CHAN时延估计算法 www.youwenfan.com/contentcsu/55230.html

3.2 演示 (`demo_chan.m`)