% TDOA算法MATLAB實現：二維定位
% 日期：2025年10月

clear; clc; close all;

%% ==================== 1. 參數設置 ====================
% 傳感器座標 (x,y)，單位：米
S = [0, 0;       % S1
     100, 0;     % S2
     0, 100;     % S3
     100, 100];  % S4 (冗餘傳感器，提高精度)
N = size(S, 1);  % 傳感器數量

% 信號源真實位置 (x,y)，單位：米
P_true = [50; 30];  

% 傳播速度 (聲速，m/s)
c = 340;  

% 噪聲參數 (高斯白噪聲標準差，s)
sigma = 0.001;  

%% ==================== 2. 計算理論時差 ====================
% 計算信號源到各傳感器的真實距離
d_true = vecnorm(S' - P_true, 2, 1);  % d_true(i)：信號源到Si的距離

% 以S1為參考，計算理論時差 τi1 = (di - d1)/c (i=2,3,4)
tau_true = (d_true(2:end) - d_true(1)) / c;  

%% ==================== 3. 模擬帶噪聲的測量時差 ====================
tau_meas = tau_true + sigma * randn(size(tau_true));  % 加入噪聲

%% ==================== 4. TDOA定位算法 (最小二乘法) ====================
% 輸入：傳感器座標S，測量時差tau_meas (相對於S1)，傳播速度c
% 輸出：估計的信號源位置P_est (x,y)
P_est = tdoa_localization(S, tau_meas, c);

%% ==================== 5. 結果評估與可視化 ====================
% 計算定位誤差
error = norm(P_est - P_true);
fprintf('真實位置: (%.2f, %.2f) m\n', P_true(1), P_true(2));
fprintf('估計位置: (%.2f, %.2f) m\n', P_est(1), P_est(2));
fprintf('定位誤差: %.4f m\n', error);

% 可視化
figure;
plot(S(:,1), S(:,2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); hold on;
plot(P_true(1), P_true(2), 'gp', 'MarkerSize', 12, 'MarkerFaceColor', 'g');
plot(P_est(1), P_est(2), 'bx', 'MarkerSize', 12, 'LineWidth', 2);
legend('傳感器', '真實位置', '估計位置');
xlabel('X (m)'); ylabel('Y (m)');
title('TDOA定位結果');
grid on; axis equal;


%% ==================== TDOA定位核心函數 (最小二乘法) ====================
function P_est = tdoa_localization(S, tau_meas, c)
    % S: 傳感器座標矩陣 (N×2)，N≥3
    % tau_meas: 測量時差向量 (N-1)×1，相對於第1個傳感器
    % c: 信號傳播速度 (m/s)
    % P_est: 估計的信號源位置 (2×1)
    
    N = size(S, 1);  % 傳感器數量
    if N < 3
        error('至少需要3個傳感器進行二維定位！');
    end
    
    % 提取參考傳感器S1和其他傳感器Si (i=2..N)
    S1 = S(1,:)';       % 參考傳感器座標 (2×1)
    Si = S(2:end,:)';   % 其他傳感器座標 (2×(N-1))
    tau = tau_meas;     % 時差向量 ((N-1)×1)
    
    % 構造線性方程組 Ax = b (偽線性化)
    A = [];
    b = [];
    for i = 1:size(Si, 2)
        xi = Si(1,i); yi = Si(2,i);
        Ai = [xi-2*S1(1), yi-2*S1(2), c^2*tau(i)^2 - (xi^2 + yi^2) + (S1(1)^2 + S1(2)^2)];
        bi = c^2 * tau(i)^2;
        A = [A; Ai];
        b = [b; bi];
    end
    
    % 最小二乘求解：x = (A^T A)^-1 A^T b
    x = pinv(A) * b;  % x = [x; y; d1^2]，其中d1是信號源到S1的距離
    
    % 提取估計位置 (x,y)
    P_est = x(1:2);
end

function P_est = tdoa_chan(S, tau_meas, c, sigma)
    % Chan算法：TDOA高精度定位 (二維)
    % 輸入：傳感器座標S(N×2)，時差tau_meas((N-1)×1)，傳播速度c，噪聲標準差sigma
    % 輸出：估計位置P_est(2×1)
    
    N = size(S, 1);
    if N < 3, error('至少需要3個傳感器！'); end
    
    % 步驟1：計算距離差向量和傳感器間距離
    d1 = vecnorm(S(2:end,:) - S(1,:), 2, 2);  % 參考傳感器S1到其他傳感器的距離
    r = c * tau_meas;  % 距離差向量 (d_i - d_1)
    
    % 步驟2：構造矩陣G和h
    G = [-d1, ones(N-1,1)];
    h = 0.5 * (r.^2 - d1.^2);
    
    % 步驟3：第一次最小二乘求解中間變量
    z = pinv(G) * h;  % z = [x; y; d1^2] (簡化形式)
    d1_est = sqrt(z(3));  % 估計d1
    
    % 步驟4：構造協方差矩陣和加權矩陣
    Q = (sigma^2) * eye(N-1);  % 噪聲協方差
    W = inv(G' * inv(Q) * G);   % 加權矩陣
    
    % 步驟5：第二次最小二乘求解最終位置
    A = [];
    b = [];
    for i = 1:N-1
        xi = S(i+1,1); yi = S(i+1,2);
        Ai = [(xi-S(1,1))/d1_est, (yi-S(1,2))/d1_est, 1];
        bi = 0.5 * ((xi^2 + yi^2 - S(1,1)^2 - S(1,2)^2)/d1_est - r(i)^2/d1_est + d1_est^2);
        A = [A; Ai];
        b = [b; bi];
    end
    P_est = pinv(A' * W * A) * A' * W * b;
end