JP7433560B1 - 目標軌跡推定装置及び目標軌跡推定方法 - Google Patents

Abstract

目標軌跡推定装置は、物体目標（１００）からの到来電波を受信した複数の方位センサ（１０）それぞれにより得られる方位測定値を示す方位情報、及び当該方位情報が示す方位測定値に付随する、あるいは、一様に設定される方位測定誤差を示す方位測定誤差情報を読み込む方位情報読込部（４１）と、複数の方位センサ（１０）それぞれが物体目標（１００）を観測した時間を時系列順に複数の時間領域に分割し、方位情報読込部（４１）が読み込む方位情報を分割された時間領域毎に分割する時間領域分割部（４３）と、時間領域分割部（４３）により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値が示す位置が一点に集中しているか、あるいは、位置と速度からなる状態ベクトルの周辺に集中するかを基に、当該方位測定値が異常値を示すか否かを判定し、異常値と判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再設定し、異常値でないと判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度をそのままに再設定する異常値判定部（４４）と、時間領域分割部（４３）により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値と当該方位測定値に付随する再設定された方位測定誤差を用いて物体目標（１００）に対する各時間領域における測位値を推定する測位処理部（４５）と、測位処理部（４５）により推定された時間領域毎の測位値を時系列につなげて物体目標（１００）の軌跡を示す物体軌跡情報を出力する出力部（４６）と、を備える。

Description

本開示は、目標の測位値を測定する目標軌跡推定装置及び目標軌跡推定方法に関する。

複数のレーダセンサなどのアクティブセンサ又は電波センサなどのパッシブセンサによる方位測定値に基づき、飛行機などの目標の軌跡を推定する目標軌跡推定装置では、複数の方位測定値それぞれの信頼度に基づいて方位測定誤差を設定することで、重みを付けた測位値を計算する技術が存在する。
例えば、複数の非同期センサの異なる方位測定誤差に基づいて、ＭＡＰ（Maximum A Posteriori）推定で方位測定時刻の違いを反映しながら目標の位置と速度、及び、その精度を推定する方法がある。

また、目標軌跡推定装置において、目標を測位する測位処理時の異常値判定を行う方法が、例えば、特許文献１に示されている。
特許文献１に示された目標軌跡推定装置は、複数のセンサから選択されるセンサ組毎の測位精度を表す指標であるＤＯＰ（Dilution Of Precision）を計算して、優先的に測位処理に使用するセンサを設定している。

特開２０１６－６１７０５号公報

特許文献１に示された目標追尾装置は、複数のセンサが目標に対して同じ方位を示すような位置に存在している場合、ＤＯＰは測位精度の劣化とともに低下する性質を持つため、測位処理時の良好な異常値判定を行うことができる。
ところで、センサ組における特定のセンサがマルチパス環境下にあると、異常値を多く含む場合があり、誤った測位処理を行ってしまう恐れがある。

本開示は上記した点に鑑みてなされたものであり、特定のセンサがマルチパス環境下にあったとしても、マルチパス環境下での目標の測位値を精度高く推定でき、物体の軌跡推定が向上した目標軌跡推定装置を得ることを目的とする。

本開示に係る目標軌跡推定装置は、目標からの到来電波を受信した複数の方位センサそれぞれにより得られる方位測定値を示す方位情報、及び当該方位情報が示す方位測定値に付随する方位測定誤差情報を読み込む方位情報読込部と、複数の方位センサそれぞれが目標を観測した時間を時系列順に複数の時間領域に分割し、方位情報読込部が読み込む方位情報を分割された時間領域毎に分割する時間領域分割部と、時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値が異常値を示すか否かを判定し、異常値と判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再設定し、異常値でないと判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度をそのままに再設定する異常値判定部と、時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値と当該方位測定値に付随する再設定された方位測定誤差を用いて目標に対する各時間領域における測位値を推定する測位処理部と、測位処理部により推定された時間領域毎の測位値を時系列につなげて目標の軌跡を示す物体軌跡情報を出力する出力部と、を備える。

本開示によれば、特定の方位センサが例えマルチパス環境下にあったとしても、目標の測位値を精度高く推定でき、目標の実際の軌跡（真値）により近い軌跡推定を行うことができる。

実施の形態１に係る目標軌跡推定装置を含む目標軌跡推定システムの概略構成を示すブロック図である。 実施の形態１に係る目標軌跡推定装置において、時間領域で分割された複数の方位センサの方位測定値を示す図である。 実施の形態１に係る目標軌跡推定装置において、複数の方位センサの方位測定値を空間的に示す図である。 実施の形態１に係る目標軌跡推定装置において、測位ゲート判定の対象である方位測定値内の正常値と異常値を示す図である。 実施の形態１に係る目標軌跡推定装置において、測位ゲート判定の異常値判定方法と使用するゲートを示す図である。 実施の形態１に係る目標軌跡推定装置において、異常値判定の手順の一例を示すフローチャートである。 実施の形態１に係る目標軌跡推定装置のハードウェア構成例の概略を示す機能ブロック図である。 実施の形態２に係る目標軌跡推定装置の概略構成を示すブロック図である。 実施の形態２に係る目標軌跡推定装置において、時間順方向のカルマンフィルタを用いた異常値判定から追尾処理の手順の一例を示すフローチャートである。 実施の形態２における目標軌跡推定装置において、時間逆方向のカルマンフィルタを用いた異常値判定から追尾処理の手順の一例を示すフローチャートである。

実施の形態１．
実施の形態１に係る目標軌跡推定装置を図１から図７を用いて説明する。
図１は、実施の形態１に係る目標軌跡推定装置を含む目標軌跡推定システムの概略構成を示すブロック図である。
目標軌跡推定システムは、図１に示すように、複数の方位センサ１０_１～１０_Ｎと複数の方位測定部２０_１～２０_Ｎと記憶装置３０と目標軌跡推定装置４０と出力装置５０を備える。

実施の形態１に係る目標軌跡推定装置４０は、複数の方位センサ１０_１～１０_Ｎの方位測定値のみを使用し、例えば、距離データを取得せずに位置を測定し、飛行機などの目標の軌跡を推定する。
方位センサ１０及び方位測定部２０に付した添え字Ｎについて、個別に説明する必要がない場合は、説明の煩雑さを避けるため、添え字を付さずに、以下説明する。

複数（Ｎ個）の方位センサ１０は連接して配設され、飛行機などの目標１００からの信号を受信し、その方位情報を得る。
各方位センサ１０は、目標１００から送信される電波を受信する電波センサなどのパッシブセンサである。
複数の方位センサ１０はそれぞれ同じ構成である。

なお、各方位センサ１０は信号を出力して目標１００に当たり反射した信号を受信するレーダセンサなどのアクティブセンサでもよい。
また、各方位センサ１０は、目標１００から発せられる音波又は熱等の物理量を感知、受信し、目標１００の方向を推定できるセンサであってもよい。
各方位センサ１０の測定誤差は各方位センサ１０それぞれによって異なっていてもよく、また、測定時刻によって異なっていてもよい。

複数（Ｎ個）の方位測定部２０それぞれは対応の方位センサ１０が受信した受信信号をもとに方位を測定し、測定結果を、方位測定値を示す方位情報として出力する。
複数の方位測定部２０それぞれは、連接しているＮ個の方位センサ１０それぞれの対応した受信信号を解析して、目標１００からの到来電波の到来方位（θ_ｋ、φ_ｋ）を方位測定値として測定する。
また、複数の方位測定部２０それぞれは、方位測定値に付随する情報としてその測定時刻ｔ_ｋと、対応する方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を方位測定値（θ_ｋ、φ_ｋ）に紐づけして方位測定値を示す方位情報として出力する。方位情報はデジタル情報である。

なお、ｋは方位測定値番号であり、測定時刻ｔ_ｋを特定するための１～Ｋ（＞１）の整数である。
方位測定値番号ｋは複数（Ｎ個）の方位センサ１０が目標１００を観測する間の非同期の方位測定値全てに対して時系列順に付した番号であり、Ｋ個あるとする。
また、位置情報において、ｘ、ｙ、ｚはｘ座標、ｙ座標、ｚ座標を示し、ｒは方位センサ１０を特定するための１～Ｎの整数である。

複数の方位測定部２０それぞれは、対応する方位センサ１０のアンテナ構成及びＳＮＲ（信号対雑音比：signal-to-noise ratio）から分析される非マルチパス環境の方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を方位測定誤差情報として出力する。方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）は対応する方位測定値（θ_ｋ、φ_ｋ）に付随する。方位測定誤差情報はデジタル情報である。

すなわち、複数の方位センサ１０それぞれが目標１００を観測する間、複数の方位センサ１０それぞれが時系列順に目標１００からの到来電波をＫ個受信し、複数の方位測定部２０が目標１００に対するＫ個の方位測定値（θ_ｋ、φ_ｋ）及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を得る。

記憶装置３０は複数（Ｎ個）の方位測定部２０それぞれからの方位情報を記憶部３１に記憶する。
記憶装置３０は、複数の方位センサ１０それぞれが目標１００を観測する間に、時系列順に送られてくる複数の方位測定部２０それぞれからの、方位測定値（θ_ｋ、φ_ｋ）を示す方位情報及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報を記憶部３１０に蓄積し、目標軌跡推定時に記憶された方位情報及び方位測定誤差情報を出力する。

目標軌跡推定装置４０は、複数の方位センサ１０が受信した方位情報による方位測定値（θ_ｋ、φ_ｋ）及び再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）と代表時刻ｔ_ｍに基づき、飛行機などの目標１００の軌跡を推定する。
目標軌跡推定装置４０は、特に、複数の方位センサ１０の内の一部の方位センサ１０が目標１００の方位を示さない環境、いわゆるマルチパス環境下であっても、目標１００の軌跡を高精度に推定できる確率が高い装置である。

目標軌跡推定装置４０は、記憶装置３０に蓄積された複数の方位測定部２０からのＫ個の方位情報が示す方位測定値（θ_ｋ、φ_ｋ）及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）と測定時刻ｔ_ｋを取得し、複数の方位センサ１０それぞれが目標１００を観測した時間を時系列順に分割した複数の時間領域ｍ毎に代表時刻ｔ_ｍの測位値ｘ_ｍを算出し、時系列順の代表時刻ｔ_ｍの測位値ｘ_ｍをつなげた目標１００の軌跡を推定し、物体軌跡情報として出力する。

目標軌跡推定装置４０は、複数の方位センサ１０それぞれが目標１００を観測する間に、一定の時間領域の方位測定値を基に軌跡を推定するリカーシブ型の処理を行ない、物体軌跡情報を得る。
出力装置５０は目標軌跡推定装置４０からの物体軌跡情報から得た目標１００の推定軌跡を表示する。

目標軌跡推定装置４０は、方位情報読込部４１と誤差設定部４２と時間領域分割部４３と異常値判定部４４と測位処理部４５と出力部４６とを備える。
方位情報読込部４１は記憶装置３０に記憶された複数の方位測定部２０からの方位測定値（θ_ｋ、φ_ｋ）を示す方位情報、及び記憶装置３０に記憶された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報（σ_θ，ｋ、σ_φ，ｋ）を読み込む。
方位情報読込部４１は、併せて記憶装置３０に記憶された方位測定値（θ_ｋ、φ_ｋ）に付随する方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）と測定時刻ｔ_ｋを読み込む。

方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報を、複数の方位測定部２０それぞれが得ず、記憶装置３０に記憶されていない場合は、誤差設定部４２が、方位情報読込部４１に読み込まれた複数の方位測定部２０からの方位情報が示す方位測定値に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を一様に設定する。
誤差設定部４２により設定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）が方位情報読込部４１に読み込まれる。
複数の方位測定部２０それぞれが方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報を出力する場合は、複数の方位測定部２０それぞれが誤差設定部４２としての機能を果たす。

方位情報読込部４１は、複数の方位センサ１０それぞれが目標１００を観測した時間の、記憶装置３０に蓄積された複数の方位測定部２０からの方位測定値（θ_ｋ、φ_ｋ）を示す方位情報と、方位測定値（θ_ｋ、φ_ｋ）に付随し紐づけされた測定時刻ｔ_ｋ及び各方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を示す方位情報を読み込む。
この時、複数の方位センサ１０それぞれが目標１００を観測した時間の、記憶装置３０に蓄積された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報、あるいは、誤差設定部４２により一様に設定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）も合わせて読み込む。

時間領域分割部４３は方位情報読込部４１に読み込まれた複数の方位測定部２０からの方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を設定した時間領域ごとに分割する。設定した時間領域は短時間の時間領域である。
時間領域分割部４３は複数の方位センサ１０それぞれが目標１００を観測した時間を時系列順に複数の時間領域に分割し、方位情報読込部４１が読み込む方位情報を分割した時間領域毎に分割する。

時間領域分割部４３は、設定した一定の時間間隔によって、測定時刻ｔ_ｋをもとに複数の方位センサ１０が受信した目標１００からの電波に基づき方位情報読込部４１に読み込まれ、記憶装置３０に蓄積された方位測定値（θ_ｋ、φ_ｋ）と方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋ及び各方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を示す方位情報をＭ個に分割する。

Ｍ個に分割した各時間領域をｍ、各時間領域ｍの代表時刻をｔ_ｍとする。
また、時間領域ｍにおいて、各方位センサ１０が受信した目標１００からの電波に対する方位情報及び方位測定誤差情報に時系列に付す方位測定値番号ｋのうち、最初の方位測定値番号をｋ_ｍ，ｓ、最後の方位測定値番号をｋ_ｍ，ｅとする。

例えば、方位センサ１０_１が目標１００を観測する間に測定したＫ個の方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を時系列順に等間隔の時間間隔によりＭ個の方位情報に分割し、ｍ番目に分割された方位情報の内の最初の方位情報に対してｋ_ｍ，ｓの方位測定値番号を付し、最後の方位情報に対してｋ_ｍ，ｅの方位測定値番号を付すことを意味している。
方位センサ１０_１の位置情報（ｘ^１ _ｋ、ｙ^１ _ｋ、ｚ^１ _ｋ）及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報は、方位測定値（θ_ｋ、φ_ｋ）が時系列順に等間隔にＭ個に分割されたとしても、対応する方位測定値（θ_ｋ、φ_ｋ）に付随する。

今、方位センサ１０が４個の場合について、時間領域分割部４３によって方位測定値を示す方位情報を時系列順に時間間隔Ｗ１によって等間隔にＭ個の時間領域ｍに分割する一例を、図２を用いて説明する。
図２において、θ_ｋ（ｓ１）は第１の方位センサ１０_１が受信した電波による方位情報の方位測定値θ_ｋ、θ_ｋ（ｓ４）は第４の方位センサ１０_４が受信した電波による方位情報の方位測定値θ_ｋを示し、１～Ｍは１番目の時間領域からＭ番目の時間領域を示している。

また、代表的に、ｋ_１，ｓは時間領域１における第１の方位センサ１０_１が受信した電波による最初の方位情報の方位測定値θ_ｋ、ｋ_１，ｅは時間領域１における第４の方位センサ１０_４が受信した電波による最後の方位情報の方位測定値θ_ｋ、ｋ_Ｍ，ｓは時間領域Ｍにおける第４の方位センサ１０_４による最初の方位情報の方位測定値θ_ｋ、ｋ_Ｍ，ｅは時間領域Ｍにおける第４の方位センサ１０_４が受信した電波による最後の方位情報の方位測定値θ_ｋを示している。

このように、時間領域分割部４３は各方位センサ１０が受信した電波による方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を時系列順に時間間隔Ｗ１によって等間隔にＭ個の時間領域ｍに分割し、方位センサ１０毎に、しかもＫ個の方位測定値（θ_ｋ、φ_ｋ）を時間領域ｍ毎に時系列に分割する。
なお、１つの方位センサ１０に対する１つの時間領域ｍに、図２に示すように、複数の方位測定値（θ_ｋ、φ_ｋ）が存在していてもよく、また、方位測定値（θ_ｋ、φ_ｋ）が存在していない、もしくは１つの方位測定値（θ_ｋ、φ_ｋ）が存在する時間領域ｍがあってもよい。

異常値判定部４４は、時間領域分割部４３が設定した時間領域ｍ毎に分割した方位測定値（θ_ｋ、φ_ｋ）を、設定した時間領域ｍ毎に存在する方位測定値（θ_ｋ、φ_ｋ）の異常を判定し、判定した異常の度合いに基づいた信頼度を設定し、信頼度情報を得る。
異常値判定部４４は、時間領域分割部４３が設定した時間領域ｍ毎に存在する方位測定値（θ_ｋ、φ_ｋ）が異常値を示すか否かの判定を実施し、異常値であると判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を設定し直し、測位値を算出する測位処理において、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げ、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる処理を行なう。

一方、異常値判定部４４は、方位測定値（θ_ｋ、φ_ｋ）が異常値でないと判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）はそのままとし、測位値を算出する測位処理において、異常値でないと判定した方位測定値（θ_ｋ、φ_ｋ）の信頼度をそのままとし、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度をそのままとする。
なお、測位値を算出する測位処理は一般に知られている処理でよい。

異常値判定部４４は、時間領域分割部４３が分割した時間領域ｍ毎に、時間領域ｍに存在する方位測定値（θ_ｋ、φ_ｋ）が目標１００の方位を示すか否かを判定し、方位測定値（θ_ｋ、φ_ｋ）が目標１００の方位を示さないと、方位測定値（θ_ｋ、φ_ｋ）を異常値と判定し、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を設定し直し、結果として、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる処理を行う。
異常値判定部４４は、方位測定値（θ_ｋ、φ_ｋ）が異常値でないと判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）はそのままとし、異常値でないと判定した方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度はそのままとする。

要するに、異常値判定部４４は、時間領域分割部４３が分割した時間領域ｍ毎に、時間領域ｍに存在する方位測定値（θ_ｋ、φ_ｋ）が異常値を示すか否かを判定し、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再設定し、異常値でないと判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度をそのままに再設定する。

測位処理部４５は、時間領域分割部４３が分割した時間領域ｍ毎に、方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する異常値判定部４４により再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を用いて測位値を算出する測位処理を行い、時間領域ｍにおける目標１００に対する測位値を得、時間領域ｍにおける目標１００の測位値を示す目標位置情報を得る。

測位処理部４５は、時間領域分割部４３が分割した時間領域ｍ毎に、時間領域ｍに存在する最初の方位測定値番号を示すｋ_ｍ，ｓから最後の方位測定値番号を示すｋ_ｍ，ｅまでの方位測定値番号ｋで定められる複数の方位測定値（θ_ｋ、φ_ｋ）、及び複数の方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）、測定時刻ｔ_ｋ、方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を用いて測位値ｘ_ｍを算出する測位処理を行い、時間領域ｍを代表する代表時刻ｔ_ｍにおける状態としての測位値ｘ_ｍを得る。
なお、測位値ｘ_ｍを算出する測位処理は一般に知られている処理でよい。

今、Ｎ個の方位センサ１０から目標１００の測位値ｘ_Ｍを得る方位センサ１０として４個を用いた場合について、時間領域Ｍでの測位値ｘ_Ｍを得る概念を、図３を用いて説明する。方位センサ１０の数は４個に限られるものではない。
図３において、Ｓ１～Ｓ４は第１の方位センサ１０_１から第４の方位センサ１０_４を示し、第１の方位センサＳ１から第４の方位センサＳ４それぞれから伸びている線分ｄ１から線分ｄ４が時間領域Ｍに存在する第１の方位センサＳ１から第４の方位センサ１０_４それぞれの方位測定値θ_ｋを示す。ｘ_{Ｍ，ｔｒｕｅ}は時間領域Ｍにおける目標１００の位置を示す。

すなわち、方位測定値θ_ｋが真値ｘ_{Ｍ，ｔｒｕｅ}を示すもしくは真値ｘ_{Ｍ，ｔｒｕｅ}に近い値を示す場合は正常値である。
図３において、線分ｄ４が時間領域Ｍにおける目標１００の方位（真値ｘ_{Ｍ，ｔｒｕｅ}）を示す正常値であるのに対し、線分ｄ１はマルチパス環境によって目標１００の方位（位置）を示さない異常値である。

図３に示す状態において、異常値判定部４４は、第１の方位センサ１０_１が受信した電波による方位測定値（θ_ｋ、φ_ｋ）を異常値であると判定し、異常値であると判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を設定し直して異常値と判定した方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる。
一方、図３に示す状態において、異常値判定部４４は、第４の方位センサ１０_４が受信した電波による方位測定値（θ_ｋ、φ_ｋ）を正常値であると判定し、方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）はそのままとして正常値と判定した方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度をそのままとする。

すなわち、異常値判定部４４は異常値であると判定した方位測定値（θ_ｋ、φ_ｋ）に対する信頼度を下げる。
その結果、測位処理部４５は、第１の方位センサ１０_１から第４の方位センサ１０_４それぞれが受信した電波による方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する異常値判定部４４により再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて測位値を算出する測位処理を行い、時間領域ｍにおける目標１００の推定位置である測位値ｘ_ｍを得、時間領域ｍ、つまり代表時刻ｔ_ｍにおける目標１００の測位値ｘ_ｍを示す目標位置情報を得る。

この時の測位値を算出する測位処理は、第１の方位センサ１０_１が受信した電波による方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）の信頼度が下げられているため、測位処理における第１の方位センサ１０_１が受信した電波による方位測定値（θ_ｋ、φ_ｋ）の影響を抑えることができ、測位処理部４５により得た目標１００の推定位置を示す測位値ｘ_ｍを目標１００の真値ｘ_{Ｍ，ｔｒｕｅ}に近い値として得られる。
このようにして、図２に示すように、時間領域１から時間領域Ｍまでの代表時刻ｔ_ｍでの測位値ｘ_１から測位値ｘ_Ｍが測位処理部４５により得られる。

出力部４６は、測位処理部４５が得た目標１００の推定位置を示す時間領域１から時間領域Ｍまでの代表時刻ｔ_ｍでの測位値ｘ_１から測位値ｘ_Ｍを、図３に示すように、時系列順に全てつなげ、目標１００の推定軌跡を示す物体軌跡情報として出力装置５０に出力する。
出力装置５０は、出力部４６からの物体軌跡情報を利用する装置であり、例えば、物体軌跡情報に基づいて物体の軌跡を表示する表示器である。

異常値判定部４４は、図１に示すように、方位情報読込部４４ａと測位ゲート判定部４４ｂと信頼度設定部４４ｃを備える。
方位情報読込部４４ａは、時間領域分割部４３が設定した時間領域ｍ毎に存在する、複数の方位センサ１０が受信した目標１００からの電波に対する方位測定値（θ_ｋ、φ_ｋ）を示す方位情報、方位情報が示す方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報、測定時刻ｔ_ｋ、方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を時系列順に読み込む。

方位情報読込部４４ａは、説明の都合上、機能部として説明しているが、方位情報読込部４１が読み込み、時間領域分割部４３により、時系列順に時間領域ｍ毎に分割した、複数の方位センサ１０が受信した目標１００からの電波に対する方位測定値（θ_ｋ、φ_ｋ）を示す方位情報、方位情報が示す方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報、測定時刻ｔ_ｋ、方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を、直接、測位ゲート判定部４４ｂが用いる。

測位ゲート判定部４４ｂは、複数の方位センサ１０それぞれに対して、時間領域ｍ毎に仮測位値と仮測位精度に基づく測位ゲートを用いて時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）の異常値を判定する処理である。
測位ゲート判定部４４ｂによる方位測定値（θ_ｋ、φ_ｋ）が異常値を示すか否かを判定する処理は、時間領域ｍに存在する複数の方位センサ１０それぞれが受信した電波による方位測定値（θ_ｋ、φ_ｋ）が示す位置が一点に集中しているか、つまり唯一の位置を示しているか、あるいは、測位処理の結果が速度成分を含む場合は位置と速度からなる状態ベクトルの周辺に方位測定値（θ_ｋ、φ_ｋ）が集中するかを基に、方位測定値（θ_ｋ、φ_ｋ）が異常値であるか正常値であるかを判定する処理である。

測位ゲート判定部４４ｂによる方位測定値（θ_ｋ、φ_ｋ）が異常値であるか正常値であるかの判定は、Ｎ個の方位センサ１０から複数の方位センサを選択し、組わけしたセンサ組毎の複数の方位センサによる方位測定値（θ_ｋ、φ_ｋ）が測位ケート内に存在するかを判定することにより信頼度を評価し、当該評価した信頼度が高いセンサ組毎に仮測位値同士の分散の大きさを時間領域における測位の異常度とし、異常度が設定値より持続的に低い時間領域を正常な時間領域、設定値より持続的に長い時間領域を異常な時間領域とする。
なお、異常度が最も低い時間領域を正常な時間領域としてもよい。

今、Ｎ個の方位センサ１０から目標１００の推定位置を求める方位センサ１０として４個を選択した場合について、測位ゲート判定部４４ｂによる異常値であるか正常値であるかを判定する処理の概念を、図４を用いて説明する。方位センサ１０の数は４個に限られるものではない。
図４において、Ｓ１～Ｓ４は第１の方位センサ１０_１から第４の方位センサ１０_４を示し、第１の方位センサＳ１から第４の方位センサＳ４それぞれから伸びている破線による線分ｄ１から線分ｄ４が時間領域ｍに存在する第１の方位センサＳ１から第４の方位センサ１０_４それぞれの方位測定値θ_ｋを示す。ｘ_{ｍ，ｔｒｕｅ}は時間領域ｍにおける目標１００の位置を示す。

また、前提として、第１の方位センサＳ１から第４の方位センサＳ４それぞれが受信した電波による方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）が等しく設定されるものとする。
第１の方位センサＳ１から第４の方位センサＳ４それぞれが受信した電波による方位測定値（θ_ｋ、φ_ｋ）とそれに付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋ及び各方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を用いて測位処理を行い、目標１００の測位値ｘ_ｍを得る。

測位値ｘ_ｍが真値ｘ_{Ｍ，ｔｒｕｅ}から離れる距離は、例えば、線分ｄ２によって示す方位測定値θ_ｋと線分ｄ３によって示す方位測定値θ_ｋが一次従属に近くなるときは、線分ｄ１によって示す方位測定値θ_ｋと線分ｄ４によって示す方位測定値θ_ｋの内どちらも同じだけ信頼して測位値ｘ_ｍを計算するため、真値ｘ_{Ｍ，ｔｒｕｅ}から大きく離れる。
すなわち、目標１００の測位値ｘ_ｍは線分ｄ１によって示す方位測定値θ_ｋの影響により目標１００の真値ｘ_{Ｍ，ｔｒｕｅ}から離れている。

一方、方位測定値（θ_ｋ、φ_ｋ）の示す位置が集中している唯一の位置、あるいは、状態ベクトルに目標１００が存在することを信頼すると、図４において目標１００が存在すると信頼できる位置は、線分ｄ２によって示す方位測定値θ_ｋと線分ｄ３によって示す方位測定値θ_ｋと線分ｄ４によって示す方位測定値θ_ｋが集中している位置は真値ｘ_{Ｍ，ｔｒｕｅ}の周辺となる。

従って、線分ｄ１によって示す方位測定値θ_ｋは異常値と判定できる。
測位ゲート判定部４４ｂは、異常値と判定した線分ｄ１によって示す方位測定値θ_ｋに付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を、測位値を算出する測位処理における寄与が低くなるように設定し直す。

次に、測位ゲート判定部４４ｂによる異常値の判定処理を、図５を用いて説明する。
今、Ｎ個の方位センサ１０から目標１００の推定位置を求める、つまり、異常値の判定処理を行う方位センサ１０として４個を選択した場合について説明する。方位センサ１０の数は４個に限られるものではない。
図５において、Ｓ１～Ｓ４は第１の方位センサ１０_１から第４の方位センサ１０_４を示し、第１の方位センサＳ１から第４の方位センサＳ４それぞれから伸びている破線による線分ｄ１から線分ｄ４が時間領域ｍに存在する第１の方位センサＳ１から第４の方位センサ１０_４それぞれの方位測定値θ_ｋを示す。

測位ゲート判定部４４ｂは次の処理を行う。
第１に、第１の方位センサＳ１から第４の方位センサＳ４から一定の台数、本例では３台の方位センサを選択し、組わけする。例えば、方位センサＳ１、Ｓ２、Ｓ３の組（第１の組とする）、方位センサＳ１、Ｓ２、Ｓ４の組（第２のセンサ組とする）、方位センサＳ１、Ｓ３、Ｓ４の組（第３のセンサ組とする）、方位センサＳ２、Ｓ３、Ｓ４の組（第４のセンサ組とする）に組わけする。

第２に、第１のセンサ組から第４のセンサ組それぞれにおいて、時間領域分割部４３により分割された時間領域ｍ毎に、各組における方位センサＳが受信した電波による方位測定値（θ_ｋ、φ_ｋ）を一般に知られている測位値を算出する測位処理を行うことにより、仮測位値ｘ_{ｍ，｛１，２，３｝}、仮測位値ｘ_{ｍ，｛１，２，４｝}、仮測位値ｘ_{ｍ，｛１，３，４｝}、仮測位値ｘ_{ｍ，｛２，３，４｝}と仮測位精度Ｒ_{ｍ，｛１，２，３｝}、仮測位精度Ｒ_{ｍ，｛１，２，４｝}、仮測位精度Ｒ_{ｍ，｛１，３，４｝}、仮測位精度Ｒ_{ｍ，｛２，３，４｝}を推定する。

第３に、センサ組それぞれにおいて、時間領域ｍ毎に、推定した対応する仮測位値ｘ_{ｍ，｛１，２，３｝}、仮測位値ｘ_{ｍ，｛１，２，４｝}、仮測位値ｘ_{ｍ，｛１，３，４｝}、仮測位値ｘ_{ｍ，｛２，３，４｝}によって定まる測位ゲートＧａ～Ｇｄそれぞれを設定する。
時間領域ｍ毎に、第１の方位センサＳ１から第４の方位センサＳ４それぞれが受信した電波による方位測定値（θ_ｋ、φ_ｋ）が測位ゲートＧａ～Ｇｄ内に存在するか否かを判定する。

第４に、各センサ組において、時間領域ｍ毎に、方位測定値（θ_ｋ、φ_ｋ）が対応する測位ゲートＧａ～Ｇｄ内に存在するか否かの判定に基づき、各センサ組による方位測定値（θ_ｋ、φ_ｋ）の信頼度を評価し、評価した結果、仮測位値ｘ_ｍ，Ａｉの信頼度が高い場合、つまり、各センサ組の方位測定値（θ_ｋ、φ_ｋ）がそれぞれの測位ゲートＧａ～Ｇｄ内に存在する場合に方位測定の一致性を認め、一致性を認めた方位測定値（θ_ｋ、φ_ｋ）を正常値と判定し、全てのセンサ組において一致性が認められなかった方位測定値（θ_ｋ、φ_ｋ）を異常値と判定する方位測定値の測位ゲート判定を行う。

要するに、測位ゲート判定部４４ｂは、複数の方位センサＳ１～Ｓ４から一定台数、一例としての３台の方位センサを選択して、複数のセンサ組に組わけし、複数のセンサ組それぞれにおいて、時間領域分割部４３により分割された時間領域ｍ毎に仮測位値を推定し、推定された仮測位値によって定まる測位ゲートＧ１～Ｇ４を設定し、時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が設定された測位ゲートＧ１～Ｇ４内に存在すると正常値と判定し、時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が設定されたいずれの測位ゲートＧ１～Ｇ４内に存在しないと異常値と判定する。

測位ゲート判定部４４ｂによる測位ゲート判定について、以下に数式を用いて説明する。
今、目標１００の観測が可能な方位センサ１０の数をＮ個であるとし、時間領域ｍでの測位で使用される番号付きの複数の方位センサ１０の集合をＵ＝｛１，２，・・・，Ｎ｝、時間領域ｍ内の測定時刻ｔ_ｋを特定するための方位測定値番号ｋの集合をＳ_ｍ＝｛ｋ_ｍ，ｓ，・・・，ｋ_ｍ，ｅ｝とする。
方位測定値番号ｋは測定時刻ｔ_ｋにおける方位測定値（θ_ｋ、φ_ｋ）と１対１に対応する。ｋ_ｍ，ｓは時間領域ｍでの最初の方位情報の方位測定値（θ_ｋ、φ_ｋ）に対応し、ｋ_ｍ，ｅは時間領域ｍでの最初の方位情報の方位測定値（θ_ｋ、φ_ｋ）に対応する。

集合Ｕから方位センサ１０を３台選択する時の組み合わせは、_ＮＣ_３通りある。
このとき選択されるセンサ組をＡ_ｉ（ｉ＝１，２，・・・，Ｉ）（Ｉ＝_ＮＣ_３）とするとセンサの組み合わせは次式（１）で表される。
Ａ_１＝｛１，２，３｝、Ａ_２＝｛１，２，４｝、・・、Ａ_Ｉ＝｛Ｎ－２，Ｎ－１，Ｎ｝
・・・（１）

センサ組Ａ_ｉの持つｍ番目の時間領域に存在する方位測定値番号ｋの集合をＳ_ｍ,Ａｉとし、集合Ｓ_ｍ,Ａｉ内の方位測定値番号ｋを時系列順に次式（２）で表す。
ｋ＝ｋ_{（ｍ,Ａｉ），ｓ}、・・・、ｋ_{（ｍ,Ａｉ），ｅ} ・・・（２）
すなわち、上式（２）は、方位測定値番号ｋをｋ_ｍ,Ａｉ個の方位測定値として表わしている。

上式（２）により方位測定値番号ｋをあらわすと、次式（３）で示す集合において、集合同士は次式（４）に示すように方位測定値番号の被りが存在する。

測位ゲート判定部４４ｂによる測位ゲートでは、初めにセンサ組Ａ_ｉの方位測定値（θ_ｋ、φ_ｋ）を用いて時間領域ｍの次式（５）で表される代表時刻ｔ_ｍ，Ａｉに用いられる仮測位値ｘ_ｍ，Ａｉと仮測位精度Ｒ_ｍ，Ａｉを一般に知られている測位値を算出する測位処理を行うことにより求める。

センサ組Ａ_ｉにおいて、仮測位値ｘ_ｍ，Ａｉは、時間領域ｍに存在する方位測定値（θ_ｋ、φ_ｋ）と方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて測位処理を行ない求められる。
また、センサ組Ａ_ｉにおいて、仮測位精度Ｒ_ｍ，Ａｉは、仮測位値ｘ_ｍ，Ａｉと仮測位値ｘ_ｍ，Ａｉに対応する方位センサＳの位置及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて推定される。

測位処理を行う測位の方法としては、例えば、重み付き最小二乗推定による方法、又はＭＡＰ（Maximum a Posteriori）推定による方法が用いられる。
また、状態（測位値）ｘ_ｍに位置成分だけでなく速度成分を含む場合、状態ｘ_ｍは３次元空間において位置３次元、速度３次元による６成分を持つ状態ベクトルであり、以下の非特許文献に示された複数のセンサの異なる方位測定時刻と方位測定誤差に基づいてＭＡＰ推定で目標の位置と速度を推定する方法を用いてもよい。
仮測位精度Ｒ_ｍ，Ａｉの推定方法に、例えば、以下の非特許文献に記載のＢＣＲＢ（Bayesian Cramer-Rao Boundary）を下限とした観測誤差共分散を設定する方法が用いられる。

非特許文献：L.Badriasl, s.Arulampalam and A.Finn, “A Novel Batch Bayesian WIV Estimator for Three-Dimensional TMA Using Bearing and Elevation Measurements,” IEEE Transactions on Signal Processing,vol.66,no.4,pp.1023-1036,Feb.15,2018.

時間領域ｍ内のｋ番目の方位測定値（θ_ｋ、φ_ｋ）の残差Ｌ_ｋ，Ａｉは上式（５）で示す測定時刻ｔ_ｍ，Ａｉを次式（７）で示す状態遷移行列Φ_{ｋ｜（ｍ，Ａｉ）}で測定時刻ｔ_ｋに変換した次式（６）で示す仮測位値ｘ_ｋ，Ａｉと次式（８）で示す仮測位精度Ｒ_ｋ，Ａｉによって定義される統計的距離で計算される。

なお、上式（８）における右辺第２項Ｑｋは駆動雑音共分散である。上式（９）は予測値と判定対象の方位測定値（θ_ｋ、φ_ｋ）の残差ｅ_ｋ，Ａｉを示す。上式（１０）はセンサ組Ａ_ｉにおけるＤＯＡの残差誤差共分散を示す。上式（１０）における右辺におけるＨ_ｋは角度成分に対する位置成分の偏微分行列である。上式（１１）はＤＯＡの誤差共分散Σ_θφ，ｋを示す。上式（１２）は時間領域ｍ内のｋ番目の方位測定値（θ_ｋ、φ_ｋ）の残差Ｌ_ｋ，Ａｉの２乗を示す。

仮測位値ｘ_ｋ，Ａｉの計算に使用されたセンサ組Ａ_ｉの方位測定値（θ_ｋ、φ_ｋ）の集合Ｓ_ｍ，Ａｉの内、一定の割合パラメータβ以上の個数の方位測定値ｋ＝ｋ_{（ｍ，Ａｉ），Ｓ}、・・・、ｋ_{（ｍ，Ａｉ），ｅ}の残差Ｌ_ｋ，Ａｉが閾値α_１内に入っている場合に、その閾値内の方位測定値全てにフラグを立てる。

センサ組Ａ_ｉにおいてフラグが立った方位測定値（θ_ｋ、φ_ｋ）の集合をＴ_{ｍ，Ａｉ´}とすると、このフラグ条件を満たすセンサ組Ａ_ｉ´の仮測位値ｘ_{ｋ，Ａｉ´}は方位測定値が集中しているために当該目標１００である可能性が高く、集合Ｔ_{ｍ，Ａｉ´}に属する方位測定値（θ_ｋ、φ_ｋ）は目標の正常値である可能性が高いと言える。
次式（１３）及び次式（１４）において、ＩＦはカッコ内の条件を満たすときに１を返し、満たさないときに０を返す判定式である。

この処理を反復し、全てのセンサ組Ａ_ｉにおいて正常と思われる方位測定値（θ_ｋ、φ_ｋ）の集合Ｔ_{ｍ，Ａｉ´}が定まる。このとき、次式（１５）に示す和集合Ｔ_ｍに属する方位測定値（θ_ｋ、φ_ｋ）をこの測位ゲート判定における正常値と判定し、次式（１６）に示す補集合Ｔ＿ｍ＾Ｃに属する時間領域ｍ内の方位測定値（θ_ｋ、φ_ｋ）、つまり、全てのセンサ組で一度も正常値の判定を受けない方位センサの方位測定値（θ_ｋ、φ_ｋ）は異常値と判定する。

また、このとき、全てのセンサ組Ａ_ｉにおいて一度でも正常値と判定された方位測定値（θ_ｋ、φ_ｋ）を集合Ｔ_ｍに含めるのではなく、仮測位値の空間クラスタリングを行ってどのセンサ組Ａ_ｉが同一の目標１００を示しているかを推定して集合Ｔ_ｍを決めることもできる。

信頼度設定部４４ｃは、測位ゲート判定部４４ｂで異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）をマルチパス環境時の精度として方位測定誤差を劣化、つまり、方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げたパラメータ値（σ_{θ，ｅｒｒ´}、σ_{φ，ｅｒｒ´}）に設定し、パラメータ値（σ_{θ，ｅｒｒ´}、σ_{φ，ｅｒｒ´}）を再設定された方位測定誤差として得、測位処理部４５により用いられる。

方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げたパラメータ値（σ_{θ，ｅｒｒ´}、σ_{φ，ｅｒｒ´}）は意図的に大きくするのが良い。
ここで言う意図的に大きくするとは、異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差におけるσ_θ，ｋの値を、σ_θ，ｋの値より大きいパラメータ値におけるσ_{θ，ｅｒｒ´}の値に設定することを意味している。
要するに、異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差が示す値を、正常値と判定される方位測定値に付随する方位測定誤差が示す値より大きい値のパラメータ値に再設定する。
すなわち、パラメータ値は、異常値と判定される方位測定値を正常値と判定される方位測定値に対して設定した信頼度によって決められるパラメータ要素である。

また、信頼度設定部４４ｃは、測位ゲート判定部４４ｂで正常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）をそのままとし、測位処理部４５により、方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げない、そのままとされた再設定された方位測定誤差として用いられる。

次に、目標軌跡推定装置の動作を、主として異常値判定部４４の動作を、図６を用いて説明する。
方位情報読込部４１が、複数の方位センサ１０それぞれが目標１００を観測した時間の、記憶装置３０に蓄積された複数の方位測定部２０からの方位測定値（θ_ｋ、φ_ｋ）を示す方位情報と方位測定値（θ_ｋ、φ_ｋ）に付随し紐づけされた測定時刻ｔ_ｋ及び各方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を示す方位情報を読み込む。

併せて、複数の方位センサ１０それぞれが目標１００を観測した時間の、記憶装置３０に蓄積された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を示す方位測定誤差情報、又は、誤差設定部４２により設定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を読み込む。

次に、 時間領域分割部４３が、方位情報読込部４１に読み込まれた方位測定値（θ_ｋ、φ_ｋ）と方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋ及び各方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を示す方位情報を測定時刻ｔ_ｋをもとにＭ個に分割する。

時間領域分割部４３に時間領域ｍ毎に分割されたＮ個のセンサが目標を観測した時間の方位測定値（θ_ｋ、φ_ｋ）について、異常値判定部４４であるか否かの判定を図６に示したステップＳＴ１からステップＳＴ１４に従い行う。
ステップＳＴ１は方位情報読込部４４ａが、時間領域ｍ毎に分割されたＮ個のセンサが目標を観測した時間の方位測定値（θ_ｋ、φ_ｋ）と方位測定値（θ_ｋ，φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と、測定時刻ｔ_ｋと、方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を読み込む。
ステップＳＴ１は、時間領域ｍ毎の方位情報を読み込むステップであり、時間領域分割部４３がＭ個に分割した方位測定値（θ_ｋ、φ_ｋ）等を測位ゲート判定部４４ｂが得るステップである。

ステップＳＴ２からステップＳＴ１４は測位ゲート判定部４４ｂが行うステップである。
ステップＳＴ２において、複数の方位センサ１０から一定台数の方位センサ１０を選択して複数のセンサ組に組わけする。例えば、４台の方位センサから３台の方位センサ１０を１組とするセンサ組に組わけする。ステップＳＴ２はセンサ組を選択するステップである。

ステップＳＴ３において、組わけされたセンサ組それぞれにおいて、時間領域ｍ毎に、方位測定値（θ_ｋ、φ_ｋ）を一般に知られている測位値を算出する測位処理を行うことにより、仮測位値を計算する。ステップＳＴ３はそれぞれの組において各時間領域ｍにおける仮測位値を計算するステップである。

例えば、４台の方位センサから３台の方位センサ１０を１組とし、第１の組から第４の組に組わけした場合、時間領域ｍにおいて、第１の組に対して仮測位値ｘ_{ｍ，｛１，２，３｝}、第２の組に対して仮測位値ｘ_{ｍ，｛１，２，４｝}、第３の組に対して仮測位値ｘ_{ｍ，｛１，３，４｝}、第４の組に仮測位値ｘ_{ｍ，｛２，３，４｝}を計算する。
このような仮測位値の計算が時間領域１から時間領域Ｍのｍ個の時間領域に対して行われ、時間領域の数とセンサ組の組数を乗算した数の仮測位値が計算される。

ステップＳＴ４において、ステップＳＴ３において計算された全ての仮測位値に付随する仮測位精度が推定される。ステップＳＴ４は仮測位値に付随する仮測位精度を推定するステップである。
仮測位精度の推定は、仮測位値と仮測位値に対応する方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて推定される。
例えば、仮測位値ｘ_{ｍ，｛１，２，３｝}に対して仮測位精度Ｒ_{ｍ，｛１，２，３｝}、仮測位値ｘ_{ｍ，｛１，２，４｝}に対して仮測位精度Ｒ_{ｍ，｛１，２，４｝}、仮測位値ｘ_{ｍ，｛１，３，４｝}に対して仮測位精度Ｒ_{ｍ，｛１，３，４｝}、仮測位値ｘ_{ｍ，｛２，３，４}に仮測位精度Ｒ_{ｍ，｛２，３，４｝}を推定する。

ステップＳＴ５において、ステップＳＴ３において計算された全ての仮測位値に対して測位ゲートを設定する。ステップＳＴ５は測位ゲートを作成するステップである。
測位ゲートの設定は仮測位値と仮測位精度から作成される。
例えば、仮測位値ｘ_{ｍ，｛１，２，３｝}に対して測位ゲートＧａ、仮測位値ｘ_{ｍ，｛１，２，４｝}に対して測位ゲートＧｂ、仮測位値ｘ_{ｍ，｛１，３，４｝}に対して測位ゲートＧｃ、仮測位値ｘ_{ｍ，｛２，３，４}に測位ゲートＧｄを設定する。

ステップＳＴ６において、方位測定値（θ_ｋ、φ_ｋ）を選択し、ステップＳＴ７において、選択した方位測定値（θ_ｋ、φ_ｋ）が対応のゲートに入るか否かを判定する。ステップＳＴ７は方位測定値の判定ステップである。
すなわち、選択された方位測定値（θ_ｋ、φ_ｋ）が存在する時間領域において、組わけされたセンサ組に対する測位ゲートＧに、選択された方位測定値（θ_ｋ、φ_ｋ）が入るか否かを判定し、入ると判定するとステップＳＴ８に進み、入らないと判定されるとステップＳＴ９に進む。

ステップＳＴ８において、ステップＳＴ７において、測位ゲートＧに入ると判定された方位測定値（θ_ｋ、φ_ｋ）は正常値の候補とされる。
ステップＳＴ９において、ステップＳＴ６において方位測定値（θ_ｋ、φ_ｋ）が全て選択されたか否かを判定し、全て選択されていない場合は、ステップＳＴ６に戻り、ステップＳＴ７及びステップＳＴ８が繰り返される。
ステップＳＴ９において、方位測定値（θ_ｋ、φ_ｋ）が全て選択されている場合はステップＳＴ１０に進む。
このようにして、ステップＳＴ１により読み込まれた方位測定値（θ_ｋ、φ_ｋ）全てについて、測位ゲートＧに入るか否かの判定が行われる。

ステップＳＴ１０において、時間領域ｍ毎に、組わけされたセンサ組それぞれにおいて、方位測定値（θ_ｋ、φ_ｋ）が多くの割合で測位ゲートＧに入った、つまり、ステップＳＴ８において選択された方位測定値（θ_ｋ、φ_ｋ）の正常値の候補とされた割合を判定し、多くの割合で測位ゲートＧに入ったと判定するとステップＳＴ１１に進み、入っていないと判定するとステップＳＴ１２に進む。

ステップＳＴ１１において、ステップＳＴ１０において、組わけされたセンサ組において、方位測定値（θ_ｋ、φ_ｋ）が多くの割合で測位ゲートＧに入ったとされると、当該センサ組において測位測定値（θ_ｋ、φ_ｋ）は正常値の候補から正常値とする。
ステップＳＴ１２において、ステップＳＴ１１において、測位測定値（θ_ｋ、φ_ｋ）がセンサ組の正常値として１回以上判定されたかを判定し、１回以上判定されたとするとステップＳＴ１３に進み、１回もないと判定されるとステップＳＴ１４に進む。

ステップＳＴ１３において、ステップＳＴ１２においてセンサ組の正常値として１回以上判定された測位測定値（θ_ｋ、φ_ｋ）を異常値判定部４４における正常値とする。
ステップＳＴ１４において、ステップＳＴ１２においてセンサ組の正常値として１回もないと判定された測位測定値（θ_ｋ、φ_ｋ）を異常値判定部４４における異常値とする。
このようにして、ステップＳＴ１により読み込まれた方位測定値（θ_ｋ、φ_ｋ）全てについて、正常値か異常値かの判定が行われる。

ステップＳＴ１０からステップＳＴ１４は、要するに、時間領域ｍにおいて、選択された方位測定値（θ_ｋ、φ_ｋ）が測位ゲートＧに入った数、つまり、選択された方位測定値（θ_ｋ、φ_ｋ）の残差Ｌ_ｋ，Ａｉが閾値α_１内に入った数が割合パラメータβ以上であるかを判定する。
言い換えれば、選択された方位測定値（θ_ｋ、φ_ｋ）が一点に集中しているか、つまり唯一の位置を示しているか、あるいは、位置と速度からなる状態ベクトルの周辺に集中しているかを判定する。
判定した結果に基づき、正常値か異常値かの判定が行われる。

ステップ１５は、信頼度設定部４４ｃが、ステップ１４において、異常値と判定された方位測定値（θ_ｋ、φ_ｋ）に対して方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を大きくして方位測定値（θ_ｋ、φ_ｋ）に対する信頼度を低下させる。
例えば、方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げたパラメータ値（σ_{θ，ｅｒｒ´}、σ_{φ，ｅｒｒ´}）に設定する。

次に、測位処理部４５が、時間領域ｍ毎に、方位情報読込部４１に読み込まれた方位測定値（θ_ｋ、φ_ｋ）と、方位測定値（θ_ｋ、φ_ｋ）に付随する異常値判定部４４により再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を用いて測位値を算出する測位処理を行い、時間領域ｍにおける目標１００に対する測位値ｘ_ｍを得、時間領域ｍにおける目標１００の測位値ｘ_ｍを示す目標位置情報を得る。

出力部４６が、測位処理部４５が得た目標１００の推定位置を示す時間領域１から時間領域Ｍまでの代表時刻ｔ_ｍでの測位値ｘ_１から測位値ｘ_Ｍを、時系列順に全てつなげ、目標１００の推定軌跡を示す物体軌跡情報として出力装置５０に出力する。

目標軌跡推定装置はコンピュータによるハードウェア構成により実現され、図７に示すように、プロセッサ４０１、メモリ４０２、入力インタフェース４０３、出力インタフェース４０４を備え、これらは、バス４０５を介して相互に接続されている。
メモリ４０２は大容量の半導体メモリ（ＲＡＭ：Random Access Memory）と、ハードディスク装置又はＳＳＤ装置などの不揮発性記録装置などの記憶装置（ＲＯＭ：Read only memory）を備える。

プロセッサ４０１はメモリ４０２、入力インタフェース４０３、出力インタフェース４０４を制御、管理する。
プロセッサ４０１は、メモリ４０２におけるＲＯＭに記憶してあるプログラムを一旦ＲＡＭに格納し、ＲＡＭに格納されたソフトウェア・プログラムに従って目標１００の推定軌跡を生成する処理を実行する。

入力インタフェース４０３は図１に示す方位情報読込部４１に該当し、出力インタフェース４０４は図１に示す出力部４６に該当する。
図１に示す時間領域分割部４３と異常値判定部４４と測位処理部４５と出力部４６の一部それぞれの機能は、プロセッサ４０１がメモリ４０２に格納されたプログラムをロードし、ロードしたプログラムに従って動作することによって実現される。

ステップＳＴ１からステップＳＴ１４を含む目標軌跡推定装置の目標軌跡推定方法はプロセッサ４０１がメモリ４０２に格納されたプログラムに従って処理を実行することにより行われる。
すなわち、メモリ４０２に格納されたプログラムは、目標１００からの到来電波を受信した複数の方位センサ１０それぞれが目標１００を観測した時間を時系列順に複数の時間領域ｍに分割し、複数の方位センサ１０それぞれにより得られる方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を分割した時間領域ｍ毎に分割する手順と、複数の方位センサ１０から一定台数の方位センサ１０を選択して複数のセンサ組に組わけし、複数のセンサ組それぞれにおいて、分割された時間領域ｍ毎に仮測位値ｘ_ｍ，Ａｉを推定し、当該得られた仮測位値ｘ_ｍ，Ａｉによって定まる測位ゲートＧを設定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定された測位ゲートＧ内に存在すると正常値と判定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定されたいずれの測位ゲートＧ内に存在しないと異常値と判定する手順と、異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再設定する手順と、分割された時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて目標１００に対する各時間領域ｍにおける測位値を推定する手順と、推定された時間領域ｍ毎の測位値を時系列につなげて目標１００の軌跡を示す物体軌跡情報を出力する手順と、を備える。

以上のように、実施の形態１に係る目標軌跡推定装置４０は、目標１００からの到来電波を受信した複数の方位センサ１０それぞれが目標１００を観測した時間を時系列順に複数の時間領域ｍに分割し、複数の方位センサ１０それぞれにより得られる方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を分割した時間領域ｍ毎に分割する時間領域分割部４３と、分割された時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が異常値を示すか否かを判定し、異常値と判定した方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再設定し、異常値でないと判定した方位測定値に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度をそのままに再設定する異常値判定部４４と、時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて目標１００に対する各時間領域ｍにおける測位値を推定する測位処理部４５と、測位処理部４５により推定された時間領域ｍ毎の測位値を時系列につなげて目標１００の軌跡を示す物体軌跡情報を出力する出力部４６を備えたので、方位センサ１０がマルチパス環境下にあったとしても、目標１００の測位値を精度高く推定でき、目標１００の実際の軌跡により近い軌跡推定を行うことができる。

実施の形態１に係る目標軌跡推定装置４０は、異常値判定部４４における方位測定値（θ_ｋ、φ_ｋ）が異常値を示すか否かの判定は、複数の方位センサ１０から一定台数の方位センサ１０を選択して複数のセンサ組に組わけし、複数のセンサ組それぞれにおいて、分割された時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて測位処理を行ない目標１００に対するセンサ組における測位値ｘ_ｍを得、当該測位値ｘ_ｍと当該測位値ｘ_ｍに対応する方位センサ１０の位置及び方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いてセンサ組における仮測位精度Ｒ_ｍを推定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が仮測位値ｘ_ｍ，Ａｉと仮測位精度Ｒ_ｍ，Ａｉによって定まる測位ゲートＧ内に存在するかを判定することにより当該方位測定値（θ_ｋ、φ_ｋ）の信頼度を評価し、当該評価した信頼度が高い方位測定値（θ_ｋ、φ_ｋ）を正常値、当該評価した信頼度が低い方位測定値（θ_ｋ、φ_ｋ）を異常値とする判定するので、方位測定値（θ_ｋ、φ_ｋ）の一致性を判定する測位ゲートＧを用いて、方位測定値（θ_ｋ、φ_ｋ）の信頼度を設定するため、異常な方位測定値（θ_ｋ、φ_ｋ）の寄与を小さくした測位値を計算することが可能となる。

また、正常な方位測定値（θ_ｋ、φ_ｋ）を出力する方位センサ１０を特定することが可能となる。
さらに、測位ゲートＧによる判定はセンサ組毎に測位値ｘ_ｍを計算して異常値の判定を行うため、仮測位精度Ｒ_ｍによって定められる測位ゲートＧに基づいて定量的に方位測定値（θ_ｋ、φ_ｋ）の異常を判定することができる。これにより、複数の方位センサ１０の方位測定値（θ_ｋ、φ_ｋ）を用いて行う測位処理の、マルチパス環境下で生じる異常値に対するロバスト性を確立し、マルチパス環境下でも正常な方位測定値（θ_ｋ、φ_ｋ）を信頼した高精度な測位処理を行うことができる。

実施の形態１に係る目標軌跡推定装置４０は、異常値判定部４４が、複数の方位センサ１０から一定台数の方位センサ１０を選択して複数のセンサ組に組わけし、複数のセンサ組それぞれにおいて、分割された時間領域ｍ毎に仮測位値ｘ_ｍ，Ａｉを推定し、当該得られた仮測位値ｘ_ｍ，Ａｉによって定まる測位ゲートＧを設定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定された測位ゲートＧ内に存在すると正常値と判定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定されたいずれの測位ゲートＧ内に存在しないと異常値と判定する測位ゲート判定部４４ｂと、異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再設定する信頼度設定部４４ｃとを備え、測位処理部４５が分割された時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて目標１００に対する各時間領域ｍにおける測位値を推定し、出力部４６が推定された時間領域ｍ毎の測位値を時系列につなげて目標１００の軌跡を示す物体軌跡情報を出力するので、測位処理部４５が異常値と判定された方位測定値（θ_ｋ、φ_ｋ）に付随する信頼度が下げられた方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて測位処理するため、方位センサ１０がマルチパス環境下にあったとしても、目標１００の測位値を精度高く推定でき、目標１００の実際の軌跡により近い軌跡推定を行うことができる。
また、仮測位値ｘ_ｍ，Ａｉによって定まる測位ゲートＧを用いて定量的に方位測定値（θ_ｋ、φ_ｋ）の異常を判定することができる。

実施の形態２．
実施の形態２に係る目標軌跡推定装置を図８から図１０を用いて説明する。
実施の形態２に係る目標軌跡推定装置は、実施の形態１に係る目標軌跡推定装置が複数の方位センサ１０により得られる方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を分割された時間領域ｍ毎に順次時系列順に測位処理を行うのに対して、複数の方位センサ１０それぞれが目標１００を観測する間（以下、目標観測時間という）に複数の方位センサ１０により得られる方位測定値（θ_ｋ、φ_ｋ）を一括で取得して軌跡を推定するバッチ型の処理を行う点が相違し、その他の点は同じである。
なお、図８から図１０中、図１から図７において付された符号と同一符号は同一又は相当部分を示す。

図８は、実施の形態２に係る目標軌跡推定装置を含む目標軌跡推定システムの概略構成を示すブロック図である。
目標軌跡推定システムは、図８に示すように、複数の方位センサ１０_１～１０_Ｎと複数の方位測定部２０_１～２０_Ｎと記憶装置３０と目標軌跡推定装置４０Ａと出力装置５０を備える。
複数の方位センサ１０_１～１０_Ｎと複数の方位測定部２０_１～２０_Ｎと記憶装置３０と出力装置５０は、実施の形態１において説明した複数の方位センサ１０_１～１０_Ｎと複数の方位測定部２０_１～２０_Ｎと記憶装置３０と出力装置５０と同じであるので、説明を省略する。

目標軌跡推定装置４０Ａは、方位情報読込部４１と誤差設定部４２と時間領域分割部４３と異常値判定部４４Ａと測位処理部４５と精度推定部４７と追尾処理部４８と出力部４６とを備える。
方位情報読込部４１と時間領域分割部４３は実施の形態１において説明した方位情報読込部４１と時間領域分割部４３と同じであるので、説明を省略する。

異常値判定部４４Ａは、方位情報読込部４４ａと測位ゲート判定部４４ｂと信頼度設定部４４ｃと時間領域選択部４４ｄと予測情報読込部４４ｅと予測ゲート判定部４４ｆを備える。
方位情報読込部４４ａと測位ゲート判定部４４ｂと信頼度設定部４４ｃは、実施の形態１に係る目標軌跡推定装置と実質同じである。

測位ゲート判定部４４ｂについて、実施の形態１において説明した一例を簡単に説明する。
測位ゲート判定部４４ｂは、複数の方位センサ１０から一定台数の方位センサ１０を選択して、複数のセンサ組に組わけし、複数のセンサ組それぞれにおいて、時間領域分割部４３により分割された時間領域ｍ毎に仮測位値ｘ_ｍ，Ａｉを推定し、推定された仮測位値ｘ_ｍ，Ａｉによって定まる測位ゲートＧを設定し、時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が設定された測位ゲートＧ内に存在すると正常値と判定し、時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が設定されたいずれの測位ゲートＧ内に存在しないと異常値と判定する。

信頼度設定部４４ｃについて、実施の形態１において説明した一例を簡単に説明する。
信頼度設定部４４ｃは、測位ゲート判定部４４ｂで異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を、方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げたパラメータ値（σ_{θ，ｅｒｒ´}、σ_{φ，ｅｒｒ´}）に再設定し、測位ゲート判定部４４ｂで正常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を、方位測定値（θ_ｋ、φ_ｋ）の信頼度を下げない、信頼度が高いそのままに再設定する。

要するに、測位ゲート判定部４４ｂと信頼度設定部４４ｃにより、目標観測時間の全時間領域に存在する全ての方位測定値（θ_ｋ、φ_ｋ）に対して測位ゲートＧを用いて正常値か異常値かの測位ゲート判定を行い、正常値と判定された方位測定値（θ_ｋ、φ_ｋ）は信頼度が高いとし、異常値と判定された方位測定値（θ_ｋ、φ_ｋ）は信頼度が低いとされる。

時間領域選択部４４ｄは、目標観測時間の全時間領域における測位ゲートＧを用いた測位ゲート判定において、信頼度が高いと判定されたセンサ組Ａ_ｉ´の仮測位値ｘ_{ｍ，Ａｉ´}同士の分散を計算し、その推移をもとに正常な時間領域ｍをｍ_ｓとして選択する。
選択された時間領域ｍ_ｓは追尾起点時間領域とする。
時間領域ｍ_ｓを選択する方法は、分散が小さい時間が続いている時間領域又は分散が最小である時間領域を選択する方法である。

測位処理部４５は、選択された追尾起点時間領域ｍ_ｓを起点として隣接する時間領域ｍ毎に順方向及び逆方向それぞれについて時系列順に、方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する信頼度設定部４４ｃにより再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を用いて測位値ｘ_ｍを算出する測位処理を行い、時間領域ｍにおける目標１００に対する測位値ｘ_ｍを得、時間領域ｍにおける目標１００の測位値ｘ_ｍを示す目標位置情報を得る。

精度推定部４７は、時間領域ｍ毎に、異常値判定部４４Ａにおける信頼度設定部４４ｃにより再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて、測位処理部４５により推定される測位値ｘ_ｍに対する測位精度Ｒ_ｍを推定する。
精度推定部４７は、時間領域ｍ毎に、測位値ｘ_ｍと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）と方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）から測位値ｘ_ｍの測位精度Ｒ_ｍを推定する。

追尾処理部４８は、測位処理部４５により推定される測位値ｘ_ｍと当該測位値ｘ_ｍに対応する精度推定部４７により推定される測位精度Ｒ_ｍを用いて当該測位値ｘ_ｍをフィルタリングして平滑値と予測値を得る
追尾処理部４８は、追尾起点時間領域ｍ_ｓを起点として隣接する時間領域ｍ毎に、測位値ｘ_ｍと測位精度Ｒ_ｍを用いてカルマンフィルタにより測位値ｘ_ｍのフィルタリングを計算する。

追尾処理部４８は、追尾起点時間領域ｍ_ｓを起点として隣接する測位値ｘ_ｍと目標１００の運動の連続性に基づく予測値とを重み付け平均した平滑値ｘ_ｍ｜ｍを得る。この平滑値ｘ_ｍ｜ｍが代表時刻の目標位置の出力となる。
追尾処理する方法には、例えば、任意の測定時刻ｔ_ｋの予測値ｘ_ｋ｜ｍを並列して出力するカルマンフィルタが存在する。

実施の形態２では、追尾起点時間領域ｍ_ｓを起点として時刻が経過する方向、つまり順方向に隣接する時間領域の測位処理に時間順方向のカルマンフィルタを適用して平滑値ｘ_ｍ｜ｍを求め、追尾起点時間領域ｍ_ｓを起点として時刻が戻る方向、つまり逆方向に隣接する時間領域の測位処理に時間逆方向のカルマンフィルタを適用して平滑値ｘ_ｍ｜ｍを求める。
追尾処理のカルマンフィルタは、前時刻の予測値と現時刻の観測値から、現時刻の平滑値（重み付け平均）と次時刻の予測値（現時刻の平滑値から運動モデルで算出）を推定する処理である。

ここで、追尾起点時間領域ｍ_ｓでは、測位ゲート判定が正常に機能することによって測位値が真値に近いと判断できるような異常度の低い時間領域を選択する必要がある。
測位ゲート判定では、方位センサの方位測定値が一点あるいは一つの状態ベクトルに集中しているかを判定するために仮測位値ｘ_{ｍ，Ａｉ´}を算出していた。
よって、測位ゲートが正常に機能したかを確かめるには、各時間領域ｍで信頼度の高いセンサ組全ての仮測位値ｘ_{ｍ，Ａｉ´}同士の分散を評価すれば良く、この分散の大きさが時間領域における測位の異常度と言える。
全時間領域の異常度を評価した際に、追尾起点時間領域ｍ_ｓは、追尾処理のしやすさから、異常度が持続的に低い時間領域、あるいは、異常度が最も低い時間領域を選択する。

まず、追尾起点時間領域ｍ_ｓ以降の時間領域ｍの追尾処理に適用した時間逆方向のカルマンフィルタの説明を、数式を用いて行う。
時間逆方向のカルマンフィルタの運動モデルは、ｘ_{ｍ｜ｍ＋１}を予測値、ｘ_{ｍ＋１｜ｍ＋１}を平滑値として代表時刻ｔ_ｍ＋１からｔ_ｍへの時間逆方向の状態遷移行列Γ_{ｍ｜ｍ＋１}を用いて記述する。
予測値ｘ_{ｍ｜ｍ＋１}は次式（１７）により表わされ、状態遷移行列Γ_{ｍ｜ｍ＋１}は次式（１８）により表わされる。

Ｗ_ｍ＋１～Ｎ（０，Ｑ´_{ｍ｜ｍ＋１}）は駆動雑音である。時間逆方向の駆動誤差共分散Ｑ´_{ｍ｜ｍ＋１}は速度成分に正の駆動雑音が加わるときに位置成分は負の方向に変化するため、非対角ブロックに負の符号を付与する必要があり、Σ_ｑを３次元位置成分の共分散行列として次式（１９）のように設定される。

観測モデルはｘ_ｍ＝ｘ_{ｍ，ｔｒｕｅ}＋ｖ_ｍとなる。
ｖ_ｍ～Ｎ（０，Ｒ_ｍ）は観測雑音であり、測位値の推定誤差に相当する。
時間逆方向のカルマンフィルタの初期ステップ（ｍ＝ｍ_ｓ）では、追尾起点時間領域ｍ_ｓの測位値ｘ_ｍｓと測位精度Ｒ_ｍｓがそのまま平滑値ｘ_{ｍｓ｜ｍｓ}（＝ｘ_ｍｓ）と平滑誤差共分散Ｐ_{ｍｓ｜ｍｓ}（＝Ｒ_ｍｓ）となる。

ｍ≦ｍ_ｓ－１の場合、時間逆方向の予測処理は次式（２０）及び次式（２１）で表わされる。
ｘ_{ｍ｜ｍ＋１}＝Γ_ｍ＋１ｘ_{ｍ＋１｜ｍ＋１} （２０）

平滑処理は次式（２２）～（２４）により表わされる。
Ｋ_ｍ＝Ｐ_{ｍ｜ｍ＋１}＋（Ｐ_{ｍ｜ｍ＋１}＋Ｒ_ｍ）^－１ （２２）
ｘ_ｍ｜ｍ＝ｘ_{ｍ｜ｍ＋１}＋Ｋ_ｍ（ｘ_ｍ－ｘ_{ｍ｜ｍ＋１}） （２３）
Ｐ_ｍ｜ｍ＝Ｐ_{ｍ｜ｍ＋１}－Ｋ_ｍＰ_{ｍ｜ｍ＋１} （２４）
Ｐ_{ｍ｜ｍ＋１}は予測誤差共分散、Ｋ_ｍはカルマンゲインである。

このようにして追尾処理部４８により時間逆方向のカルマンフィルタを用いて時間領域ｍにおける測位値ｘ_ｍと時間領域ｍ＋１の時に上式（２０）で予測した時間領域ｍの予測値ｘ_{ｍ｜ｍ＋１}により得た平滑値ｘ_ｍ｜ｍは出力部４６において目標１００の推定位置を示す代表時刻ｔ_ｍでの追尾処理部４８の出力とし，追尾起点時間領域ｍ_ｓ以前の測位値ｘ_ｍを時系列順につなげ、追尾起点時間領域ｍ_ｓ以前における目標１００の推定軌跡を示す物体軌跡情報として得る。

一方、追尾処理部４８により得られた予測情報は予測情報読込部４４ｅにより読み込まれ、予測ゲート判定部４４ｆが当該予測情報に基づき隣接する時間領域ｍ内の方位測定値（θ_ｋ、φ_ｋ）に対する異常値判定である予測ゲート判定を行う。
予測情報読込部４４ｅは、説明の都合上、機能部として説明しているが、追尾処理部４８により得られた予測情報を、直接、予測ゲート判定部４４ｆが用いる。

予測ゲート判定部４４ｆは予測値ｘ_{ｋ｜ｍ＋１}を中心とした予測ゲート判定を行う。
予測ゲート判定部４４ｆは、追尾処理部４８による予測値ｘ_{ｋ｜ｍ＋１}と精度推定部４７により推定される測位精度Ｒ_ｍから推定される予測精度Ｐ_{ｋ｜ｍ＋１}によって定まる予測ゲートを設定し、時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定された予測ゲート内に存在すると正常値と判定し、時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）がいずれの予測ゲート内に存在しないと異常値と判定する。

以下に、時間逆方向のカルマンフィルタによる追尾処理を用いて予測ゲート判定部４４ｆが予測ゲート判定を行う方法を式で示す。
時間領域ｍ内のｋ番目の方位測定値（θ_ｋ、φ_ｋ）の残差Ｌ_ｋ´は、状態遷移行列Φ_{ｋ｜（ｍ，Ａｉ）}で測定時刻ｔ_ｋに変換した予測値ｘ_{ｋ｜ｍ＋１}は予測誤差共分散Ｐ_{ｋ｜ｍ＋１}によって定義される統計的距離で計算され、次式（２５）により表わされる。
予測誤差共分散Ｐ_{ｋ｜ｍ＋１}は次式（２７）により表わされる。
次式（２６）は状態遷移行列Φ_{ｋ｜ｍ＋１}を表わす。

上式（２８）は予測値と判定対象の方位測定値（θ_ｋ、φ_ｋ）の残差ｅ_ｋを示す。
上式（２９）は予測されるＤＯＡの残差誤差共分散を示す。
上式（３０）は時間領域ｍ内のｋ番目の方位測定値（θ_ｋ、φ_ｋ）の残差Ｌ_ｋ´の２乗を示す。

予測ゲート判定部４４ｆが予測ゲート判定で使用する予測ゲート中心の予測値ｘ_{ｍ｜ｍ＋１}は、測位ゲート判定部４４ｂが測位ゲート判定で使用する測位ゲート中心の仮測位値ｘ_ｋ，Ａｉとは異なり，既に測位ゲートによる異常値判定が施された方位測定値を基に測位処理をして得られるため信頼度が高い。
従って、信頼度設定部４４ｃは残差Ｌ_ｋ´が閾値α_２を超えるかによって、方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を再設定する。

すなわち、次式（３１）及び次式（３２）に示すように、ｉｆ内の残差Ｌ_ｋ´が閾値α_２以下である条件を満たすときに、方位測定誤差σ_θｋは正常値であるσ_θｋ´に、方位測定誤差σ_φｋは正常値であるσ_φｋ´に、ｉｆ内の残差Ｌ_ｋ´が閾値α_２を超えている条件を満たすときに、方位測定誤差σ_θｋは異常値であるσ_{θ，ｅｒｒ´}に、方位測定誤差σ_φｋは異常値であるσ_{φ，ｅｒｒ´}に再設定する。
再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）は、追尾起点時間領域ｍ_ｓの次の時間領域ｍからの測位処理部４５における測位処理に用いられる。

一方、追尾処理部４８は追尾起点時間領域ｍ_ｓを起点として時刻が経過する方向に隣接する時間領域の追尾処理は、時間順方向のカルマンフィルタを適用して測位値ｘ_ｍを求める。
すなわち，追尾処理部４８は、隣接する時間領域ｍにおける測位値ｘ_ｍと時間領域ｍ－１の時に予測した時間領域ｍの予測値ｘ_{ｍ｜ｍ－１}とを重み付け平均した、平滑値ｘ_ｍ｜ｍを得る。
予測値ｘ_{ｍ｜ｍ－１}と平滑値ｘ_ｍ｜ｍを求める追尾処理は、実質、時間逆方向のカルマンフィルタによる追尾処理と同様であり、通常の時間順方向のカルマンフィルタであるので、説明は省略する。

このようにして追尾処理部４８により時間順方向のカルマンフィルタを用いて隣接する時間領域ｍにおける測位値ｘ_ｍと時間領域ｍ－１の時に予測した時間領域ｍの予測値ｘ_{ｍ｜ｍ－１}の追尾処理により得た平滑値ｘ_ｍ｜ｍは出力部４６において目標１００の推定位置を示す代表時刻ｔ_ｍでの測位値ｘ_ｍとし、追尾起点時間領域ｍ_ｓ以降の測位値ｘ_ｍを時系列順につなげ、追尾起点時間領域ｍ_ｓ以降における目標１００の推定軌跡を示す物体軌跡情報として得る。

出力部４６は、追尾処理部４８により時間順方向のカルマンフィルタを用いて得た追尾起点時間領域ｍ_ｓ以降における目標１００の推定軌跡と、追尾処理部４８により時間逆方向のカルマンフィルタを用いて得た追尾起点時間領域ｍ_ｓより前における目標１００の推定軌跡をつなぎ、目標１００における目標観測時間の全時間領域における推定軌跡を示す物体軌跡情報として得る。

また、予測ゲート判定部４４ｆが行う予測ゲート判定も時間順方向のカルマンフィルタによる追尾処理を用いて行われ、予測ゲート判定部４４ｆが予測値ｘ_{ｋ｜ｍ－１}を中心とした予測ゲート判定を行う。
予測値ｘ_{ｋ｜ｍ－１}を中心とした予測ゲート判定は、実質、時間逆方向のカルマンフィルタによる予測値ｘ_{ｋ｜ｍ＋１}を中心とした予測ゲート判定と同じであるので、説明は省略する。

信頼度設定部４４ｃは、時間逆方向のカルマンフィルタによる再設定と同様に、予測ゲート判定部４４ｆによる予測ゲート判定により得た残差Ｌ_ｋ´が閾値α_２を超えるかによって、方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を再設定する。
時間逆方向のカルマンフィルタによる予測ゲート判定と同様にして得た再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）は、追尾起点時間領域ｍ_ｓ以降の時間領域ｍからの測位処理部４５における測位処理に用いられる。

次に、目標軌跡推定装置の動作を、主として異常値判定部４４Ａの動作について実施の形態１と異なる追尾処理を中心に、図９及び図１０を用いて説明する。
実施の形態１において動作説明したと同様に、図６に示したステップＳＴ１５までが行われる。

図６に示したステップＳＴ１２からステップＳＴ１５までが図９に示したステップＳＴ１０１及びステップＳＴ１０２に相当する。
すなわち、測位ゲート判定部４４ｂが時間領域ｍ毎に測位ゲート判定を行い、目標観測時間の全時間領域（以下、単に全時間領域と言う）における方位測定値（θ_ｋ、φ_ｋ）について、正常値か異常値かの判定が行われ、信頼度設定部４４ｃが全時間領域における方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を再設定する。

ステップＳＴ１０１及びステップＳＴ１０２は、測位ゲート判定部４４ｂと信頼度設定部４４ｃが、全時間領域に存在する全ての方位測定値（θ_ｋ、φ_ｋ）に対して測位ゲートＧを用いて正常値か異常値かの測位ゲート判定を行い、正常値と判定された方位測定値（θ_ｋ、φ_ｋ）は信頼度が高いとし、異常値と判定された方位測定値（θ_ｋ、φ_ｋ）は信頼度が低いとする信頼度の再設定ステップである。

ステップＳＴ１０３において、時間領域選択部４４ｄが全時間領域における測位ゲートＧを用いた測位ゲート判定において、信頼度が高いと判定されたセンサ組Ａ_ｉ´の仮測位値ｘ_{ｍ，Ａｉ´}同士の分散を計算する。
ステップＳＴ１０４において、時間領域選択部４４ｄにおいて計算された分散の推移をもとに正常な時間領域ｍを追尾起点時間領域ｍ_ｓとして選択する。

ここで、追尾起点時間領域ｍ_ｓとして、測位ゲート判定が正常に機能することによって測位値が真値に近いと判断できるような異常度の低い時間領域を選択する必要がある。
測位ゲート判定では、方位センサの方位測定値が一点あるいは一つの状態ベクトルに集中しているかを判定するために仮測位値ｘ_{ｍ，Ａｉ´}を算出していた。
よって、測位ゲートが正常に機能したかを確かめるには、各時間領域ｍで信頼度の高いセンサ組全ての仮測位値ｘ_{ｍ，Ａｉ´}同士の分散を評価すれば良く、この分散の大きさが時間領域における測位の異常度と言える。
全時間領域の異常度を評価した際に、追尾起点時間領域ｍ_ｓは、追尾処理のしやすさから、異常度が持続的に低い時間領域、あるいは、異常度が最も低い時間領域を選択する。

選択された追尾起点時間領域ｍ_ｓを起点として時刻が経過する方向、つまり順方向に隣接する時間領域の測位処理を行う場合は図９のステップＳＴ１０５に進み、追尾起点時間領域ｍ_ｓを起点として時刻が戻る方向、つまり逆方向に隣接する時間領域の測位処理を行う場合は図１０のステップＳＴ２０５に進む。

ステップＳＴ１０５に進み、追尾起点時間領域ｍ_ｓを起点として時間領域Ｍまでの処理が終了した後、ステップＳＴ２０５に進んでもよく、逆に、ステップＳＴ２０５に進み、追尾起点時間領域ｍ_ｓを起点として時間領域１までの処理が終了した後、ステップＳＴ１０５に進んでもよい。

図９に示すステップＳＴ１０５からステップＳＴ１１３は時間順方向のカルマンフィルタを適用したステップである。
ステップＳＴ１０５において、まず、起点として測位処理を行う時間領域ｍとして追尾起点時間領域ｍ_ｓを選択する。

測位処理部４５が、ステップＳＴ１０６において、時間領域ｍにおける方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する信頼度設定部４４ｃにより再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を読込み、ステップＳＴ１０７において、読み込んだ情報を用いて測位処理を行い、時間領域ｍにおける目標１００に対する測位値ｘ_ｍを算出する。

ステップＳＴ１０８において、精度推定部４７が時間領域ｍにおける測位値ｘ_ｍと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）と方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）から測位値ｘ_ｍの測位精度Ｒ_ｍを推定する。

ステップＳＴ１０９において、追尾処理部４８が、時間領域ｍに対して時間順方向のフィルタ処理を行い、平滑値ｘ_ｍ｜ｍと予測値ｘ_{ｍ＋１｜ｍ}を計算する。
計算して得た平滑値ｘ_ｍ｜ｍは目標１００の推定位置を示す代表時刻ｔ_ｍでの追尾処理の出力として用いられる。

ステップＳＴ１１０において、時間領域ｍが時間領域Ｍであると処理を終了し、時間領域ｍが時間領域Ｍでないと、ステップＳＴ１１１に進み、時間領域ｍを次の時間領域ｍ＋１にしてステップＳＴ１１２に進む。
ステップＳＴ１１２において、予測ゲート判定部４４ｆが、予測情報読込部４４ｅに読み込まれた予測値ｘ_{ｋ｜ｍ－１}を中心とした予測ゲート判定を行う。
ステップＳＴ１１２における予測ゲート判定部４４ｆによる予測ゲート判定は、予測値ｘ_{ｋ｜ｍ－１}に基づき更新された時間領域ｍ内の方位測定値（θ_ｋ、φ_ｋ）に対する異常値判定である予測ゲート判定を行う。

ステップＳＴ１１３において、信頼度設定部４４ｃが、予測ゲート判定において異常値と判定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）の信頼度を下げた方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）に再設定し、正常値と判定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を信頼度が高いそのままに再設定し、ステップＳＴ１０５に進む。

ステップＳＴ１０５において、ステップＳＴ１１１で更新された時間領域ｍを選択し、ステップＳＴ１０６からステップＳＴ１１３のステップをステップＳＴ１１０において時間領域ｍが時間領域Ｍになるまで繰り返される。
このようにして、追尾起点時間領域ｍ_ｓから時間領域Ｍまでの代表時刻ｔ_ｍでの測位値ｘ_ｍが得られる。

測位処理部４５における測位処理は、時間領域ｍに対して隣接する時間領域ｍ－１の方位測定値（θ_ｋ、φ_ｋ）に対する予測ゲート判定が行われ、再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を追尾起点時間領域ｍ_ｓから時間領域Ｍまでの全ての時間領域ｍに用いられる。

図１０に示すステップＳＴ２０５からステップＳＴ２１３は時間逆方向のカルマンフィルタを適用したステップである。
ステップＳＴ２０５において、まず、起点として測位処理を行う時間領域ｍとして追尾起点時間領域ｍ_ｓを選択する。

測位処理部４５が、ステップＳＴ２０６において、時間領域ｍにおける方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する信頼度設定部４４ｃにより再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）と測定時刻ｔ_ｋと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）を読込み、ステップＳＴ２０７において、読み込んだ情報を用いて測位処理を行い、時間領域ｍにおける目標１００に対する測位値ｘ_ｍを算出する。

ステップＳＴ２０８において、精度推定部４７が時間領域ｍにおける測位値ｘ_ｍと方位センサ１０の位置情報（ｘ^ｒ _ｋ、ｙ^ｒ _ｋ、ｚ^ｒ _ｋ）と方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）から測位値ｘ_ｍの測位精度Ｒ_ｍを推定する。

ステップＳＴ２０９において、追尾処理部４８が、時間領域ｍに対して時間逆方向のフィルタ処理を行い、平滑値ｘ_ｍ｜ｍと予測値ｘ_{ｍ－１｜ｍ}を計算する。予測値ｘ_{ｍ－１｜ｍ}は上式（１７）により求められ、平滑値ｘ_ｍ｜ｍは上式（２３）により求められる。
計算して得た平滑値ｘ_ｍ｜ｍは目標１００の推定位置を示す代表時刻ｔ_ｍでの追尾処理の出力として用いられる。

ステップＳＴ２１０において、時間領域ｍが時間領域１であると処理を終了し、時間領域ｍが時間領域１でないと、ステップＳＴ２１１に進み、時間領域ｍを次の時間領域ｍ－１にしてステップＳＴ２１２に進む。
ステップＳＴ２１２において、予測ゲート判定部４４ｆが、予測情報読込部４４ｅに読み込まれた予測値ｘ_{ｋ｜ｍ＋１}を中心とした予測ゲート判定を行う。
ステップＳＴ２１２における予測ゲート判定部４４ｆによる予測ゲート判定は、予測値ｘ_{ｋ｜ｍ＋１}に基づき更新された時間領域ｍ内の方位測定値（θ_ｋ、φ_ｋ）に対する異常値判定である予測ゲート判定を行う。

ステップＳＴ２１３において、信頼度設定部４４ｃが、予測ゲート判定において異常値と判定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）の信頼度を下げた方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）に再設定し、正常値と判定された方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を信頼度が高いそのままに再設定し、ステップＳＴ２０５に進む。

ステップＳＴ２０５において、ステップＳＴ２１１で更新された時間領域ｍを選択し、ステップＳＴ２０６からステップＳＴ２１３のステップをステップＳＴ２１０において時間領域ｍが時間領域１になるまで繰り返される。
このようにして、時間領域１から追尾起点時間領域ｍ_ｓまでの代表時刻ｔ_ｍでの測位値ｘ_ｍが得られる。

測位処理部４５における測位処理は、時間領域ｍに対して隣接する時間領域ｍ＋１の方位測定値（θ_ｋ、φ_ｋ）に対する予測ゲート判定が行われ、再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を追尾起点時間領域ｍ_ｓから時間領域１までの全ての時間領域ｍに用いられる。

出力部４６が、追尾処理部４８がステップＳＴ１０９及びステップＳＴ２０９において得た目標１００の推定位置を示す時間領域１から時間領域Ｍまでの代表時刻ｔ_ｍでの測位値ｘ_１から測位値ｘ_Ｍを、時系列順に全てつなげ、目標１００の推定軌跡を示す物体軌跡情報として出力装置５０に出力する。

目標軌跡推定装置は、実施の形態１に係る目標軌跡推定装置と同様に、図７に示すコンピュータによるハードウェア構成により実現される。
入力インタフェース４０３は図７に示す方位情報読込部４１に該当し、出力インタフェース４０４は図７に示す出力部４６に該当する。
図７に示す時間領域分割部４３と異常値判定部４４Ａと測位処理部４５と精度推定部４７と追尾処理部４８と出力部４６の一部それぞれの機能は、プロセッサ４０１がメモリ４０２に格納されたプログラムをロードし、ロードしたプログラムに従って動作することによって実現される。

ステップＳＴ１からステップＳＴ１４、ステップＳＴ１０１からステップ１１３、及びステップＳＴ２０１からステップ２１３を含む目標軌跡推定装置の目標軌跡推定方法はプロセッサ４０１がメモリ４０２に格納されたプログラムに従って処理を実行することにより行われる。
すなわち、メモリ４０２に格納されたプログラムは、「目標１００からの到来電波を受信した複数の方位センサ１０それぞれが目標１００を観測した時間を時系列順に複数の時間領域ｍに分割し、複数の方位センサ１０それぞれにより得られる方位測定値（θ_ｋ、φ_ｋ）を示す方位情報を分割した時間領域ｍ毎に分割する手順と、複数の方位センサ１０から一定台数の方位センサ１０を選択して複数のセンサ組に組わけし、複数のセンサ組それぞれにおいて、分割された時間領域ｍ毎に仮測位値ｘ_ｍ，Ａｉを推定し、当該得られた仮測位値ｘ_ｍ，Ａｉによって定まる測位ゲートＧを設定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定された測位ゲートＧ内に存在すると正常値と判定し、当該時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定されたいずれの測位ゲートＧ内に存在しないと異常値と判定する手順と、異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再設定する手順と、分割された時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて目標１００に対する各時間領域ｍにおける測位値を推定する手順と、推定された時間領域ｍ毎の測位値を時系列につなげて目標１００の軌跡を示す物体軌跡情報を出力する手順と、正常値又は異常値を判定する手順において、信頼度が高いと判定されたセンサ組の仮測位値ｘ_ｍ，Ａｉ同士の分散を計算し、計算された分散の推移を基に追尾起点時間領域ｍ_ｓを選択する手順と、時間順方向のカルマンフィルタにより、追尾起点時間領域ｍ_ｓを起点に分割された時間領域ｍにおける推定される測位値と当該測位値に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて、推定される測位値に対する測位精度を推定する手順と、時間順方向のカルマンフィルタにより、追尾起点時間領域ｍ_ｓを起点に分割された時間領域ｍにおける推定される測位値と当該測位値に対応する測位精度を用いて予測値と平滑値からなる測位値を得る手順と、追尾起点時間領域ｍ_ｓを起点に分割された時間領域ｍにおける平滑値からなる測位値と当該測位値に対応する測位精度から推定される予測精度によって定まる予測ゲートを設定し、時間経過方向の隣接時間領域ｍ＋１に存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定された予測ゲート内に存在すると正常値と判定し、当該時間領域に存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）がいずれの予測ゲート内に存在しないと異常値と判定する手順と、追尾起点時間領域ｍ_ｓを起点に分割された一つ後の時間領域ｍ＋１における再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）の内、いずれの予測ゲート内に存在しないとして異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再度設定する手順と、時間逆方向のカルマンフィルタにより、追尾起点時間領域ｍ_ｓを起点に分割された時間領域ｍと一つ前の時間領域ｍ－１それぞれにおける推定される測位値と当該測位値に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて、推定される測位値に対する測位精度を推定する手順と、時間逆方向のカルマンフィルタにより、追尾起点時間領域ｍ_ｓを起点に分割された時間領域ｍにおける推定される測位値と当該測位値に対応する測位精度を用いて予測値と平滑値からなる測位値を得る手順と、追尾起点時間領域ｍ_ｓを起点に分割された時間領域ｍにおける平滑値からなる測位値と当該測位値に対応する測位精度から推定される予測精度によって定まる予測ゲートを設定し、時間逆方向の隣接時間領域ｍ－１に存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）が当該設定された予測ゲート内に存在すると正常値と判定し、当該時間領域に存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）がいずれの予測ゲート内に存在しないと異常値と判定する手順と、追尾起点時間領域ｍ_ｓを起点に分割された一つ前の時間領域ｍ－１における再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）の内、いずれの予測ゲート内に存在しないとして異常値と判定される方位測定値（θ_ｋ、φ_ｋ）に付随する方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を当該方位測定値（θ_ｋ、φ_ｋ）の軌跡推定に対する信頼度を下げる設定に再度設定する手順と、分割された時間領域ｍ毎に、各時間領域ｍに存在する方位情報が示す方位測定値（θ_ｋ、φ_ｋ）と当該方位測定値（θ_ｋ、φ_ｋ）に付随する再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を再度設定した方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて目標に対する各時間領域ｍにおける測位値を推定する手順と、時間領域ｍ毎の平滑値からなる測位値を時系列につなげて目標１００の軌跡を示す物体軌跡情報を出力する手順と、を備える。

以上のように、実施の形態２に係る目標軌跡推定装置４０Ａは、実施の形態１に係る目標軌跡推定装置４０と同様の効果を有する他、時間領域分割部４３により分割された時間領域ｍ毎に、異常値判定部４４Ａにより再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を用いて、測位処理部４５により推定される測位値ｘ_ｍに対する測位精度Ｒ_ｍを推定する精度推定部４７と、測位処理部４５により推定される測位値ｘ_ｍと当該測位値ｘ_ｍに対応する精度推定部４７により推定される測位精度Ｒ_ｍを用いて当該測位値ｘ_ｍをフィルタリングして平滑値と予測値を得る追尾処理部４８とをさらに備え、異常値判定部４４Ａは、追尾処理部４８による予測値により、再設定された方位測定誤差（σ_θ，ｋ、σ_φ，ｋ）を再度設定するので、２つの異なる相補的な異常値の判定により、より精度高くなり、信頼度の高い測位値を用いた目標１００の軌跡推定を行うことができる。
特に、軌跡推定が困難なマルチパス環境が目標周辺にあっても軌跡推定を行うことができる。

なお、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

本開示に係る目標軌跡推定装置は、飛行機などの移動物体の目標の軌跡を推定する目標軌跡推定システムに用いられる目標軌跡推定装置に好適である。

１０、１０_１～１０_Ｎと複数の方位センサ、２０_１～２０_Ｎ 複数の方位測定部、３０ 記憶装置、４０ 目標軌跡推定装置、４１ 方位情報読込部、４２ 誤差設定部、４３ 時間領域分割部、４４ 異常値判定部、４４ｂ 測位ゲート判定部、４４ｃ 信頼度設定部、４４ｆ 予測ゲート判定部、４５ 測位処理部、４６ 出力部、４７ 精度推定部、４８ 追尾処理部、５０ 出力装置、１００ 目標。

Claims (15)

1. 目標からの到来電波を受信した複数の方位センサそれぞれにより得られる方位測定値を示す方位情報、及び当該方位情報が示す方位測定値に付随する方位測定誤差を示す方位測定誤差情報を読み込む方位情報読込部と、
   前記複数の方位センサそれぞれが前記目標を観測した時間を時系列順に複数の時間領域に分割し、前記方位情報読込部が読み込む方位情報を前記分割された時間領域毎に分割する時間領域分割部と、
   前記時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値が異常値を示すか否かを判定し、異常値と判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再設定し、異常値でないと判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度をそのままに再設定する異常値判定部と、
   前記時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値と当該方位測定値に付随する再設定された方位測定誤差を用いて前記目標に対する各時間領域における測位値を推定する測位処理部と、
   前記測位処理部により推定された時間領域毎の測位値を時系列につなげて前記目標の軌跡を示す物体軌跡情報を出力する出力部と、
   を備える目標軌跡推定装置。
2. 目標からの到来電波を受信した複数の方位センサそれぞれにより得られる方位測定値を示す方位情報、及び当該方位情報が示す方位測定値に付随する、あるいは、一様に設定される方位測定誤差を示す方位測定誤差情報を読み込む方位情報読込部と、
   前記複数の方位センサそれぞれが前記目標を観測した時間を時系列順に複数の時間領域に分割し、前記方位情報読込部が読み込む方位情報を前記分割された時間領域毎に分割する時間領域分割部と、
   前記時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値が示す位置が一点に集中しているか、あるいは、位置と速度からなる状態ベクトルの周辺に集中するかを基に、当該方位測定値が異常値を示すか否かを判定し、異常値と判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再設定し、異常値でないと判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度をそのままに再設定する異常値判定部と、
   前記時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値と当該方位測定値に付随する再設定された方位測定誤差を用いて前記目標に対する各時間領域における測位値を推定する測位処理部と、
   前記測位処理部により推定された時間領域毎の測位値を時系列につなげて前記目標の軌跡を示す物体軌跡情報を出力する出力部と、
   を備える目標軌跡推定装置。
3. 前記異常値判定部における方位測定値が異常値を示すか否かの判定は、前記複数の方位センサから一定台数の方位センサを選択して複数のセンサ組に組わけし、前記複数のセンサ組それぞれにおいて、前記時間領域分割部により分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値と当該方位測定値に付随する方位測定誤差を用いて測位処理を行ない前記目標に対するセンサ組における仮測位値を得、当該仮測位値と当該仮測位値に対応する前記方位センサの位置及び前記方位測定誤差を用いてセンサ組における仮測位精度を推定し、当該時間領域に存在する方位情報が示す方位測定値が前記仮測位値と前記仮測位精度によって定まる測位ゲート内に存在するかを判定することにより当該方位測定値の信頼度を評価し、当該評価した信頼度が高い方位測定値を正常値、当該評価した信頼度が低い方位測定値を異常値とする判定である請求項２に記載の目標軌跡推定装置。
4. 前記異常値判定部における異常値と判定した方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定は、異常値と判定した方位測定値に付随する方位測定誤差が示す値を異常値でないと判定される方位測定値に付随する方位測定誤差が示す値より大きい値のパラメータ値に再設定することである請求項２に記載の目標軌跡推定装置。
5. 前記異常値判定部におけるセンサ組における仮測位精度の推定は、前記センサ組における仮測位値と対応する前記方位センサの位置と誤差設定部によって設定される方位測定誤差を用いて前記仮測位値が理論的に取り得る分散の下限値（ＢＣＲＢ：Bayesian Cramer-Rao Boundary）を計算することにより行う請求項３に記載の目標軌跡推定装置。
6. 前記異常値判定部は、
   前記複数の方位センサから一定台数の方位センサを選択して複数のセンサ組に組わけし、前記複数のセンサ組それぞれにおいて、前記時間領域分割部により分割された時間領域毎に仮測位値を推定し、当該得られた仮測位値によって定まる測位ゲートを設定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定された測位ゲート内に存在すると正常値と判定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定されたいずれの測位ゲート内に存在しないと異常値と判定する測位ケート判定部と、
   前記測位ケート判定部により異常値と判定される方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再設定する信頼度設定部と、
   備える請求項２に記載の目標軌跡推定装置。
7. 前記測位ケート判定部で行う仮測位値の推定は、重み付き最小二乗推定による方法又はＭＡＰ（Maximum a Posteriori）推定による方法のいずれかの方法により行う請求項６に記載の目標軌跡推定装置。
8. 前記異常値判定部における方位測定値が異常値を示すか否かの判定は、前記複数の方位センサから一定台数の方位センサを選択して複数のセンサ組に組わけし、前記複数のセンサ組それぞれにおいて、前記時間領域分割部により分割された時間領域毎に仮測位値と仮測位精度を推定し、当該得られた仮測位値が、当該得られた仮測位値と仮測位精度によって定まる測位ゲート内に存在するかを判定することにより信頼度を評価し、当該評価した信頼度が高いセンサ組毎に仮測位値同士の分散の大きさが時間領域における測位の異常度とする請求項２に記載の目標軌跡推定装置。
9. 前記時間領域分割部により分割された時間領域毎に、前記異常値判定部により再設定された方位測定誤差を用いて、前記測位処理部により推定される測位値に対する測位精度を推定する精度推定部と、
   前記測位処理部により推定される測位値と当該測位値に対応する前記精度推定部により推定される測位精度を用いて当該測位値をフィルタリングして平滑値と予測値を得る追尾処理部と、をさらに備え、
   前記異常値判定部は、前記追尾処理部による予測値により、再設定された方位測定誤差を再度設定する、
   請求項１から請求項８のいずれか１項に記載の目標軌跡推定装置。
10. 前記追尾処理部は、前記異常値判定部により異常値でないと判定した方位測定値が存在する時間領域を１つ選択し、選択した時間領域である追尾起点時間領域を起点とした時間順方向のカルマンフィルタにより、前記追尾起点時間領域の１つ後の時間領域における予測値と平滑値を求め、前記追尾起点時間領域を起点とした時間逆方向のカルマンフィルタにより、前記追尾起点時間領域の１つ前の時間領域における予測値と平滑値を求め、
    前記出力部により用いられる前記追尾処理部により推定された時間領域毎の平滑値を目標軌跡推定装置の出力とした、
    請求項９に記載の目標軌跡推定装置。
11. 前記追尾処理部による追尾起点時間領域の選択は、時間領域内に存在する方位測定値の分散が小さい時間が続いている時間領域又は分散が最小である時間領域を選択する請求項１０に記載の目標軌跡推定装置。
12. 前記追尾処理部による追尾起点時間領域の選択は、前記複数の方位センサから一定台数の方位センサを選択して複数のセンサ組に組わけし、前記複数のセンサ組それぞれにおいて、前記時間領域分割部により分割された時間領域毎に仮測位値と仮測位精度を推定し、当該得られた仮測位値が、当該得られた仮測位値と仮測位精度によって定まる測位ゲート内に存在するかを判定することにより信頼度を評価し、当該評価した信頼度が高いセンサ組毎に仮測位値同士の分散の大きさが時間領域における測位の異常度とし、異常度が設定値より持続的に低い時間領域、又は異常度が最も低い時間領域を選択する請求項１０に記載の目標軌跡推定装置。
13. 前記異常値判定部による再設定された方位測定誤差の再度の設定は、前記時間領域分割部により分割された時間領域毎に、前記追尾処理部による予測値と前記精度推定部により推定される測位精度から推定される予測精度によって定まる予測ゲートを設定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定された予測ゲート内に存在すると正常値と判定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定されたいずれの予測ゲート内に存在しないと異常値と判定することにより行われる、請求項１０に記載の目標軌跡推定装置。
14. 前記時間領域分割部により分割された時間領域毎に、前記異常値判定部における前記信頼度設定部により再設定された方位測定誤差を用いて、前記測位処理部により推定される測位値に対する測位精度を推定する精度推定部と、
    前記時間領域分割部により分割された隣接する時間領域それぞれにおける前記測位処理部により推定される測位値と当該測位値に付随する前記精度推定部により推定される測位精度を用いてカルマンフィルタによりフィルタリングして予測値と平滑値を得る追尾処理部と、をさらに備え、
    前記異常値判定部は、前記追尾処理部による予測値と前記精度推定部により推定される測位精度から推定される予測精度によって定まる予測ゲートを設定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定された予測ゲート内に存在すると正常値と判定し、当該時間領域に存在する方位情報が示す方位測定値がいずれの予測ゲート内に存在しないと異常値と判定する予測ゲート判定部を備え、
    前記異常値判定部における前記信頼度設定部は、再設定された方位測定誤差の内、前記予測ゲート判定部により異常値と判定される方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再度設定し、
    前記出力部により用いられる前記追尾処理部により推定された時間領域毎の平滑値を目標軌跡推定装置の出力とした、
    請求項６に記載の目標軌跡推定装置。
15. 時間領域分割部と、測位ケート判定部及び信頼度設定部を有する異常値判定部と、測位処理部と、出力部を備える目標軌跡推定装置による目標の測位値を測定する目標軌跡推定方法であって、
    前記時間領域分割部が、前記目標からの到来電波を受信した複数の方位センサそれぞれが前記目標を観測した時間を時系列順に複数の時間領域に分割し、前記複数の方位センサそれぞれにより得られる方位測定値を示す方位情報を前記分割された時間領域毎に分割するステップと、
    前記異常値判定部における測位ケート判定部が、前記複数の方位センサから一定台数の方位センサを選択して複数のセンサ組に組わけし、前記複数のセンサ組それぞれにおいて、前記分割された時間領域毎に仮測位値を推定し、当該得られた仮測位値によって定まる測位ゲートを設定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定された測位ゲート内に存在すると正常値と判定し、当該時間領域に存在する方位情報が示す方位測定値が当該設定されたいずれの測位ゲート内に存在しないと異常値と判定するステップと、
    前記異常値判定部における信頼度設定部が、前記異常値と判定される方位測定値に付随する方位測定誤差を当該方位測定値の軌跡推定に対する信頼度を下げる設定に再設定するステップと、
    前記測位処理部が前記分割された時間領域毎に、各時間領域に存在する方位情報が示す方位測定値と当該方位測定値に付随する再設定された方位測定誤差を用いて前記目標に対する各時間領域における測位値を推定するステップと、
    前記出力部が前記推定された時間領域毎の測位値を時系列につなげて前記目標の軌跡を示す物体軌跡情報を出力するステップと、
    を備える目標軌跡推定方法。

Images