到着時間差 (TDOA) を使用したオブジェクト追跡

R2022a 以降

この例では、到着時間差 (TDOA) を使用してオブジェクトを追跡する方法を示します。この例では、TDOA 測定による位置推定の課題のほか、TDOA 手法で単一および複数のオブジェクトの追跡に使用できるアルゴリズムと手法を紹介します。

はじめに

TDOA 測位は、空間的に分離された複数の受信機への信号の到達時間の差を利用して出力オブジェクトの位置推定と追跡を行う受動的な手法です。オブジェクトからの信号出力時間 ( $t_{e}$ ) と媒体の伝播速度 ( $c$ ) が与えられれば、範囲 $r_{1}$ と $r_{2}$ にそれぞれ位置する 2 つの受信機へのオブジェクトからの信号の到着時間 (TOA) を次のように表すことができます。

$\begin{array}{l} t_{1} = t_{e} + \frac{r_{1}}{c} \\ t_{2} = t_{e} + \frac{r_{2}}{c} \end{array}$

真の信号出力時間は通常はわからないため、 $t_{1}$ と $t_{2}$ の差を使用してオブジェクトの位置に関するある程度の情報を得ることができます。具体的には、各受信機の位置が与えられた場合、時間差測定は 2 つの受信機から不明なオブジェクトまでの範囲の差に対応します。

$\sqrt{{(x_{t} - x_{1})}^{2} + {(y_{t} - y_{1})}^{2} {+ (z_{t} - z_{1})}^{2}} - \sqrt{{(x_{t} - x_{2})}^{2} + {(y_{t} - y_{2})}^{2} {+ (z_{t} - z_{2})}^{2}} = c (t_{1} - t_{2})$

ここで、 $[x_{t} y_{t} z_{t}]$ は不明なオブジェクトの位置、 $t_{1} - t_{2}$ は TDOA 測定、 $[x_{1} y_{1} z_{1}]$ と $[x_{2} y_{2} z_{2}]$ は 2 つの受信機の位置です。TDOA とオブジェクトの位置の間のこの非線形関係は、2 つの受信機を焦点とする双曲線 (2 次元座標の場合) または双曲面 (3 次元座標の場合) を表します。次の図は、さまざまな TDOA 測定 ( $c$ = 光の速度) についてのオブジェクトの双曲線を示したものです。これらの曲線は一般に "一定の TDOA 曲線" と呼ばれます。TDOA 測定の符号を使用して、オブジェクトの位置を双曲線の単一の分岐に制約できることに注目してください。

HelperTDOATrackingExampleDisplay.plotConstantTDOACurves;

Figure contains an axes object. The axes object with xlabel X (km), ylabel Y (km) contains 15 objects of type line, text. One or more of the lines displays its values using only markers

TDOA 計算

TDOA 測定をオブジェクトの信号から計算するために使用される手法は主に 2 つあります。1 つ目の手法では、上記の $t_{i}$ の定義に従って、信号の絶対的な到着時点 (到着時間または TOA) を各受信機で測定します。その後、2 つの受信機間の到着時間の差を計算することで TDOA 測定が得られます。この手法は、特定の信号属性が事前にわかっていて、受信機で信号の立ち上がりエッジを検出できる場合に適用できます。

${TDOA}_{12} = {TOA}_{2} - {TOA}_{1}$

2 つ目の手法では、各受信機で受信信号を記録して共有の処理ハブに送ります。2 つの受信機から受け取った信号間の相互相関が処理ハブで計算されます。2 つの信号間の相互相関が最大になる遅延が推定される TDOA になります。

${TDOA}_{12} = \underset{t \in [- T_{\max} T_{\max}]}{argmax} ([S_{1} ⋆ S_{2}] (t))$

ここで、 $[S_{1} ⋆ S_{2}] (t)$ は、受信機における信号間の相互相関を時間遅延 $t$ の関数として表したものです。 $T_{\max}$ は、起こり得る最大の絶対遅延であり、 $\frac{D}{c}$ として計算できます。ここで、 $D$ は受信機間の距離です。

TDOA 計算のどちらの手法でも、同期されたクロックが受信機に必要になります。実際の応用では、これを実現するために全地球測位システム (GPS) クロックを使用するのが一般的です。

TDOA 位置推定

2 つの受信機間の TDOA はオブジェクトの位置推定を双曲線または双曲面で表すため、2 つの固定受信機を使用するだけではオブジェクトの完全な状態を観測できません。2 次元の位置推定の場合、オブジェクトの状態を推定するには、空間的に分離された受信機が少なくとも 3 つ必要です。同様に、3 次元追跡の場合は、空間的に分離された受信機が少なくとも 4 つ必要です。

受信機が $N (N \geq 2)$ 個の場合、受信機のそれぞれの組み合わせを使用して到着時間差を計算すると、オブジェクトからの TDOA 測定が合計で $N (\frac{N - 1}{2})$ 個得られます。ただし、それらの測定のうち、独立した測定は $N - 1$ 個だけになります。残りの TDOA 測定は、それらの独立した測定の線形結合として定式化できます。これらの $N - 1$ 個の測定がオブジェクトから与えられる場合、通常は推定アルゴリズムを使用してオブジェクトの位置を見つけます。この例では、球面交差アルゴリズム [1] を使用して位置推定における位置と不確かさを求めます。球面交差アルゴリズムでは、 $N - 1$ 個のすべての TDOA が同じ受信機を基準に計算されると仮定します。この受信機を "基準受信機" と呼ぶことがあります。

位置推定から得られる推定位置の精度または不確かさは、ネットワークのジオメトリに依存します。TDOA 測定の誤差が小さく、結果として推定位置の誤差が大きくなる領域は、推定位置の精度が低くなります。この影響を "幾何学的精度低下率 (GDOP)" と呼びます。次の図は、3 つの受信機がある 2 次元シナリオについて、ネットワークジオメトリの GDOP のヒートマップを示したものです。ネットワークによって生成される包絡線の内側は精度が高く、受信機間の見通し線に沿った受信機のすぐ後ろで精度が最も低くなっていることがわかります。

HelperTDOATrackingExampleDisplay.plotGDOPAccuracyMap;

Figure contains an axes object. The axes object with title Geometric Dilution of Precision, xlabel X (km), ylabel Y (km) contains 2 objects of type image, line. One or more of the lines displays its values using only markers This object represents Receivers.

単一のエミッターの追跡

このセクションでは、単一のオブジェクトからの TDOA 測定値をシミュレートし、その値を使用して偽警報や検出漏れが一切ないオブジェクトの位置推定と追跡を行います。

補助関数 helperCreateSingleTargetTDOAScenario を使用して 2 次元シナリオを定義します。この関数は、一定の速度で移動する単一のオブジェクトを表すtrackingScenarioRecordingオブジェクトを返します。このオブジェクトが空間的に分離された 3 つの固定受信機で観測されます。補助関数は、受信機によって形成されるペアを表す 2 列の行列も返します。各行は TDOA 測定を形成するプラットフォームの PlatformID を表します。

% Reproducible results
rng(2021);

% Setup scenario with 3 receivers
numReceivers = 3;
[scenario, rxPairs] = helperCreateSingleTargetTDOAScenario(numReceivers);

次に、補助関数 helperSimulateTDOA を使用してオブジェクトからの TDOA 測定をシミュレートします。補助関数で TDOA の測定精度を指定できます。100 ナノ秒の測定精度を使用して、クロックおよび TDOA 推定のタイミングの不正確性を表します。出力速度は真空中の光の速度で、報告される時間の単位はナノ秒です。これらは、helperGetGlobalParameters 関数を使用して指定されます。helperSimulateTDOA 関数は、TDOA 検出をobjectDetectionオブジェクトの cell 配列として出力します。

tdoaDets = helperSimulateTDOA(scenario, rxPairs, measNoise); % Specify measurement noise variance (nanosecond^2)

各 TDOA 検出は、objectDetection を使用して次の形式に従って表されます。

次に、シナリオを実行し、オブジェクトからの TDOA 検出を生成します。補助関数 helperTDOA2Pos を使用して、ステップごとのオブジェクトの位置と位置共分散を球面交差アルゴリズム [1] を用いて推定します。trackerGNN System object™ で等速の線形カルマンフィルターを使用して構成されたグローバル最近傍 (GNN) トラッカーを使って、オブジェクトの位置推定をさらにフィルター処理します。この等速のカルマンフィルターは、オブジェクトの速度が速いことを考慮し、補助関数 helperInitHighSpeedKF を使用して構成されています。

% Define accuracy in measurements
measNoise = (100)^2; % nanoseconds^2

% Display object
display = HelperTDOATrackingExampleDisplay(XLimits=[-1e4 1e4],...
                                           YLimits=[-1e4 1e4],...
                                           LogAccuracy=true,...
                                           Title="Single Object Tracking");

% Create a GNN tracker
tracker = trackerGNN(FilterInitializationFcn=@helperInitHighSpeedKF,...
                     AssignmentThreshold=100);

while advance(scenario)
    % Elapsed time
    time = scenario.SimulationTime;

    % Simulate TDOA detections without false alarms and missed detections
    tdoaDets = helperSimulateTDOA(scenario, rxPairs, measNoise);

    % Get estimated position and position covariance as objectDetection
    % objects
    posDet = helperTDOA2Pos(tdoaDets,true);

    % Update the tracker with position detections
    tracks = tracker(posDet, time);

    % Display results
    display(scenario, rxPairs, tdoaDets, {posDet}, tracks);
end

オブジェクトのトラックをトラッカーで維持できていることがわかります。オブジェクトの推定位置は、各受信機ペアで作成される双曲線の曲線の交差付近になっています。また、下のズームしたプロットから、ステップごとの融合された推定に比べ、フィルター処理されたオブジェクトの推定の方が不確かさが少ないことがわかります。オブジェクトが正の X 軸方向に移動するにつれ、幾何学的精度低下率によって推定位置の誤差が増えていきます。等速の運動モデルをもつカルマンフィルターを使用してオブジェクトの位置推定をフィルター処理することで、トラッカーによるオブジェクトの推定が向上することに注目してください。

zoomOnTrack(display,tracks(1).TrackID);

Figure contains an axes object. The axes object with title Single Object Tracking, xlabel X (km), ylabel Y (km) contains 13 objects of type line, image, patch, text. One or more of the lines displays its values using only markers These objects represent Emitters, Receivers, Static Fused Position, Tracks, (history).

plotDetectionVsTrackAccuracy(display);

Figure contains an axes object. The axes object with title Target Position Accuracy, xlabel Time (s), ylabel Position Uncertainty (m) contains 2 objects of type line. These objects represent Fused Position, Track.

ID がわかっている複数のエミッターの追跡

前のセクションでは、単一のオブジェクトからの TDOA 測定を使用してオブジェクトの位置の測定を生成する方法を学びました。実際の状況では、シナリオが複数のオブジェクトで構成されていることがあります。

マルチオブジェクトの TDOA 追跡では、各ターゲットの TDOA の推定が主な課題となります。複数のオブジェクトが存在する場合、各受信機で複数のオブジェクトからの信号を受け取ります。TDOA 計算の 1 つ目の手法では、各受信機で複数の到着時間測定を記録します。各受信機から報告される到着時間測定間のデータの関連付けが不明なため、可能性のある TDOA の組み合わせが多くなります。TDOA 計算の 2 つ目の手法では、受信機の信号間の信号相互相関関数により、真のオブジェクトに対応するピークが複数になり、偽警報も含まれます。特定の用途では、受信機間のこのような不明なデータの関連付けを信号レベルで簡単に解決できます。たとえば、LTE 信号または 5G 信号などの無線通信信号や ADSB 信号などの航空通信信号など、信号の符号化が事前にわかっていれば、通常は到着時間とオブジェクトの固有 ID の両方を受信機で計算できます。同様に、オブジェクトの搬送周波数が分かれていれば、それぞれの周波数帯域に各オブジェクトの個別の TDOA 計算を追加できます。

このセクションでは、信号レベルのデータの関連付けが受信機で行われると仮定します。これにより、識別されたオブジェクトごとに、あいまいさのない TDOA 測定を処理ハブで形成できます。既知のエミッター識別による TDOA 測定をシミュレートするには、補助関数 helperSimulateTDOA に 4 番目の入力引数を提供します。関数は objectDetection オブジェクトの objectClassID プロパティとしてエミッターの固有 ID を出力します。このオブジェクト ID (複数の受信機ペアからの TDOA 間で共有) を使用して、各オブジェクトからの検出を個別に融合し、それぞれの位置と不確かさを取得します。その後、それらの融合された測定を使用して、それぞれのオブジェクトを GNN トラッカーで追跡します。

% Create the multiple object scenario
numReceivers = 3;
numObjects = 4;
[scenario, rxPairs] = helperCreateMultiTargetTDOAScenario(numReceivers, numObjects);

% Reset display 
release(display);
display.LogAccuracy = false;
display.Title = 'Tracking Multiple Emitters with Known IDs';

% Release tracker
release(tracker);

while advance(scenario)
    % Elapsed time
    time = scenario.SimulationTime;
   
    % Simulate classified TDOA detections
    classifiedTDOADets = helperSimulateTDOA(scenario, rxPairs, measNoise, true);
    
    % Find unique object identities
    tgtIDs = cellfun(@(x)x.ObjectClassID,classifiedTDOADets);
    uniqueIDs = unique(tgtIDs);
    
    % Estimate the position and covariance of each emitter as objectDection
    posDets = cell(numel(uniqueIDs),1);
    for i = 1:numel(uniqueIDs)
        thisEmitterTDOADets = classifiedTDOADets(tgtIDs == uniqueIDs(i));
        posDets{i} = helperTDOA2Pos(thisEmitterTDOADets, true);
    end

    % Update tracker
    tracks = tracker(posDets, time);

    % Update display
    display(scenario, rxPairs, classifiedTDOADets, posDets, tracks);
end

Figure contains an axes object. The axes object with title Tracking Multiple Emitters with Known IDs, xlabel X (km), ylabel Y (km) contains 19 objects of type line, image, patch, text. One or more of the lines displays its values using only markers These objects represent Emitters, Receivers, Static Fused Position, Tracks, (history).

TDOA 間の関連付けが既知であると仮定すると、4 つのすべてのオブジェクトでトラックをトラッカーで維持できることがわかります。データの関連付けが受信機から提供される場合、既知の単一オブジェクトの関連付けを使用して複数の TDOA 検出を生成するようにマルチオブジェクトの TDOA 推定が単純化されます。TDOA の誤検出がないため、マルチオブジェクトの追跡の問題も同様に単純化されます。

ID が不明な複数のエミッターの追跡

このセクションでは、受信機データ間のデータの関連付けが事前にわからない、つまり受信機でオブジェクトを識別できないと仮定します。このデータの関連付けがない場合、ID がわかっている複数のエミッターの追跡のセクションのプロットに示されている青色とえび茶色の双曲線の曲線の各交差が可能性のあるオブジェクトの位置になり得ます。そのため、データの関連付けが受信機から得られない場合、複数の受信機からの到着時間 (TOA) または TDOA の検出を関連付けると、"ゴースト" オブジェクトからの検出になる傾向があります。ゴーストの検出を真のオブジェクトの検出と分けるには、受信機をさらに追加してあいまいさを減らす必要があります。このセクションでは、静的フュージョンアルゴリズム [2] を使用して不明なデータの関連付けを調べ、各オブジェクトからの位置測定を推定します。複数の到着時間測定を処理ハブに送るシステムと記録された信号をハブに送るシステムに対して、この静的フュージョンアルゴリズムを使用し、それらの処理前にオブジェクト追跡用に TDOA を計算します。

到着時間 (TOA) 測定による追跡

このセクションでは、各受信機から中央の処理ハブに複数の到着時間 (TOA) 測定を送るシステムを構成します。データの関連付けのあいまいさを減らし、潜在的なゴーストオブジェクトの数を少なくするために、4 つの固定受信機を使用してオブジェクトの位置推定と追跡を行います。

オブジェクトからの到着時間測定をシミュレートするために、補助関数 helperSimulateTOA を使用して複数のオブジェクトからの到着時間測定をシミュレートします。これらの到着時間測定では、受信機が信号を受け取ったグローバル基準クロックにおける正確な時点を記録します。正確な時点をシミュレートするために、補助関数はシナリオ時間をグローバルクロック時間として使用し、シナリオのその時点にオブジェクトから信号が出力されると仮定します。オブジェクトからの正確な出力時間は受信機にはわからないため、これらの測定を使用した追跡に使用されるアルゴリズムはこの仮定に縛られないことに注意してください。

nFalse 入力を 1 として指定して、各受信機からの偽の到着時間測定を 1 つシミュレートします。さらに、Pd 入力を 0.95 として指定します。これは、各受信機で真の到着時間測定を検出する確率を定義します。

toaDets = helperSimulateTOA(scenario, receiverIDs, measNoise, nFalse, Pd); 
% receiverIDs - PlatformID of all receivers
% measNoise - Accuracy in TOA measurements (ns^2)
% nFalse - number of false alarms per receiver
% Pd - Detection probability for receivers

到着時間 (TOA) 検出の形式は次のとおりです。

各受信機からの複数の到着時間測定を融合するには、staticDetectionFuserオブジェクトを構成して静的フュージョンアルゴリズムを使用します。staticDetectionFuser オブジェクトにより、到着時間測定間の最適なデータの関連付けが決まり、可能性のあるオブジェクトからの融合された検出が提供されます。静的フュージョンアルゴリズムには、2 つのサブルーチンまたは関数が必要です。1 つ目の関数 (MeasurementFusionFcn) により、アルゴリズムは、同じオブジェクトからのものと仮定される与えられた一連の到着時間測定に基づいて、オブジェクトからの融合された測定を推定できます。2 つ目の関数 (MeasurementFcn) により、アルゴリズムは、融合された測定から想定される到着時間測定を取得できます。到着時間測定を取得するための測定関数を定義するには、融合された測定にオブジェクトからの信号出力時間の推定が含まれていなければならないことに注意してください。そのため、融合された測定を $[x_{t} y_{t} z_{t} t_{e}]$ として定義します。ここで、 $t_{e}$ はオブジェクトが信号を出力した時点です。

補助関数 helperTOA2Pos を使用して、オブジェクトの融合された測定 (位置と出力時間) を推定します。さらに、補助関数 helperMeasureTOA を使用して、融合された測定から到着時間測定を計算する測定モデルを定義します。フュージョンアルゴリズムは、融合された検出を objectDetection オブジェクトの cell 配列として出力します。cell 配列の各要素は、可能性のあるオブジェクトからの測定をその位置および出力時間として含む objectDetection オブジェクトを定義します。この静的フュージョンアルゴリズムは、UseParallel プロパティを true に指定することで高速化できます。UseParallel を true として指定するには、Parallel Computing Toolbox™ が必要です。

% Create scenario
[scenario, ~, receiverIDs] = helperCreateMultiTargetTDOAScenario(4, 4);

% Specify statistics of TOA simulation
measNoise = 1e4; % 100 ns per receiver
nFalse = 1; % 1 false alarm per receiver per step
Pd = 0.95; % Detection probability of true signal

% Release display
release(display);
display.Title = 'Tracking Using TOA Measurements with Unknown IDs';

% Release tracker 
release(tracker);
tracker.ConfirmationThreshold = [4 6];

% Use Parallel Computing Toolbox if available
useParallel = false;

% Define fuser. 
toaFuser = staticDetectionFuser(MaxNumSensors=4,...
    MeasurementFormat='custom',...
    MeasurementFcn=@helperMeasureTOA,...
    MeasurementFusionFcn=@helperTOA2Pos,...
    FalseAlarmRate=1e-8,...
    DetectionProbability=Pd,...
    UseParallel=useParallel);

while advance(scenario)
    % Current time
    time = scenario.SimulationTime;

    % Simulate TOA detections with false alarms and missed detections
    toaDets = helperSimulateTOA(scenario, receiverIDs, measNoise, Pd, nFalse);

    % Fuse TOA detections to estimate position amd emission time of
    % unidentified and unknown number of emitters. 
    [posDets, info] = toaFuser(toaDets);
    
    % Remove emission time before feeding detections to the tracker as it
    % is not tracked in this example.
    for i = 1:numel(posDets)
        posDets{i}.Measurement = posDets{i}.Measurement(1:3);
        posDets{i}.MeasurementNoise = posDets{i}.MeasurementNoise(1:3,1:3);
    end

    % Update tracker. The target emission is assumed at timestamp, time.
    % The timestamp of the detection's is therefore time plus signal
    % propagation time. The tracker timestamp must be greater than
    % detection time. Use time + 0.05 here. In real-world, this is
    % absolute timestamp at which tracker was updated. 
    tracks = tracker(posDets, time + 0.05);

    % Update display
    % Form TDOA measurements from assigned TOAs for visualization
    tdoaDets = helperFormulateTDOAs(toaDets,info.Assignments);
    display(scenario, receiverIDs, tdoaDets, posDets, tracks);
end

Figure contains an axes object. The axes object with title Tracking Using TOA Measurements with Unknown IDs, xlabel X (km), ylabel Y (km) contains 19 objects of type line, image, patch, text. One or more of the lines displays its values using only markers These objects represent Emitters, Receivers, Static Fused Position, Tracks, (history).

ほとんどの場合について、フューザーアルゴリズムでオブジェクトの位置を正確に推定できていることがわかります。ただし、偽警報と測定漏れがあるため、フュージョンアルゴリズムが一部のステップで誤ったデータの関連付けを選択している可能性があります。これにより、ゴースト交差からの検出になることがあります。トラッカーは、それらの誤った関連付けを偽警報として破棄することで静的フュージョンアルゴリズムを支援します。

到着時間差 (TDOA) 測定による追跡

このセクションでは、エミッター識別がない複数のオブジェクトからの受信機ペアから TDOA が形成されるシステムの追跡アルゴリズムを構成します。マルチオブジェクトシナリオにおける受信機ペアからの TDOA の計算では、相互相関関数の偽のピークによる偽警報がいくつか生成される可能性があります。そのようなシステムからの TDOA 測定をシミュレートするには、受信機ペアあたりの偽警報の数を 2 に増やします。

staticDetectionFuser オブジェクトを使用して、前のセクションと同じように静的フュージョンアルゴリズムが構成されます。到着時間の融合とは対照的に、ここでは測定フュージョン関数 helperFuseTDOA を使用して複数の TDOA を融合された測定に融合します。さらに、補助関数 helperMeasureTDOA を使用して、融合された測定から TDOA 測定への変換を定義します。TDOA 測定には真の信号出力時間に関する情報は含まれておらず、必要もないため、融合された測定を $[x_{t} y_{t} z_{t}]$ として定義します。TDOA のペアは 4 つの受信機からは 3 つ形成されるため、MaxNumSensors を 3 に設定します。

% Create scenario
[scenario, rxPairs] = helperCreateMultiTargetTDOAScenario(4, 4);

% Measurement statistics
measNoise = 1e4; % 100 ns per receiver
nFalse = 2; % 2 false alarms per receiver pair
Pd = 0.95; % Detection probability per receiver pair

% Release display
release(display);
display.Title = 'Tracking Using TDOA Measurements with Unknown IDs';

% Release tracker 
release(tracker);

% Define fuser. 
tdoaFuser = staticDetectionFuser(MaxNumSensors=3,...
    MeasurementFormat='custom',...
    MeasurementFcn=@helperMeasureTDOA,...
    MeasurementFusionFcn=@helperTDOA2Pos,...
    DetectionProbability=Pd,...
    FalseAlarmRate=1e-8,...
    UseParallel=useParallel);

while advance(scenario)
    % Current elapsed time
    time = scenario.SimulationTime;

    % Simulate TDOA detections with false alarms and missed detections
    tdoaDets = helperSimulateTDOA(scenario, rxPairs, measNoise, false, Pd, nFalse);

    % Fuse TDOA detections to esimate position detections of unidentified
    % and unknown number of emitters
    posDets = tdoaFuser(tdoaDets);
    
    % Update tracker with position detections
    if ~isempty(posDets)
        tracks = tracker(posDets, time);
    end

    % Update display
    display(scenario, receiverIDs, tdoaDets, posDets, tracks);
end

Figure contains an axes object. The axes object with title Tracking Using TDOA Measurements with Unknown IDs, xlabel X (km), ylabel Y (km) contains 21 objects of type line, image, patch, text. One or more of the lines displays its values using only markers These objects represent Emitters, Receivers, Static Fused Position, Tracks, (history).

この例の定義されている測定の統計では、このネットワークジオメトリに基づくすべてのオブジェクトのトラックがトラッカーで維持されています。データの関連付けが不明な TOA と TDOA のどちらのシステムでも、静的フュージョンアルゴリズムのデータの関連付けの正確性には、問題のジオメトリ、測定精度、および偽警報の数がいずれも大きく影響します。この例で使用しているシナリオでは、真のオブジェクトのトラックを維持するための十分な時点における真の検出を静的フュージョンアルゴリズムで報告できています。

まとめ

この例では、単一のオブジェクトおよび複数のオブジェクトを TDOA 測定を使用して追跡する方法を学びました。受信機からのエミッター識別がないマルチオブジェクトの追跡に関連する課題を学習し、静的フュージョンアルゴリズムを使用して測定レベルでデータの関連付けを計算しました。

参考文献

[1] Smith, Julius, and Jonathan Abel. "Closed-form least-squares source location estimation from range-difference measurements." IEEE Transactions on Acoustics, Speech, and Signal Processing 35.12 (1987): 1661-1669.

[2] Sathyan, T., A. Sinha, and T. Kirubarajan. "Passive geolocation and tracking of an unknown number of emitters." IEEE Transactions on Aerospace and Electronic Systems 42.2 (2006): 740-750.

サポート関数

このセクションでは、この例で使用されるいくつかのサポート関数を定義します。補助関数の完全な一覧については、現在の作業ディレクトリで確認できます。

helperTDOA2Pos

function varargout = helperTDOA2Pos(tdoaDets, reportDetection)
% This function uses the spherical intersection algorithm to find the
% object position from the TDOA detections assumed to be from the same
% object.
%
% This function assumes that all TDOAs are measured with respect to the
% same reference sensor. 
%
% [pos, posCov] = helperTDOA2Pos(tdoaDets) returns the estimated position
% and position uncertainty covariance.
%
% posDetection = helperTDOA2Pos(tdoaDets, true) returns the estimate
% position and uncertainty covariance as an objectDetection object. 

if nargin < 2
    reportDetection = false;
end

% Collect scaling information
params = helperGetGlobalParameters;
emissionSpeed = params.EmissionSpeed;
timeScale = params.TimeScale;

% Location of the reference receiver
referenceLoc = tdoaDets{1}.MeasurementParameters(2).OriginPosition(:);

% Formulate the problem. See [1] for more details
d = zeros(numel(tdoaDets),1);
delta = zeros(numel(tdoaDets),1);
S = zeros(numel(tdoaDets),3);
for i = 1:numel(tdoaDets)
    receiverLoc = tdoaDets{i}.MeasurementParameters(1).OriginPosition(:);
    d(i) = tdoaDets{i}.Measurement*emissionSpeed/timeScale;
    delta(i) = norm(receiverLoc - referenceLoc)^2 - d(i)^2;
    S(i,:) = receiverLoc - referenceLoc;
end

% Pseudo-inverse of S
Swstar = pinv(S);

% Assemble the quadratic range equation
STS = (Swstar'*Swstar);
a = 4 - 4*d'*STS*d;
b = 4*d'*STS*delta;
c = -delta'*STS*delta;

Rs = zeros(2,1);
% Imaginary solution, return a location outside coverage
if b^2 < 4*a*c 
    varargout{1} = 1e10*ones(3,1);
    varargout{2} = 1e10*eye(3);
    return;
end

% Two range values
Rs(1) = (-b + sqrt(b^2 - 4*a*c))/(2*a);
Rs(2) = (-b - sqrt(b^2 - 4*a*c))/(2*a);

% If one is negative, use the positive solution
if prod(Rs) < 0
    Rs = Rs(Rs > 0);
    pos = 1/2*Swstar*(delta - 2*Rs(1)*d) + referenceLoc;
else % Use range which minimize the error
    xs1 = 1/2*Swstar*(delta - 2*Rs(1)*d);
    xs2 = 1/2*Swstar*(delta - 2*Rs(2)*d);
    e1 = norm(delta - 2*Rs(1)*d - 2*S*xs1);
    e2 = norm(delta - 2*Rs(2)*d - 2*S*xs2);
    if e1  > e2
        pos = xs2 + referenceLoc;
    else
        pos = xs1 + referenceLoc;
    end
end

% If required, compute the uncertainty in the position
if nargout > 1 || reportDetection
    posCov = helperCalcPositionCovariance(pos,tdoaDets,timeScale,emissionSpeed);
end

if reportDetection
    varargout{1} = objectDetection(tdoaDets{1}.Time,pos,'MeasurementNoise',posCov);
else
    varargout{1} = pos;
    if nargout > 1
        varargout{2} = posCov;
    end
end

end

function measCov = helperCalcPositionCovariance(pos,thisDetections,timeScale,emissionSpeed)
n = numel(thisDetections);
% Complete Jacobian from position to N TDOAs
H = zeros(n,3);
% Covariance of all TDOAs
S = zeros(n,n);
for i = 1:n
   e1 = pos - thisDetections{i}.MeasurementParameters(1).OriginPosition(:);
   e2 = pos - thisDetections{i}.MeasurementParameters(2).OriginPosition(:);
   Htdoar1 = (e1'/norm(e1))*timeScale/emissionSpeed;
   Htdoar2 = (e2'/norm(e2))*timeScale/emissionSpeed;
   H(i,:) = Htdoar1 - Htdoar2;
   S(i,i) = thisDetections{i}.MeasurementNoise;
end
Pinv = H'/S*H;
% Z is not measured, use 1 as the covariance
if Pinv(3,3) < eps
    Pinv(3,3) = 1;
end

% Insufficient information in TDOA
if rank(Pinv) >= 3
    measCov = eye(3)/Pinv;
else
    measCov = inf(3);
end

% Replace inf with large number
measCov(~isfinite(measCov)) = 100;

% Return a true symmetric, positive definite matrix for covariance.
measCov = (measCov + measCov')/2;
end

helperInitHighSpeedKF

function filter = helperInitHighSpeedKF(detection)
% This function initializes a constant velocity Kalman filter and sets a
% higher initial state covariance on velocity components to account for
% high speed of the object vehicles. 
filter = initcvkf(detection);
filter.StateCovariance(2:2:end,2:2:end) = 500*eye(3);
end

helperTOA2Pos

function [posTime, posTimeCov] = helperTOA2Pos(toaDetections)
% This function computes the position and emission time of a target given
% its time-of-arrival detections from multiple receivers. 
%

% Convert TOAs to TDOAs for using spherical intersection
[tdoaDetections, isValid] = helperTOA2TDOADetections(toaDetections);

% At least 3 TOAs (2 TDOAs) required. Some TOA pairings can lead to an
% invalid TDOA (> maximum TDOA). In those situations, discard the tuple by
% using an arbitrary position with a large covariance.
if numel(tdoaDetections) < 2 || any(~isValid)
    posTime = 1e10*ones(4,1);
    posTimeCov = 1e10*eye(4);
else
    % Get position estimate using TDOA fusion. 
    % Get time estimate using calculated position.
    if nargout > 1 % Only calculate covariance when requested to save time
        [pos, posCov] = helperTDOA2Pos(tdoaDetections);
        [time, timeCov] = helperCalcEmissionTime(toaDetections, pos, posCov);
        posTime = [pos;time];
        posTimeCov = blkdiag(posCov,timeCov);
    else
        pos = helperTDOA2Pos(tdoaDetections);
        time = helperCalcEmissionTime(toaDetections, pos);
        posTime = [pos;time];
    end
end
end

helperCalcEmissionTime

function [time, timeCov] = helperCalcEmissionTime(toaDetections, pos, posCov)
% This function calculates the emission time of an object given its
% position and obtained TOA detections. It also computes the uncertainty in
% the estimate time. 
globalParams = helperGetGlobalParameters;
emissionSpeed = globalParams.EmissionSpeed;
timeScale = globalParams.TimeScale;
n = numel(toaDetections);
emissionTime = zeros(n,1);
emissionTimeCov = zeros(n,1);
for i = 1:numel(toaDetections)
    % Calculate range from this receiver
    p0 = toaDetections{i}.MeasurementParameters.OriginPosition(:);
    r = norm(pos - p0);
    emissionTime(i) = toaDetections{i}.Measurement - r/emissionSpeed*timeScale;
    if nargout > 1
        rJac = (pos - p0)'/r;
        rCov = rJac*posCov*rJac';
        emissionTimeCov(i) = rCov./emissionSpeed^2*timeScale^2;
    end
end
% Gaussian merge each time and covariance estimate
time = mean(emissionTime);
if nargout > 1
    e = emissionTime - time;
    timeCov = mean(emissionTimeCov) + mean(e.^2);
end
end

helperMeasureTOA

function toa = helperMeasureTOA(posTime, params)
% This function calculates the expected TOA at a receiver given an object
% position and emission time and the receiver's measurement parameters
% (OriginPosition)
globalParams = helperGetGlobalParameters;
emissionSpeed = globalParams.EmissionSpeed;
timeScale = globalParams.TimeScale;
r = norm(posTime(1:3) - params.OriginPosition);
toa = posTime(4) + r/emissionSpeed*timeScale;
end

helperMeasureTDOA

function toa = helperMeasureTDOA(pos, params)
% This function calculates the expected TDOA measurement given object
% position and measurement parameters of a TDOA detection
globalParams = helperGetGlobalParameters;
emissionSpeed = globalParams.EmissionSpeed;
timeScale = globalParams.TimeScale;
r1 = norm(pos(1:3) - params(1).OriginPosition);
r2 = norm(pos(1:3) - params(2).OriginPosition);
toa = (r1 - r2)/emissionSpeed*timeScale;
end

参考

トピック

Vehicle Tracking for Urban Air Mobility Using 5G Position Reference Signal