(57)【要約】【課題】バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制できる追尾処理装置を得ること。【解決手段】追尾処理装置１００は、観測情報を基に観測誤差共分散行列を演算する第１の演算部２０、前回の観測時刻における平滑値と、前回の観測時刻における平滑誤差共分散行列と、今回の観測時刻における観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値及び今回の観測時刻における予測誤差共分散行列を演算する第２の演算部３０、観測情報と、予測値とを基に、観測誤差共分散行列及び前記予測誤差共分散行列のうちの何れかを補正する補正処理部４０、並びに、観測情報、平滑値、補正された観測誤差共分散行列、及び補正されていない予測誤差共分散行列４０ｂ、又は、観測情報、平滑値、補正されていない観測誤差共分散行列、及び補正された予測誤差共分散行列に基づいて、平滑値及び平滑誤差共分散行列を演算する第３の演算部５０を備える。【選択図】図１
2019124637.tif 000002
【特許請求の範囲】
【請求項１】
複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理装置であって、
前記観測情報を基に、観測誤差共分散行列を演算する第１の演算部と、
前回の観測時刻における平滑値と、前回の観測時刻における平滑誤差共分散行列と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列とを演算する第２の演算部と、
前記観測情報と、前記予測値とを基に、前記観測誤差共分散行列及び前記予測誤差共分散行列のうちの何れかを補正する補正処理部と、
前記観測情報、前記平滑値、補正された前記観測誤差共分散行列、及び補正されていない前記予測誤差共分散行列、又は、前記観測情報、前記平滑値、補正されていない前記観測誤差共分散行列、及び補正された前記予測誤差共分散行列に基づいて、前記平滑値及び前記平滑誤差共分散行列を演算する第３の演算部と、
を備えたことを特徴とする追尾処理装置。
【請求項２】
前記観測情報には、前記目標の運動諸元に関する観測値と、前記運動諸元を観測したときの観測時刻とが含まれることを特徴とする請求項１に記載の追尾処理装置。
【請求項３】
前記補正処理部は、複数のセンサ間にバイアス誤差が存在するか否かを判定し、前記バイアス誤差が存在すると判定した場合には、前記観測誤差共分散行列及び前記予測誤差共分散行列のうちの何れかを補正することを特徴とする請求項１又は２に記載の追尾処理装置。
【請求項４】
前記補正処理部は、複数のセンサ間に時刻同期誤差が存在するか否かを判定し、前記時刻同期誤差が存在すると判定した場合には、前記予測誤差共分散行列を補正することを特徴とする請求項３に記載の追尾処理装置。
【請求項５】
前記補正処理部は、前記予測誤差共分散行列を補正する際に、前記予測誤差共分散行列における各要素の中で誤差の広がりが最も小さい要素を補正することを特徴とする請求項４に記載の追尾処理装置。
【請求項６】
前記補正処理部は、複数のセンサ間に時刻同期誤差が存在するか否かを判定し、前記時刻同期誤差が存在しないと判定した場合には、前記観測誤差共分散行列を補正することを特徴とする請求項３に記載の追尾処理装置。
【請求項７】
前記補正処理部は、前記観測誤差共分散行列を補正する際に、前記観測誤差共分散行列における各要素の中で誤差の広がりが最も小さい要素を補正することを特徴とする請求項６に記載の追尾処理装置。
【請求項８】
複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理方法であって、
前記観測情報を基に、観測誤差共分散行列を演算する第１のステップと、
前回の観測時刻における平滑値と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列とを演算する第２のステップと、
前記観測情報と、前記予測値とを基に、前記観測誤差共分散行列を補正する第３のステップと、
前記観測情報と、前記予測値と、前記予測誤差共分散行列と、補正後の前記観測誤差共分散行列とを基に、前記平滑値を演算する第４のステップと、
を含むことを特徴とする追尾処理方法。
【請求項９】
複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理方法であって、
前記観測情報を基に、観測誤差共分散行列を演算する第１のステップと、
前回の観測時刻における平滑値と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列とを演算する第２のステップと、
前記観測情報と、前記予測値とを基に、前記予測誤差共分散行列を補正する第３のステップと、
前記観測情報と、前記予測値と、前記観測誤差共分散行列と、補正後の前記予測誤差共分散行列とを基に、前記平滑値を演算する第４のステップと、
を含むことを特徴とする追尾処理方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、目標の追尾処理を行う追尾処理装置及び追尾処理方法に関する。
【背景技術】
【０００２】
追尾処理装置は、センサの観測情報を基に、目標の運動諸元を推定しつつ、観測時刻毎に当該運動諸元を逐次更新して行く装置である。目標の運動諸元には、目標位置、目標速度及び目標加速度が含まれる。センサの観測情報には、目標の運動諸元に関する観測値、及び目標の運動諸元を観測したときの観測時刻が含まれる。
【０００３】
下記特許文献１及び特許文献２には、複数のセンサの観測情報を用いて目標の追尾処理を行う際に、センサ間のバイアス誤差を推定して目標の追尾処理に関する推定精度を向上させる技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１１−６４４８４号公報
【特許文献２】特開２０１２−４７４９０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１及び特許文献２に代表される従来技術では、センサ間のバイアス誤差を推定するためには、一定時間、目標の観測情報を蓄積する必要がある。すなわち、従来技術では、複数のセンサの観測情報を用いて追尾処理を行う際には、一定時間の経過を待たなければ追尾処理の演算を行うことができず、追尾処理が遅延し、追尾精度が低下するという問題があった。
【０００６】
本発明は、上記に鑑みてなされたものであって、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することができる追尾処理装置を得ることを目的とする。
【課題を解決するための手段】
【０００７】
上述した課題を解決し、目的を達成するために、本発明は、複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理装置である。追尾処理装置は、観測情報を基に、観測誤差共分散行列を演算する第１の演算部、前回の観測時刻における平滑値と、前回の観測時刻における平滑誤差共分散行列と、今回の観測時刻における観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値及び今回の観測時刻における予測誤差共分散行列を演算する第２の演算部、観測情報と、予測値とを基に、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正する補正処理部、並びに、観測情報、平滑値、補正された観測誤差共分散行列、及び補正されていない予測誤差共分散行列、又は、観測情報、平滑値、補正されていない観測誤差共分散行列、及び補正された予測誤差共分散行列に基づいて、平滑値及び平滑誤差共分散行列を演算する第３の演算部を備える。
【発明の効果】
【０００８】
本発明によれば、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することができるという効果を奏する。
【図面の簡単な説明】
【０００９】
【図１】実施の形態に係る追尾処理装置の構成を示すブロック図
【図２】実施の形態における第１の演算部の動作説明に供する図
【図３】実施の形態における第２の演算部の動作説明に供する図
【図４】実施の形態における第３の演算部の動作説明に供する第１の図
【図５】実施の形態における第３の演算部の動作説明に供する第２の図
【図６】実施の形態に係る追尾処理装置における要部の処理の流れを示すフローチャート
【図７】実施の形態の補正処理部における補正処理の詳細説明に供する第１の図
【図８】実施の形態の補正処理部における補正処理の詳細説明に供する第２の図
【図９】実施の形態の追尾処理装置におけるハードウェア構成の一例を示すブロック図
【図１０】実施の形態の追尾処理装置におけるハードウェア構成の他の例を示すブロック図
【発明を実施するための形態】
【００１０】
以下に添付図面を参照し、本発明の実施の形態に係る追尾処理装置及び追尾処理方法について詳細に説明する。なお、以下の実施の形態により、本発明が限定されるものではない。
【００１１】
実施の形態．
図１は、実施の形態に係る追尾処理装置１００の構成を示すブロック図である。実施の形態に係る追尾処理装置１００は、複数のセンサからの観測情報を基に、目標の追尾処理を行う装置である。実施の形態に係る追尾処理装置１００は、図１に示すように、第１の演算部２０、第２の演算部３０、及び第３の演算部５０を備えると共に、第２の演算部３０と第３の演算部５０との間に補正処理部４０を備えている。なお、第１の演算部２０、第２の演算部３０及び第３の演算部５０は機能区分であり、機能区分の変更に伴って図１とは異なる構成を採ってもよい。
【００１２】
図１に示すように、追尾処理装置１００には、第１のセンサ１０及び第２のセンサ１１からの観測情報が入力される。第１のセンサ１０及び第２のセンサ１１は複数のセンサの例示である。すなわち、追尾処理装置１００は、３つ以上のセンサからの観測情報が入力される構成であってもよい。前述したように、観測情報には、目標の運動諸元に関する観測値と、目標の運動諸元を観測したときの観測時刻とが含まれる。以下、「目標の運動諸元」を「目標運動諸元」と呼ぶ。なお、観測値とは、センサが観測した観測情報の値である。
【００１３】
追尾処理装置１００の外部には、表示装置１１０が備えられている。表示装置１１０は、表示手段の一例である。追尾処理装置１００の演算結果は、表示装置１１０に出力される。追尾処理装置１００の演算結果は、図１では図示しない記憶装置に出力されてもよい。
【００１４】
次に、追尾処理装置１００を構成する各構成部間の信号又は情報の流れについて説明する。
【００１５】
第１のセンサ１０は、目標８０を観測する。第１のセンサ１０による観測値１０ａは、第１の演算部２０、補正処理部４０及び第３の演算部５０に伝送される。観測値１０ａには、第１のセンサ１０が観測した目標運動諸元に関する情報と、第１のセンサ１０が目標８０を観測したときの観測精度に関する情報とが含まれる。第２の演算部３０には、第１のセンサ１０が目標８０を観測したときの観測時刻１０ｂに関する情報が第１のセンサ１０から伝送される。
【００１６】
第２のセンサ１１も第１のセンサ１０と同様に、目標８０を観測する。第１のセンサ１０が観測する目標と第２のセンサ１１が観測する目標とは、同じ目標である場合もあれば、異なる目標である場合もある。第２のセンサ１１による観測値１１ａは、第１の演算部２０、補正処理部４０及び第３の演算部５０に伝送される。観測値１１ａには、第２のセンサ１１が観測した目標運動諸元に関する情報と、第２のセンサ１１が目標８０を観測したときの観測精度に関する情報とが含まれる。第２の演算部３０には、第２のセンサ１１が目標８０を観測したときの観測時刻１１ｂに関する情報が第２のセンサ１１から伝送される。
【００１７】
第１の演算部２０には、上述した観測値１０ａ及び観測値１１ａが入力される。第１の演算部２０は、観測値１０ａ及び観測値１１ａの情報を基に、観測誤差共分散行列２０ａを演算する。観測誤差共分散行列２０ａの詳細については後述する。第１の演算部２０が演算した観測誤差共分散行列２０ａは、補正処理部４０及び第３の演算部５０に伝送される。
【００１８】
第２の演算部３０には、上述した観測時刻１０ｂ及び観測時刻１１ｂに加え、第３の演算部５０が演算した平滑値５０ａ及び平滑誤差共分散行列５０ｂが入力される。平滑値５０ａ及び平滑誤差共分散行列５０ｂは、前回の観測時刻で取得した観測情報を基に演算された値である。平滑値５０ａ及び平滑誤差共分散行列５０ｂの詳細については後述する。なお、演算処理は、観測時刻を基準に行われる。このため、「観測時刻」と「演算時刻」とは同義とし、本明細書では、「観測時刻」という用語を使用する。
【００１９】
第２の演算部３０は、前回の観測時刻において演算された平滑値５０ａ及び平滑誤差共分散行列５０ｂの情報と、今回の観測時刻１０ｂ及び１１ｂの情報とを基に、今回の観測時刻における目標運動諸元の予測値３０ａと、今回の観測時刻における予測誤差共分散行列３０ｂとを演算する。予測値３０ａ及び予測誤差共分散行列３０ｂの詳細については後述する。第２の演算部３０が演算した予測値３０ａ及び予測誤差共分散行列３０ｂは、補正処理部４０及び第３の演算部５０に伝送される。
【００２０】
補正処理部４０には、観測値１０ａ及び観測値１１ａと、第１の演算部２０が演算した観測誤差共分散行列２０ａと、第２の演算部３０が演算した予測値３０ａ及び予測誤差共分散行列３０ｂとが入力される。補正処理部４０による補正処理は、観測値１０ａ、観測値１１ａ、観測誤差共分散行列２０ａ、予測値３０ａ及び予測誤差共分散行列３０ｂに関する各情報を基に行われる。当該補正処理では、第１の演算部２０が演算した観測誤差共分散行列２０ａと、第２の演算部３０が演算した予測誤差共分散行列３０ｂに対する補正処理の要否が判定される。
【００２１】
当該補正処理が必要と判定された場合、観測誤差共分散行列２０ａ、または、予測誤差共分散行列３０ｂに対する補正処理が実施される。一方、当該補正処理が不要と判定された場合、補正処理は実施されずに、第１の演算部２０により演算された観測誤差共分散行列２０ａと、第２の演算部３０により演算された予測誤差共分散行列３０ｂが、そのまま出力される。補正処理の詳細については後述する。
【００２２】
第３の演算部５０には、観測値１０ａ及び観測値１１ａと、第２の演算部３０が演算した予測値３０ａと、補正処理部４０が演算した補正処理後の観測誤差共分散行列４０ａもしくは補正処理前の観測誤差共分散行列４０ａと、補正処理後の予測誤差共分散行列４０ｂもしくは補正処理前の予測誤差共分散行列４０ｂとが入力される。第３の演算部５０は、これらの入力情報を基に、平滑値５０ａ及び平滑誤差共分散行列５０ｂを演算し、表示装置１１０に出力する。
【００２３】
次に、補正処理部４０を設ける理由について、図１から図５の図面を参照して説明する。図２は、第１の演算部２０の動作説明に供する図である。図３は、第２の演算部３０の動作説明に供する図である。図４は、第３の演算部５０の動作説明に供する第１の図である。図５は、第３の演算部５０の動作説明に供する第２の図である。
【００２４】
図２には、観測値が丸記号「○」で示され、当該観測値の観測誤差の広がりを表す概念を有する観測誤差共分散行列が一点鎖線で示されている。前述したように、観測誤差共分散行列は、第１の演算部２０によって演算される。観測誤差共分散行列は、図２の一点鎖線で示されるように、観測値に対して、破線で示されるセンサ視線方向にセンサの距離精度に応じた広がりと、センサの視線方向に直交する方向にセンサの角度精度に応じた広がりとを持って表される。すなわち、第１の演算部２０は、センサの観測精度である距離精度及び角度精度に応じた観測誤差共分散行列を演算することにより、目標状態の真値が存在すると考えられる範囲を推定している。
【００２５】
図３には、前回の観測時刻ｔｋ−１において演算された平滑値がプラス記号「＋」で示され、当該平滑値の推定誤差の広がりを表す概念を有する平滑誤差共分散行列が実線で示されている。また、図３には、今回の観測時刻ｔｋにおいて演算される予測値が白抜きの三角記号「△」で示され、当該予測値の予測誤差の広がりを表す概念を有する予測誤差共分散行列が破線で示されている。前述したように、予測値及び予測誤差共分散行列は、第２の演算部３０によって演算される。予測値及び予測誤差共分散行列は、前回の観測時刻ｔｋ−１において演算された平滑値及び平滑誤差共分散行列と、前回の観測時刻ｔｋ−１から今回の観測時刻ｔｋまでの経過時間Δｔ＝ｔｋ−ｔｋ−１とを基に演算される。すなわち、予測値は、前回の観測時刻ｔｋ−１で取得した観測情報を基に演算で求めた平滑値を用いて予測される目標運動諸元の予測値である。
【００２６】
図４は、センサ間にバイアス誤差が存在しない場合の図である。ここで、バイアス誤差とは、異なるセンサ間の観測値に存在する誤差である。バイアス誤差には、センサ間の位置誤差、及びセンサ間の角度誤差が含まれる。図４には、プラス記号「＋」で示される平滑値が、丸記号「○」で示される観測値と、白抜きの三角記号「△」で示される予測値とを用いて算出される様子が示されている。すなわち、平滑値は、観測値と予測値とから求められる目標運動諸元の推定値である。また、図４には、図２に示した観測誤差共分散行列が一点鎖線で示され、図３に示した予測誤差共分散行列が破線で示されている。図４を参照すると、平滑値は、観測値と予測値とを内分する値として求められている。すなわち、図４によれば、平滑値は、観測値と予測値とから精度よく求められる状況が理解できる。
【００２７】
図４に対して図５は、センサ間にバイアス誤差が存在する場合の図である。図５において、破線で示される予測誤差共分散行列は、図４に示すものと同一である。一方、図５において、二点鎖線で示される観測誤差共分散行列は、図４に示すものよりも紙面の左上部寄りに変位している。この変位は、バイアス誤差によるズレに起因するものである。バイアス誤差のズレによって、演算される観測値は丸記号で示される位置から太字の丸記号で示される位置に推移する。その結果、演算される平滑値はプラス記号で示される位置から太字のプラス記号で示される位置に推移する。このように、バイアス誤差のズレによって、目標運動諸元の推定精度は悪化してしまうことになる。
【００２８】
ここで、図１に示す構成から補正処理部４０を有さない追尾処理装置を仮定し、この追尾処理装置が複数センサの観測情報を処理する場合について考える。具体的には、第１のセンサ１０による目標８０の観測情報を基に追尾処理演算を実施後、次の追尾処理演算において、第２のセンサ１１による目標８０の観測情報を基に追尾処理演算を実施することを考える。
【００２９】
まず、一般的に、センサ間において、時刻同期が常に行われているわけではない。このため、各センサは絶対時刻を検知することができない。その結果、第１のセンサ１０から伝送される観測時刻と、第２のセンサ１１から伝送される観測時刻との間には、誤差が存在する。この観測時刻の誤差もバイアス誤差の一つであり、本明細書では「時刻同期誤差」と呼ぶ。よって、センサ間に存在するバイアス誤差には、上記特許文献１及び特許文献２に示されているセンサ間の位置誤差、及びセンサ間の角度誤差に、ここで定義した時刻同期誤差が加わる。その結果、第１のセンサ１０の観測情報を基に演算される予測値と、第２のセンサ１１の観測情報を基に演算される予測値との間には、センサ間のバイアス誤差が付加される。
【００３０】
よって、第３の演算部５０が、平滑値及び平滑誤差共分散行列を演算する際には、センサ間のバイアス誤差によって、図５に示すように、平滑値の推定精度が悪化してしまう。そこで、図１に示すように、第３の演算部５０の前段に補正処理部４０を設ける構成にしたものが、実施の形態に係る追尾処理装置１００である。
【００３１】
次に、実施の形態に係る追尾処理装置における演算処理の詳細について、図１、図６、図７、及び図８の図面、並びに下記に示す幾つかの数式を参照して説明する。図６は、実施の形態に係る追尾処理装置１００における要部の処理の流れを示すフローチャートである。図７は、補正処理部４０における補正処理の詳細説明に供する第１の図である。図８は、補正処理部４０における補正処理の詳細説明に供する第２の図である。
【００３２】
まず、各センサから伝送される観測値及び観測精度をベクトル量として、それぞれ「ｚ→ｋ」及び「ｖ→ｋ」で表す。観測値ベクトルｚ→ｋは、今回の観測時刻ｔｋにおいて観測された観測値を表すベクトルである。観測精度ベクトルｖ→ｋは、今回の観測時刻ｔｋにおいて観測された観測値の観測精度を表すベクトルである。ここで、「ｚ→ｋ」における「ｚ→」の表記は、「ｚ」の上部に矢印記号「→」が配置された文字の代替表記である。本明細書では、文書内においては、当該代替表記を使用する。
【００３３】
観測時刻ｔｋにおける観測値ベクトルｚ→ｋ、及び観測精度ベクトルｖ→ｋは、以下の（１−１）式、及び（１−２）式で表すことができる。ここで、観測値ベクトルｚ→ｋは、基準となる直交座標系である追尾基準座標系で定義されたベクトルとする。追尾基準座標系の一例は、北基準直交座標系である。また、観測精度ベクトルｖ→ｋは、センサを基準とする極座標系であるセンサ基準極座標系で定義されたベクトルとする。
【００３４】
【数１】
2019124637.tif 000003
【００３５】
上記（１−１）式において、「ｘｏｋ」は、観測時刻ｔｋにおける観測値のｘ軸成分である。「ｙｏｋ」は、観測時刻ｔｋにおける観測値のｙ軸成分である。「ｚｏｋ」は、観測時刻ｔｋにおける観測値のｚ軸成分である。記号「Ｔ」は、転置を表す。この表記は、後述の数式においても同様である。また、上記（１−２）式において、「σｒｋ」は、観測時刻ｔｋにおける観測精度の距離方向の成分である。「σｅｋ」は、観測時刻ｔｋにおける観測精度の仰角方向の成分である。「σｂｙｋ」は、観測時刻ｔｋにおける観測精度の方位角方向の成分である。なお、観測精度の距離方向の成分を「距離精度」と呼び、観測精度の仰角方向の成分を「仰角精度」と呼び、観測精度の方位角方向の成分を「方位角精度」と呼ぶ場合がある。
【００３６】
次いで、第１の演算部２０によって演算される観測誤差共分散行列を「Ｒｋ」で表す。観測誤差共分散行列Ｒｋは、目標運動諸元の真値が存在すると考えられる範囲を表す行列である。観測誤差共分散行列Ｒｋは、以下の（２）式で表すことができる。
【００３７】
【数２】
2019124637.tif 000004
【００３８】
上記（２）式に示されるように、観測誤差共分散行列Ｒｋは、上記（１−２）式に示した観測精度ベクトルｖ→ｋの距離精度σｒｋ、仰角精度σｅｋ及び方位角精度σｂｙｋの２乗成分を対角要素とする行列と、座標変換行列Ｇｋとを用いて表すことができる。座標変換行列Ｇｋは、センサ基準極座標系から基準となる直交座標系への変換行列である。
【００３９】
また、第２の演算部３０によって演算される観測時刻ｔｋにおける予測値のベクトルを「ｘ→ｋ｜ｋ−１」で表す。観測時刻ｔｋにおける予測値ベクトルｘ→ｋ｜ｋ−１は、以下の（３）式で表すことができる。
【００４０】
【数３】
2019124637.tif 000005
【００４１】
上記（３）式において、「ｘｋ｜ｋ−１」は、観測時刻ｔｋにおける位置予測値のｘ軸成分である。「ｙｋ｜ｋ−１」は、観測時刻ｔｋにおける位置予測値のｙ軸成分である。「ｚｋ｜ｋ−１」は、観測時刻ｔｋにおける位置予測値のｚ軸成分である。「ｘ・ｋ｜ｋ−１」は、観測時刻ｔｋにおける速度予測値のｘ軸成分である。「ｙ・ｋ｜ｋ−１」は、観測時刻ｔｋにおける速度予測値のｙ軸成分である。「ｚ・ｋ｜ｋ−１」は、観測時刻ｔｋにおける速度予測値のｚ軸成分である。ここで、「ｘ・ｋ｜ｋ−１」における「ｘ・」の表記は、「ｘ」の上部にドット記号「・」が配置された文字の代替表記である。「ｙ」及び「ｚ」についても同様である。本明細書では、イメージで挿入する数式を除き、当該代替表記を使用する。
【００４２】
また、第３の演算部５０によって演算される、前回の観測時刻ｔｋ−１における平滑値のベクトルを「ｘ→ｋ−１｜ｋ−１」で表す。前回の観測時刻ｔｋ−１における平滑値ベクトルｘ→ｋ−１｜ｋ−１は、以下の（４）式で表すことができる。
【００４３】
【数４】
2019124637.tif 000006
【００４４】
上記（４）式において、「ｘｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における位置平滑値のｘ軸成分である。「ｙｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における位置平滑値のｙ軸成分である。「ｚｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における位置平滑値のｚ軸成分である。「ｘ・ｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における速度平滑値のｘ軸成分である。「ｙ・ｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における速度平滑値のｙ軸成分である。「ｚ・ｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における速度平滑値のｚ軸成分である。
【００４５】
前回の観測時刻ｔｋ−１における平滑値ベクトルｘ→ｋ−１｜ｋ−１と、今回の観測時刻ｔｋにおける予測値ベクトルｘ→ｋ｜ｋ−１との関係は、以下の（５）式で表すことができる。
【００４６】
【数５】
2019124637.tif 000007
【００４７】
上記（５）式において、Φ（ｔｋ，ｔｋ−１）は、前回の観測時刻ｔｋ−１から今回の観測時刻ｔｋまでの経過時間による目標諸元の変化を表す行列である。以下、この行列を「状態遷移行列」と呼ぶ。
【００４８】
上記（５）式に示した状態遷移行列Φ（ｔｋ，ｔｋ−１）を用いると、第２の演算部３０によって演算される観測時刻ｔｋにおける予測誤差共分散行列Ｐｋ｜ｋ−１は、以下の（６）式を用いて表すことができる。
【００４９】
【数６】
2019124637.tif 000008
【００５０】
上記（６）式において、「Ｐｋ−１｜ｋ−１」は、前回の観測時刻ｔｋ−１における平滑誤差共分散行列であり、「Ｑｋ−１」は、前回の観測時刻ｔｋ−１におけるシステム雑音を表すシステム雑音共分散行列である。
【００５１】
上述したように、第３の演算部５０は、平滑値及び平滑誤差共分散行列を演算する。平滑値は、上述した通りである。また、観測時刻ｔｋにおけるカルマンゲイン行列を「Ｋｋ」で表す。カルマンゲイン行列Ｋｋは、以下の（７）式で表すことができる。
【００５２】
【数７】
2019124637.tif 000009
【００５３】
上記（７）式において、「Ｐｋ｜ｋ−１」は、上記（６）式で表される観測時刻ｔｋにおける予測誤差共分散行列であり、「Ｒｋ」は、上記（２）式で表される観測時刻ｔｋにおける観測誤差共分散行列である。また、「Ｈｋ」は、観測時刻ｔｋにおける位置のセンサ観測行列である。位置のセンサ観測行列Ｈｋは、以下の（８）式で表すことができる。
【００５４】
【数８】
2019124637.tif 000010
【００５５】
上記（８）式において、「Ｏ３×３」は、３行３列の零行列である。位置のセンサ観測行列Ｈｋは、予測値ベクトルｘ→ｋ｜ｋ−１からｘ軸、ｙ軸及びｚ軸の位置成分を取り出すための行列である。
【００５６】
上記（７）式、及び（８）式を用いると、観測時刻ｔｋにおける平滑値ベクトルｘ→ｋ｜ｋ、及び観測時刻ｔｋにおける平滑誤差共分散行列Ｐｋ｜ｋのそれぞれは、以下の（９−１）式、及び（９−２）式で表すことができる。
【００５７】
【数９】
2019124637.tif 000011
【００５８】
補正処理部４０の処理の流れは図６に示す通りである。図６に示す処理は、補正処理部４０によって実施される。ステップＳＴ６−１では、第１のセンサ１０と第２のセンサ１１との間にバイアス誤差が存在するか否かが判定される。前述の通り、バイアス誤差とは、異なるセンサ間の観測値に存在する誤差である。
【００５９】
前述した図５の例に示すように、観測値のズレが大きい場合、第１のセンサ１０と第２のセンサ１１との間に、バイアス誤差が存在すると判定される（ステップＳＴ６−１，Ｙｅｓ）。この場合、ステップＳＴ６−２に進む。観測値のズレが大きいか否かは、閾値で判定することができる。
【００６０】
一方、ステップＳＴ６−１において、バイアス誤差が存在しないと判定された場合（ステップＳＴ６−１，Ｎｏ）、図６に示す補正処理のフローを終了する。
【００６１】
ステップＳＴ６−２では、バイアス誤差の発生要因が時刻であるか否かが判定される。バイアス誤差の発生要因が時刻である場合、第１のセンサ１０の観測情報を基に演算される予測値と、第２のセンサ１１の観測情報を基に演算される予測値との間に、前述した時刻同期誤差が表れる。予測値のズレが大きいか否かは、閾値で判定することができる。
【００６２】
第１のセンサ１０と第２のセンサ１１との間に時刻同期誤差があると判定された場合（ステップＳＴ６−２，Ｙｅｓ）、ステップＳＴ６−３に進む。
【００６３】
ステップＳＴ６−３では、予測誤差共分散行列が補正される。より詳細には、予測誤差共分散行列の位置に関する広がりが最も小さい要素のみが補正される。ステップＳＴ６−３の処理を終えると、図６のフローを抜ける。
【００６４】
予測誤差共分散行列の補正式は、以下の（１０−１）式、（１０−２）式、（１０−３）式、（１０−４）式及び（１０−５）式で表すことができる。
【００６５】
【数１０】
2019124637.tif 000012
【００６６】
上記（１０−２）式において、「Ｄｋ」は、「（Ｈｋ・Ｐｋ｜ｋ−１・ＨｋＴ）−１」の固有値を対角要素に持つ対角行列である。「Ｈｋ・Ｐｋ｜ｋ−１・ＨｋＴ」は、予測誤差共分散行列の位置に関する成分から成る行列である。
【００６７】
また、上記（１０−２）式の右辺に現れる「Ｖｋ」は、対角行列Ｄｋに対応する右固有ベクトルを列にもつ行列である。（１０−２）式に示されるように、予測誤差共分散行列Ｐｋ｜ｋ−１の位置に関する成分から成る行列「Ｈｋ・Ｐｋ｜ｋ−１・ＨｋＴ」は、右固有ベクトルを列にもつ行列Ｖｋによって、対角化される。
【００６８】
上記（１０−１）式には、時刻Δｉの補正式が示されている。添字「ｉ」は、ｘ，ｙ，ｚのうちの何れか１つを表すという意味で使用される。すなわち、Δｉは、Δｘ，Δｙ，Δｚのうちの何れか１つを表す。δｉについても同様である。
【００６９】
上記（１０−３）式に示されるように、対角行列Ｄｋにおける対角要素ｄｘ，ｄｙ，ｄｚのうちの最大要素に対して、δｉが“１”に設定され、最大要素以外の他の２つの要素は“０”に設定される。なお、対角行列Ｄｋにおいて、対角要素ｄｘ，ｄｙ，ｄｚのうちの最大要素は、誤差の広がりが最小の要素を意味する。これにより、対角要素ｄｘ，ｄｙ，ｄｚのうちの最大要素のみが、（１０−１）式を用いて補正される。また、（１０−４）式によって、対角行列Ｄｋが補正される。そして、補正後の対角行列Ｄｋ，ｎｅｗを（１０−５）式に代入して演算することにより、補正後の予測誤差共分散行列Ｐｋ｜ｋ−１，ｎｅｗが生成される。
【００７０】
なお、（１０−１）式において、「ｍ」は予測誤差共分散行列の補正量を決定する補正係数である。ここで、ｍ＞１の場合、観測値と予測値とを結ぶ直線方向において、観測誤差共分散行列と予測誤差共分散行列との間で、重なる範囲がある状態を意味している。
【００７１】
図７には、補正前の予測誤差共分散行列が破線で示され、補正後の予測誤差共分散行列が太破線で示されている。図７に示すように、補正後の予測誤差共分散行列は、（１０−１）式で表される補正量によって、ある方向に膨らむように制御される。このときに制御される方向が、「対角要素ｄｘ，ｄｙ，ｄｚのうちの最大要素」に対応する方向である。
【００７２】
また、図７には、センサ間にバイアス誤差が存在しない場合の平滑値が、細字のプラス記号で示され、センサ間に時刻同期誤差が存在する場合の補正前の平滑値が、中太字のプラス記号で示され、センサ間に時刻同期誤差が存在する場合の補正後の平滑値が太字のプラス記号で示されている。ステップＳＴ６−３による補正処理によって、中太字のプラス記号で示される補正前の平滑値は、太字のプラス記号で示される補正後の平滑値に変更される。これにより、補正後の平滑値は、センサ間に時刻同期誤差が存在しない場合の平滑値に近い位置となる。すなわち、ステップＳＴ６−３の補正処理によって、平滑値が時刻同期誤差が存在しない場合の平滑値に近い値となるように補正されるので、平滑値の推定精度を高めることができる。
【００７３】
ステップＳＴ６−２に戻り、第１のセンサ１０と第２のセンサ１１との間に時刻同期誤差がないと判定された場合（ステップＳＴ６−２，Ｎｏ）、ステップＳＴ６−４に進む。ステップＳＴ６−４に進む場合は、第１のセンサ１０と第２のセンサ１１との間において、バイアス誤差があり、且つ時刻同期誤差がない場合である。すなわち、ステップＳＴ６−４は、時刻同期誤差以外のバイアス誤差である、センサ間の位置誤差及びセンサ間の角度誤差により、観測値にズレが生じた場合の補正処理である。
【００７４】
ステップＳＴ６−４では、観測誤差共分散行列が補正される。より詳細には、観測誤差共分散行列の位置及び角度に関する広がりが最も小さい要素のみが補正される。ステップＳＴ６−４の処理を終えると、図６のフローを抜ける。
【００７５】
なお、図６のフローでは、バイアス誤差の発生要因が時刻であるか否かを判定するステップＳＴ６−２が設けられているが、このステップＳＴ６−２の判定処理は省略してもよい。ステップＳＴ６−２の判定処理を省略した場合、予測誤差共分散行列及び観測誤差共分散行列のうちの何れの誤差共分散行列を補正するか、予め決めておけばよい。予測誤差共分散行列及び観測誤差共分散行列のうちの何れかの誤差共分散行列の補正を実施した場合、平滑値の精度は、補正前よりも改善されるため、ステップＳＴ６−１の判定処理をクリアすることが可能となる。なお、一般的に、バイアス誤差による平滑値への影響が大きいのは方位角成分である。このため、何れか一方のみを補正するのであれば、観測誤差共分散行列を補正する方が平滑値の精度を改善するのに適している。
【００７６】
観測誤差共分散行列の補正式は、以下の（１１−１）式、（１１−２）式、及び（１１−３）式で表すことができる。
【００７７】
【数１１】
2019124637.tif 000013
【００７８】
上記（１１−１）式には、観測誤差共分散行列Ｒｋにおける補正後の各要素であるσｊ，ｎｅｗの補正式が示されている。添字「ｊ」は、「ｒｋ」，「ｅｋ」，「ｂｙｋ」のうちの何れか１つを表すという意味で使用される。上記（１１−２）式に示されるδｊについても同様である。すなわち、σｊ，ｎｅｗは、σｒｋ，ｎｅｗ，σｅｋ，ｎｅｗ，σｂｙｋ，ｎｅｗのうちの何れか１つを表す。
【００７９】
（１１−２）式に示されるように、観測誤差共分散行列Ｒｋにおける補正前の各要素であるσｒｋ，σｅｋ，σｂｙｋのうちの最小要素に対して、δｊが“１”に設定され、最小要素以外の他の２つの要素は“０”に設定される。これらの値が（１１−１）式に代入され、（１１−１）式に従って補正量が演算される。また、（１１−３）式によって、補正後の観測誤差共分散行列Ｒｋ，ｎｅｗが演算される。
【００８０】
なお、（１１−１）式において、「ｎ」は観測誤差共分散行列の補正量を決定する補正係数である。ここで、ｎ＞１の場合、観測値と予測値を結ぶ直線方向において、観測誤差共分散行列と予測誤差共分散行列が重なる範囲がある状態を意味している。
【００８１】
図８には、補正前の観測誤差共分散行列が破線で示され、補正後の観測誤差共分散行列が太破線で示されている。図８に示すように、補正後の観測誤差共分散行列は、（１１−１）式で表される補正量によって、ある方向に膨らむように制御される。このときに制御される方向が、「σｒｋ，σｅｋ，σｂｙｋのうちの最小要素」に対応する方向である。
【００８２】
また、図８には、センサ間にバイアス誤差が存在しない場合の平滑値が、細字のプラス記号で示され、センサ間に時刻同期誤差以外のバイアス誤差が存在する場合の補正前の平滑値が、中太字のプラス記号で示され、センサ間に時刻同期誤差以外のバイアス誤差が存在する場合の補正後の平滑値が太字のプラス記号で示されている。ステップＳＴ６−４による補正処理によって、中太字のプラス記号で示される補正前の平滑値は、太字のプラス記号で示される補正後の平滑値に変更される。これにより、補正後の平滑値は、センサ間にバイアス誤差が存在しない場合の平滑値に近い位置となる。すなわち、ステップＳＴ６−４の補正処理によって、平滑値がバイアス誤差の存在しない場合の平滑値に近い値となるように補正されるので、平滑値の推定精度を高めることができる。
【００８３】
以上のように、補正処理部４０は、第１のセンサ１０と第２のセンサ１１との間にバイアス誤差が存在すると判定された場合に、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正し、補正後の観測誤差共分散行列又は補正後の予測誤差共分散行列を第３の演算部５０に出力する。第３の演算部５０は、補正後の観測誤差共分散行列又は補正後の予測誤差共分散行列を基に、平滑値５０ａ及び平滑誤差共分散行列５０ｂの演算を行う。これにより、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することができる。
【００８４】
また、補正処理部４０は、バイアス誤差の要因が時刻であるか否かの判定処理によって、バイアス誤差の要因の切り分けを行い、切り分けの結果に基づいて、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正する。これにより、精度を一時的に下げる制御が、観測誤差共分散行列及び予測誤差共分散行列のうちの何れか一方に限定されるので、補正処理によって平滑値の推定精度が悪化することを防止することができる。また、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正するので、平滑値５０ａ及び平滑誤差共分散行列５０ｂの出力が遅延するのを抑制することができる。
【００８５】
次に、実施の形態に係る追尾処理方法について説明する。実施の形態に係る追尾処理装置を用いて行う追尾処理方法の特徴は、以下の通りである。
【００８６】
（１）第１のステップでは、観測情報を基に、観測誤差共分散行列が演算される。
（２）第２のステップでは、前回の観測時刻における平滑値と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、今回の観測時刻における目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列が演算される。
（３）第３のステップでは、観測情報及び予測値を基に、観測誤差共分散行列が補正される。或いは、観測情報及び予測値を基に、予測誤差共分散行列が補正される。
（４）第４のステップでは、観測情報、予測値、補正後の観測誤差共分散行列、及び補正前の予測誤差共分散行列を基に、平滑値及び平滑誤差共分散行列が演算される。或いは、観測情報、予測値、補正前の観測誤差共分散行列、及び補正後の予測誤差共分散行列を基に、平滑値及び平滑誤差共分散行列が演算される。
【００８７】
上記したように、第３のステップでは、観測情報、観測誤差共分散行列、及び予測値を基に、観測誤差共分散行列が補正され、又は、観測情報、予測値、及び予測誤差共分散行列を基に、予測誤差共分散行列が補正される。これらの処理により、平滑値の推定精度を高めることができ、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することが可能となる。
【００８８】
最後に、実施の形態における追尾処理装置１００の機能を実現するためのハードウェア構成について、図９及び図１０の図面を参照して説明する。図９は、実施の形態の追尾処理装置１００におけるハードウェア構成の一例を示すブロック図である。図１０は、実施の形態の追尾処理装置１００におけるハードウェア構成の他の例を示すブロック図である。
【００８９】
実施の形態における追尾処理装置１００の機能を実現する場合には、図９に示すように、演算を行うプロセッサ２００、プロセッサ２００によって読みとられるプログラムが保存されるメモリ２０２、及び信号の入出力を行うインタフェース２０４を含む構成とすることができる。
【００９０】
プロセッサ２００は、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、又はＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）といった演算手段であってもよい。また、メモリ２０２には、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ  Ｐｒｏｇｒａｍｍａｂｌｅ  ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ  ＥＰＲＯＭ）といった不揮発性又は揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｃ）を例示することができる。
【００９１】
メモリ２０２には、追尾処理装置１００の機能を実行するプログラム及びプロセッサ２００によって参照されるテーブルが格納されている。プロセッサ２００は、インタフェース２０４を介して必要な情報を授受し、メモリ２０２に格納されたプログラムをプロセッサ２００が実行し、メモリ２０２に格納されたテーブルをプロセッサ２００が参照することにより、上述した第１の演算部２０、第２の演算部３０、補正処理部４０及び第３の演算部５０の演算処理を行うことができる。プロセッサ２００による演算結果は、インタフェース２０４を介して表示装置１１０に出力することができる。プロセッサ２００による演算結果は、表示装置１１０への出力と共に、メモリ２０２に記憶してもよい。
【００９２】
図９に示すプロセッサ２００及びメモリ２０２は、図１０のように処理回路２０３に置き換えてもよい。処理回路２０３は、単一回路、複合回路、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ  Ｓｐｅｃｉｆｉｃ  Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ）、又は、これらを組み合わせたものが該当する。なお、第１の演算部２０、第２の演算部３０、補正処理部４０及び第３の演算部５０における一部の処理を処理回路２０３で実施し、処理回路２０３で実施しない処理をプロセッサ２００及びメモリ２０２で実施してもよい。
【００９３】
なお、以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。
【符号の説明】
【００９４】
１０  第１のセンサ、１０ａ，１１ａ  観測値、１０ｂ，１１ｂ  観測時刻、１１  第２のセンサ、２０  第１の演算部、２０ａ，４０ａ  観測誤差共分散行列、３０  第２の演算部、３０ａ  予測値、３０ｂ，４０ｂ  予測誤差共分散行列、４０  補正処理部、５０  第３の演算部、５０ａ  平滑値、５０ｂ  平滑誤差共分散行列、８０  目標、１００  追尾処理装置、１１０  表示装置、２００  プロセッサ、２０２  メモリ、２０３  処理回路、２０４  インタフェース。
【図１】
2019124637.tif 000014
【図２】
2019124637.tif 000015
【図３】
2019124637.tif 000016
【図４】
2019124637.tif 000017
【図５】
2019124637.tif 000018
【図６】
2019124637.tif 000019
【図７】
2019124637.tif 000020
【図８】
2019124637.tif 000021
【図９】
2019124637.tif 000022
【図１０】
2019124637.tif 000023