TOA定位算法MATLAB实现（二维三维场景）

一、TOA定位原理概述

1.1 基本原理

到达时间（Time of Arrival, TOA）定位技术通过测量信号从发射源到多个接收节点的传播时间，结合信号传播速度（如光速c=3×108m/sc=3×10^8 m/sc=3×108m/s），计算出发射源与各节点的距离，再利用几何关系确定发射源位置。

核心公式：

di=c⋅tid_i=c⋅t_idi=c⋅ti

其中，did_idi为发射源到第iii个接收节点的距离，tit_iti为信号传播时间，ccc为信号传播速度。

1.2 定位模型

二维场景（最常见）

设未知节点坐标为(x,y)(x,y)(x,y)，锚节点（已知位置）坐标为(xi,yi)（i=1,2,...,N，N≥3）(x_i,y_i)（i=1,2,...,N，N≥3）(xi,yi)（i=1,2,...,N，N≥3），则距离方程为：

(x−xi)2+(y−yi)2=di+ni\sqrt{(x−x_i)^2+(y−y_i)^2}=d_i+n_i(x−xi)2+(y−yi)2 =di+ni

其中，nin_ini为测量噪声（通常假设为零均值高斯噪声）。

三维场景

扩展至三维空间，未知节点坐标为(x,y,z)(x,y,z)(x,y,z)，距离方程为：

(x−xi)2+(y−yi)2+(z−zi)2=di+ni\sqrt{(x−x_i)^2+(y−y_i)^2+(z−z_i)^2}=d_i+n_i(x−xi)2+(y−yi)2+(z−zi)2 =di+ni

1.3 定位算法分类

算法类型	核心思想	适用场景	复杂度
最小二乘法（LS）	直接最小化残差平方和	低噪声、节点布局规则	低
加权最小二乘法（WLS）	考虑测量噪声方差的加权优化	噪声非均匀场景	中
泰勒级数展开法	迭代线性化非线性方程	高精度定位、初始值已知	中
Chan算法	闭式解（解析解），抗噪声能力强	视距（LOS）环境、高精度需求	中

二、MATLAB实现（二维场景）

2.1 主程序框架

function toa_localization()
    % TOA定位算法主程序
    % 步骤：
    % 1. 参数配置（节点部署、噪声等）
    % 2. 生成距离测量值（含噪声）
    % 3. 实现定位算法（LS/WLS/Taylor/Chan）
    % 4. 性能评估（RMSE、定位误差）
    % 5. 可视化结果
    
    % 参数配置
    params = configure_toa_params();
    
    % 生成节点位置与真实距离
    [anchor_pos, target_pos, true_dist] = generate_node_positions(params);
    
    % 添加测量噪声（TOA时间转距离）
    noisy_dist = add_measurement_noise(true_dist, params.noise_var, params.c);
    
    % 定位算法实现
    algorithms = {'LS', 'WLS', 'Taylor', 'Chan'};
    est_pos = cell(1, length(algorithms));
    rmse = zeros(1, length(algorithms));
    
    for i = 1:length(algorithms)
        method = algorithms{i};
        est_pos{i} = toa_algorithm(anchor_pos, noisy_dist, method, params);
        rmse(i) = sqrt(mean((est_pos{i} - target_pos).^2)); % RMSE
    end
    
    % 性能评估与可视化
    evaluate_performance(target_pos, est_pos, rmse, algorithms, params);
    visualize_results(anchor_pos, target_pos, est_pos, algorithms, params);
end

2.2 参数配置

function params = configure_toa_params()
    % TOA定位参数配置
    params = struct();
    
    % 场景参数
    params.dim = 2;                  % 维度（2D/3D）
    params.area_size = [100, 100];   % 区域大小（m×m）
    params.num_anchors = 4;          % 锚节点数量（≥3 for 2D）
    params.target_pos = [30, 40];    % 目标节点真实位置（x,y）
    
    % 信号参数
    params.c = 3e8;                  % 光速（m/s）
    params.noise_var = 1e-8;         % 时间测量噪声方差（s²）→ 距离噪声方差= c²·noise_var
    params.time_error_std = sqrt(params.noise_var); % 时间测量标准差（s）
    
    % 算法参数
    params.max_iter = 20;            % Taylor算法最大迭代次数
    params.tol = 1e-3;               % 迭代收敛阈值
    params.chan_confidence = 0.95;   % Chan算法置信度
end

2.3 节点位置生成

function [anchor_pos, target_pos, true_dist] = generate_node_positions(params)
    % 生成锚节点和目标节点位置
    rng(0); % 固定随机种子，确保可复现
    
    if params.dim == 2
        % 二维场景：锚节点随机部署在区域内（避免共线）
        anchor_pos = rand(params.num_anchors, 2) * params.area_size(1);
        % 确保锚节点不共线（简单处理：固定一个锚节点在原点）
        anchor_pos(1,:) = [0, 0];
        anchor_pos(2,:) = [params.area_size(1), 0];
        anchor_pos(3,:) = [0, params.area_size(2)];
        if params.num_anchors > 3
            anchor_pos(4:end,:) = rand(params.num_anchors-3, 2) * params.area_size(1);
        end
        
        % 目标节点真实位置
        target_pos = params.target_pos;
        
        % 计算真实距离（无噪声）
        true_dist = sqrt(sum((anchor_pos - target_pos).^2, 2));
    else
        % 三维场景（扩展）
        anchor_pos = rand(params.num_anchors, 3) * params.area_size(1);
        target_pos = params.target_pos;
        true_dist = sqrt(sum((anchor_pos - target_pos).^2, 2));
    end
end

2.4 距离测量噪声添加

function noisy_dist = add_measurement_noise(true_dist, noise_var, c)
    % 添加TOA测量噪声（时间→距离）
    time_noise = randn(size(true_dist)) * sqrt(noise_var); % 时间噪声（s）
    dist_noise = c * time_noise; % 距离噪声（m）
    noisy_dist = true_dist + dist_noise;
end

2.5 核心定位算法实现

2.5.1 最小二乘法（LS）

function est_pos = ls_algorithm(anchor_pos, noisy_dist, params)
    % 最小二乘法（LS）定位
    num_anchors = size(anchor_pos, 1);
    A = [];
    b = [];
    
    for i = 1:num_anchors
        xi = anchor_pos(i, 1); yi = anchor_pos(i, 2);
        di = noisy_dist(i);
        A = [A; 2*(xi - anchor_pos(1,1)), 2*(yi - anchor_pos(1,2))];
        b = [b; xi^2 + yi^2 - di^2 - (anchor_pos(1,1)^2 + anchor_pos(1,2)^2 - noisy_dist(1)^2)];
    end
    
    % 求解线性方程组 Ax = b
    est_pos = (A'*A) \ (A'*b); % 最小二乘解
end

2.5.2 加权最小二乘法（WLS）

function est_pos = wls_algorithm(anchor_pos, noisy_dist, params)
    % 加权最小二乘法（WLS）定位
    num_anchors = size(anchor_pos, 1);
    A = [];
    b = [];
    W = diag(1./noisy_dist.^2); % 权重矩阵（噪声方差与距离平方成正比）
    
    for i = 1:num_anchors
        xi = anchor_pos(i, 1); yi = anchor_pos(i, 2);
        di = noisy_dist(i);
        A = [A; 2*(xi - anchor_pos(1,1)), 2*(yi - anchor_pos(1,2))];
        b = [b; xi^2 + yi^2 - di^2 - (anchor_pos(1,1)^2 + anchor_pos(1,2)^2 - noisy_dist(1)^2)];
    end
    
    % 求解加权最小二乘问题：min ||W^(1/2)(Ax - b)||²
    est_pos = (A'*W*A) \ (A'*W*b);
end

2.5.3 泰勒级数展开法

function est_pos = taylor_algorithm(anchor_pos, noisy_dist, params)
    % 泰勒级数展开法（迭代优化）
    num_anchors = size(anchor_pos, 1);
    est_pos = mean(anchor_pos, 1)'; % 初始估计（锚节点质心）
    prev_pos = est_pos + 1e6; % 确保进入迭代
    
    for iter = 1:params.max_iter
        if norm(est_pos - prev_pos) < params.tol
            break;
        end
        prev_pos = est_pos;
        
        % 构建雅可比矩阵J和残差向量r
        J = [];
        r = [];
        for i = 1:num_anchors
            xi = anchor_pos(i, 1); yi = anchor_pos(i, 2);
            dx = est_pos(1) - xi; dy = est_pos(2) - yi;
            di = sqrt(dx^2 + dy^2);
            J = [J; dx/di, dy/di]; % 雅可比矩阵行
            r = [r; di - noisy_dist(i)]; % 残差
        end
        
        % 迭代更新：Δx = -(J^T J)^-1 J^T r
        delta = -(J'*J) \ (J'*r);
        est_pos = est_pos + delta';
    end
    est_pos = est_pos'; % 转为行向量
end

2.5.4 Chan算法（闭式解）

function est_pos = chan_algorithm(anchor_pos, noisy_dist, params)
    % Chan算法（TOA定位闭式解）
    num_anchors = size(anchor_pos, 1);
    c = params.c;
    sigma = params.time_error_std; % 时间测量标准差（s）
    
    % 第一步：计算初始估计（类似LS）
    A = [];
    b = [];
    for i = 1:num_anchors
        xi = anchor_pos(i, 1); yi = anchor_pos(i, 2);
        di = noisy_dist(i);
        A = [A; 2*(xi - anchor_pos(1,1)), 2*(yi - anchor_pos(1,2))];
        b = [b; xi^2 + yi^2 - di^2 - (anchor_pos(1,1)^2 + anchor_pos(1,2)^2 - noisy_dist(1)^2)];
    end
    theta = (A'*A) \ (A'*b); % 初始位置估计
    x0 = theta(1); y0 = theta(2);
    
    % 第二步：计算距离差和协方差矩阵
    d0 = sqrt((x0 - anchor_pos(1,1))^2 + (y0 - anchor_pos(1,2))^2);
    ri = noisy_dist;
    ri0 = d0;
    r = ri - ri0; % 距离差
    
    G = [];
    for i = 2:num_anchors
        xi = anchor_pos(i, 1); yi = anchor_pos(i, 2);
        dx = x0 - xi; dy = y0 - yi;
        di = sqrt(dx^2 + dy^2);
        G = [G; (x0 - xi)/di, (y0 - yi)/di, 1];
    end
    
    % 协方差矩阵Q
    Q = (2*c*sigma)^2 * eye(num_anchors); % 距离测量协方差
    Q_inv = inv(Q);
    phi = G * Q_inv * G';
    phi_inv = inv(phi);
    
    % 第三步：求解偏移量
    z = r(2:end); % 排除第一个锚节点
    mu = phi_inv * G * Q_inv * z;
    delta_x = mu(1); delta_y = mu(2); delta_rho = mu(3);
    
    % 最终估计
    est_pos = [x0 + delta_x, y0 + delta_y];
end

2.6 算法统一调用接口

function est_pos = toa_algorithm(anchor_pos, noisy_dist, method, params)
    % TOA算法统一调用接口
    switch lower(method)
        case 'ls'
            est_pos = ls_algorithm(anchor_pos, noisy_dist, params);
        case 'wls'
            est_pos = wls_algorithm(anchor_pos, noisy_dist, params);
        case 'taylor'
            est_pos = taylor_algorithm(anchor_pos, noisy_dist, params);
        case 'chan'
            est_pos = chan_algorithm(anchor_pos, noisy_dist, params);
        otherwise
            error('未知算法: %s', method);
    end
end

2.7 性能评估与可视化

function evaluate_performance(target_pos, est_pos, rmse, algorithms, params)
    % 性能评估（RMSE）
    fprintf('===== TOA定位性能评估 =====\n');
    fprintf('真实位置: (%.2f, %.2f)\n', target_pos(1), target_pos(2));
    fprintf('噪声标准差: %.2e m\n', params.c*sqrt(params.noise_var));
    fprintf('-----------------------------\n');
    fprintf('算法\t\t估计位置\t\tRMSE (m)\n');
    for i = 1:length(algorithms)
        pos = est_pos{i};
        fprintf('%s\t(%.2f, %.2f)\t%.2f\n', algorithms{i}, pos(1), pos(2), rmse(i));
    end
end

function visualize_results(anchor_pos, target_pos, est_pos, algorithms, params)
    % 可视化结果
    figure('Name', 'TOA定位结果', 'Position', [100, 100, 800, 600]);
    
    % 绘制锚节点和目标节点
    scatter(anchor_pos(:,1), anchor_pos(:,2), 100, 'ro', 'filled', 'DisplayName', '锚节点');
    hold on;
    scatter(target_pos(1), target_pos(2), 150, 'b*', 'DisplayName', '真实位置');
    
    % 绘制各算法估计位置
    colors = lines(length(algorithms));
    for i = 1:length(algorithms)
        pos = est_pos{i};
        scatter(pos(1), pos(2), 100, colors(i,:), 's', 'filled', 'DisplayName', [algorithms{i} '估计']);
        plot([target_pos(1), pos(1)], [target_pos(2), pos(2)], 'k--', 'HandleVisibility', 'off');
    end
    
    % 图形设置
    axis equal;
    grid on;
    xlim([0, params.area_size(1)]);
    ylim([0, params.area_size(2)]);
    xlabel('X坐标 (m)');
    ylabel('Y坐标 (m)');
    title('TOA定位结果对比');
    legend('Location', 'Best');
    
    % 绘制误差直方图（以LS为例）
    figure('Name', '定位误差分布');
    errors = zeros(length(algorithms), 1);
    for i = 1:length(algorithms)
        errors(i) = norm(est_pos{i} - target_pos);
    end
    bar(errors);
    set(gca, 'XTickLabel', algorithms);
    ylabel('定位误差 (m)');
    title('各算法定位误差对比');
    grid on;
end

三、三维场景扩展

3.1 三维节点生成与距离计算

function [anchor_pos, target_pos, true_dist] = generate_3d_node_positions(params)
    % 三维场景节点生成
    rng(0);
    anchor_pos = rand(params.num_anchors, 3) * params.area_size(1); % 三维坐标
    target_pos = params.target_pos; % 三维目标位置 [x,y,z]
    true_dist = sqrt(sum((anchor_pos - target_pos).^2, 2)); % 三维距离
end

3.2 三维LS算法

function est_pos = ls_3d_algorithm(anchor_pos, noisy_dist, params)
    % 三维最小二乘法
    num_anchors = size(anchor_pos, 1);
    A = [];
    b = [];
    for i = 1:num_anchors
        xi = anchor_pos(i, 1); yi = anchor_pos(i, 2); zi = anchor_pos(i, 3);
        di = noisy_dist(i);
        A = [A; 2*(xi - anchor_pos(1,1)), 2*(yi - anchor_pos(1,2)), 2*(zi - anchor_pos(1,3))];
        b = [b; xi^2 + yi^2 + zi^2 - di^2 - (anchor_pos(1,1)^2 + anchor_pos(1,2)^2 + anchor_pos(1,3)^2 - noisy_dist(1)^2)];
    end
    est_pos = (A'*A) \ (A'*b); % 三维坐标 [x,y,z]
end

参考代码 TOA的MATLAB定位算法 www.youwenfan.com/contentcss/79834.html

四、性能优化与扩展

4.1 非视距（NLOS）误差抑制

function noisy_dist = add_nlos_error(true_dist, params)
    % 添加NLOS误差（常数偏差或指数分布误差）
    los_prob = 0.8; % LOS概率
    nlos_bias = 5; % NLOS偏差（m）
    for i = 1:length(true_dist)
        if rand() > los_prob % NLOS场景
            noisy_dist(i) = true_dist(i) + nlos_bias + exprnd(2); % 指数分布误差
        else % LOS场景
            noisy_dist(i) = true_dist(i) + randn()*sqrt(params.c^2*params.noise_var);
        end
    end
end

4.2 蒙特卡洛仿真（统计性能）

function monte_carlo_simulation()
    % 蒙特卡洛仿真：统计不同SNR下的RMSE
    snr_range = 0:5:30; % SNR范围（dB）
    num_mc = 100; % 蒙特卡洛次数
    algorithms = {'LS', 'WLS', 'Taylor', 'Chan'};
    results = zeros(length(snr_range), length(algorithms));
    
    for snr_idx = 1:length(snr_range)
        snr_db = snr_range(snr_idx);
        noise_var = 1/(2 * 10^(snr_db/10)); % 噪声方差（假设信号功率1W）
        params.noise_var = noise_var;
        
        rmse_sum = zeros(1, length(algorithms));
        for mc = 1:num_mc
            [anchor_pos, target_pos, true_dist] = generate_node_positions(params);
            noisy_dist = add_measurement_noise(true_dist, noise_var, params.c);
            for alg_idx = 1:length(algorithms)
                method = algorithms{alg_idx};
                est_pos = toa_algorithm(anchor_pos, noisy_dist, method, params);
                rmse_sum(alg_idx) = rmse_sum(alg_idx) + norm(est_pos - target_pos);
            end
        end
        results(snr_idx, :) = rmse_sum / num_mc; % 平均RMSE
    end
    
    % 绘制SNR-RMSE曲线
    figure;
    semilogy(snr_range, results, 'LineWidth', 2);
    legend(algorithms);
    xlabel('SNR (dB)');
    ylabel('平均RMSE (m)');
    title('不同SNR下TOA定位算法性能');
    grid on;
end

五、工程实践指南

5.1 参数选择建议

参数	推荐值/说明
锚节点数量	≥3（二维）/≥4（三维），避免共线/共面
噪声方差	根据硬件精度设置（如GPS误差5m，UWB误差0.1m）
算法选择	低复杂度选LS/WLS，高精度选Taylor/Chan
迭代次数（Taylor）	10~20次（通常5次内收敛）

5.2 常见问题解决

1. 定位发散（Taylor算法）：检查初始值是否接近真实位置，可先用LS估计作为初始值。
2. NLOS误差影响：采用NLOS识别算法（如K-means聚类）或鲁棒统计方法（如M估计）。
3. 锚节点部署优化：使用凸优化或遗传算法优化锚节点位置，最小化定位误差。
4. 计算效率：Chan算法为闭式解，适合实时系统；Taylor算法需迭代，适合离线高精度场景。

六、总结

本实现提供了TOA定位的完整MATLAB解决方案，涵盖二维/三维场景、4种核心算法（LS/WLS/Taylor/Chan）、性能评估与可视化。通过调整参数（如噪声方差、锚节点数量）和扩展功能（如NLOS抑制、蒙特卡洛仿真），可满足室内定位、无线传感器网络、雷达跟踪等多种应用需求。实际应用中需根据场景特点（如噪声水平、节点密度）选择合适算法，并通过实测数据校准模型参数。