3次元空間における複数レーダーからの観測データを統合し、移動目標の軌道を高精度で推定するためには、確率的データ関連付け（PDA）と拡張カルマンフィルタ（EKF）の組み合わせが有効です。以下では、Matlab環境での実装例を解説します。

状態ベクトルと初期化

6次元状態ベクトル [位置x, 速度vx, 位置y, 速度vy, 位置z, 速度vz] を採用し、初期誤差共分散行列は位置成分に大きな不確かさを与えることで物理的制約を反映しています：

initPos = [x_start; y_start; z_start];
stateVec = [initPos; zeros(3,1)]; % 初期速度ゼロ
covariance = diag([50, 5, 50, 5, 50, 5].^2); % 位置誤差 > 速度誤差
processNoise = diag([0.05, 0.005, 0.05, 0.005, 0.05, 0.005]);

非線形観測モデルとヤコビアン

球座標系への変換関数とその微分は、EKFの更新ステップで必要不可欠です。方位角・仰角の偏微分は自動生成ツールで導出することを推奨しますが、高速化のため事前計算済み式を使用することも可能です：

function [r, theta, phi] = toSpherical(px, py, pz)
    r = norm([px, py, pz]);
    theta = atan2(py, px);
    phi = asin(pz / r);
end

確率データ関連付けの重み計算

複数の検出信号から真の観測を選別するため、各イノベーションに対する尤度を正規化して重み付けします。虚警報項を含むことで、雑音環境下でも安定した追跡を実現します：

weights = zeros(numDetections + 1, 1);
for k = 1:numDetections
    residual = predictedMeas - actualMeas(:,k);
    weights(k) = detectionProb * mvnpdf(residual, zeros(3,1), innovationCov);
end
weights(end) = clutterDensity * volumeGate * (1 - detectionProb);
weights = weights / sum(weights);

マルチセンサ融合戦略

単一フィルタの結果ではなく、複数センサの推定値を共分散行列のトレースに基づいて動的に重み付けすることで、より信頼性の高い軌道を得ます：

trace1 = trace(covariance_sensor1);
trace2 = trace(covariance_sensor2);
fusionWeight = trace2 / (trace1 + trace2);
fusedState = fusionWeight * state1 + (1 - fusionWeight) * state2;

可視化と物理制約の適用

3Dプロットで真値、観測点、単一フィルタ結果、融合結果を色分け表示。また、高度が負になる非物理的な推定を防ぐために、更新後に状態をクリップします：

if fusedState(5) < 0
    fusedState(5:6) = 0; % 高度と垂直速度をゼロ固定
end

figure;
plot3(trueTraj(:,1), trueTraj(:,2), trueTraj(:,3), 'k--', 'LineWidth', 1.8);
hold on;
scatter3(rawObs(:,1), rawObs(:,2), rawObs(:,3), 20, 'r', 'filled');
plot3(filtered1(:,1), filtered1(:,2), filtered1(:,3), 'b-', 'LineWidth', 1.2);
plot3(fusedTraj(:,1), fusedTraj(:,2), fusedTraj(:,3), 'g-', 'LineWidth', 2);
legend('真の軌道', '観測データ', '単一フィルタ', '融合結果');

パラメータ調整では、プロセスノイズを意図的に大きく設定することで、急激な機動にも追従できる柔軟なフィルタ動作を引き出せます。ただし過剰な増加は発振を招くため、バランスが重要です。