追尾方法及びその装置
【課題】 加速度運動を行う目標を精度よく追尾する追尾方法を得る。
【解決手段】 第１の時間の平滑誤差共分散行列から第２の時間の予測誤差共分散行列を算出し、第２の時間の予測誤差共分散行列と第２の時間のゲイン行列と所定の観測雑音共分散行列とに基づいて第２の時間の平滑誤差共分散行列を算出し、第２の時間の予測誤差共分散行列と観測雑音共分散行列から、第２の時間の予測値ベクトルに含まれる目標の位置についてのゲインである位置ゲインと第２の時間の予測値ベクトルに含まれる目標の速度についてのゲインである速度ゲインとを含む第２の時間のゲイン行列を算出し、第２の時間の観測値を取得し、取得した観測値と第２の時間の予測値ベクトルと第２の時間のゲイン行列とに基づいて第２の時間の平滑値ベクトルを算出する追尾方法において、速度ゲインとして固定値のゲインを含む第２の時間のゲイン行列を算出する。
【解決手段】 第１の時間の平滑誤差共分散行列から第２の時間の予測誤差共分散行列を算出し、第２の時間の予測誤差共分散行列と第２の時間のゲイン行列と所定の観測雑音共分散行列とに基づいて第２の時間の平滑誤差共分散行列を算出し、第２の時間の予測誤差共分散行列と観測雑音共分散行列から、第２の時間の予測値ベクトルに含まれる目標の位置についてのゲインである位置ゲインと第２の時間の予測値ベクトルに含まれる目標の速度についてのゲインである速度ゲインとを含む第２の時間のゲイン行列を算出し、第２の時間の観測値を取得し、取得した観測値と第２の時間の予測値ベクトルと第２の時間のゲイン行列とに基づいて第２の時間の平滑値ベクトルを算出する追尾方法において、速度ゲインとして固定値のゲインを含む第２の時間のゲイン行列を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、観測値に基づいて移動体の運動諸元を推定する追尾方法及びその装置に関する。
【背景技術】
【０００２】
観測装置による目標観測結果を用いて目標を追尾する手段として、従来はカルマンフィルタを使用したものが一般的である。このような一般的な追尾装置では、目標の基本的な運動を等速直線運動と仮定して運動モデルを記述しておき、目標の等速直線運動からのずれを表現するための加速度項を白色雑音と仮定して追尾処理を行う（例えば、特許文献１）。
【０００３】
【特許文献１】特開２００５−３３１２４８号「目標追尾装置及び目標追尾方法」
【発明の開示】
【発明が解決しようとする課題】
【０００４】
航空機、船舶、車両などの現実の目標は、直進しながら加速または減速したり、半径がほぼ一定の円弧を描きながら旋回する、といった加速度運動を行うことが普通である。このような場合、目標の加速度ベクトルは一定値あるいは緩やかに時間変化する値であり、白色雑音としてモデル化することは難しい。このために、従来の追尾装置では、結果的に最適性能を実現できないという問題があった。
【０００５】
この発明は、このような課題を解決することを目的としてなされたものであり、目標運動における加速度項を白色雑音と仮定せずに、良好な推定性能を得るものである。
【課題を解決するための手段】
【０００６】
この発明の追尾装置では、目標が所与の一定の加速度（想定される目標の最大運動能力より決定する）で継続的に運動した場合の追従遅れの最大値を所定範囲内に収めるとともに、その一方で加速度が零の場合、すなわち目標が等速直線運動を行っている場合の、観測雑音に起因する追尾誤差を最小化する新たなフィルタを使用するものである。
【発明の効果】
【０００７】
この結果、目標の最大の加速度運動に対する追尾誤差を許容範囲内に収めつつ、等速直線運動時の追尾結果の安定化を図ることができ、結果的に容易に最適な追尾性能を実現することができる
【発明を実施するための最良の形態】
【０００８】
以下、この発明の実施の形態について図を用いて説明する。
実施の形態．
まず初めに、カルマンフィルタを用いた一般的な追尾処理について説明する。カルマンフィルタを使用した追尾処理では、目標の運動モデルを式（１）のように仮定する。ただし、サンプリング時刻（以下、単に時刻）をｔ(ｋ)で表す。
【数１】
【０００９】
カルマンフィルタを用いた一般的な追尾処理では、目標の基本的な運動を等速直線運動と仮定して式（１）の運動モデルを記述している。例えば、ｘ−ｙ−ｚの３次元直交座標系における目標の位置と速度を推定する場合、状態ベクトルｘ(ｋ)、状態推移行列Φを式（２）、（３）、（４）のように定義している。
【数２】
【００１０】
ただし、Ｔはベクトル、行列の転置、ドットは時間微分を表す。また、Ｉn×n，Ｏn×nはそれぞれｎ行ｎ列の単位行列及び零行列であり、τは一定のサンプリング周期を表す。
【００１１】
さらに、カルマンフィルタを用いた追尾処理において、式（１）のｗ(ｋ)は現実の目標運動を等速直線で近似したことで生じるｘ(ｋ)の誤差を表現するために設けられたｘ、ｙ、ｚの各方向の加速度項の雑音成分で構成され、より詳細には平均零ベクトル、共分散行列Ｑ(ｋ)の３変量正規分布に従う白色雑音であると仮定されている。
【００１２】
また追尾処理に供給される観測値の観測モデルは式（５）に従うものと仮定する。
【数３】
【００１３】
たとえば、観測装置によりｘ−ｙ−ｚの３次元直交座標系における目標位置の観測結果が得られるとする。このとき、観測値ベクトルｚ(ｋ)を式（６）、観測行列Ｈを式（７）のように定義する。
【数４】
【００１４】
また式（５）において、ｖ(ｋ)は観測装置の観測誤差を表すための雑音成分であって、平均零ベクトル、共分散行列Ｒ(ｋ)の３変量正規分布に従う白色雑音であると仮定する。
【００１５】
式（１）、式（５）のような運動モデル、観測モデルの仮定に基づいたカルマンフィルタのアルゴリズムによる状態ベクトルの推定処理について、以下に説明する。
【００１６】
なお、以降の説明では、時刻ｔ(ｋ−１)までの観測結果に基づいて時刻ｔ(ｋ)の状態ベクトルを推定した予測値ベクトルをｘ^(ｋ｜ｋ−１)と表し、時刻ｔ(ｋ)までの観測結果に基づいて時刻ｔ(ｋ)の状態ベクトルを推定した平滑値ベクトルをｘ^(ｋ｜ｋ)と表す。なお、(変数)^は(変数)の上に^を付記したベクトルを表している。すなわち、式（８）のような関係が成立する。
【００１７】
さらに、ｘ^(ｋ｜ｋ−１)の誤差共分散行列を表す予測誤差共分散行列をＰ(ｋ｜ｋ−１)、ｘ^(ｋ｜ｋ)の誤差共分散行列を表す平滑誤差共分散行列をＰ(ｋ｜ｋ)と表す。すなわち、式（９）から式（１２）までの関係が成立する。さらに、Ｋ(ｋ)はフィルタのゲイン行列であり、式（２）〜（４），式（６）、（７）の設定を行った場合、６行３列の行列である。さらに、Ｉは６行６列の単位行列である。
【数５】
【００１８】
なお、ｘ^(ｋ｜ｋ)の初期値ｘ^(０｜０)と、Ｐ(ｋ｜ｋ)の初期値Ｐ(０｜０)が別途算出され、与えられるものとする。また、システム雑音共分散行列Ｑ(ｋ)、観測雑音共分散行列Ｒ(ｋ)はパラメータとして与える。
【００１９】
カルマンフィルタは、式（８）、式（１１）により、平均的な偏りのない不偏推定値を算出するとともに、式の（１０）でゲイン行列Ｋ(ｋ)を算出することにより、平均２乗誤差を最小化する最適なフィルタとして知られている。
【００２０】
ここまで説明してきたカルマンフィルタは、目標の運動モデルと観測装置の観測モデルとが正しく表現されていれば、最適な推定性能を与えることができる。しかし、カルマンフィルタでは、式（１）が示すように、目標の等速直線運動からのずれを表現するための加速度項ｗ(ｋ)を白色雑音と仮定する必要がある。
【００２１】
しかしながら、航空機、船舶、車両などの現実の目標が加速度運動を行う場合、図３に示すように、直進しながら加速または減速したり、半径がほぼ一定の円弧を描きながら旋回することが多い。
【００２２】
これらの場合、目標の加速度ベクトルは一定値あるいは緩やかに時間変化する値であり、白色雑音としてモデル化することには無理である。この結果、従来の追尾装置では、所望の追尾精度を得るために必要なシステム雑音共分散行列Ｑ(ｋ)の値をパラメータとして設定することが困難であった。換言すれば、従来の追尾装置では、追尾すべき目標の運動特性に関する知識を基に、パラメータＱ(ｋ)の値を的確に設定することができないため、結果的に最適性能を実現できないという問題があった。
【００２３】
この発明の実施の形態による追尾装置では、上記のような問題を解決するために、目標の加速度項を白色雑音であると仮定することをやめる。以下に、その詳細な方法について説明する。
【００２４】
追尾フィルタの計算式における状態ベクトルの予測式と更新式として、従来のカルマンフィルタと同じように式（８）と式（１１）を使用する。式（８）と式（１１）を用いることにより、等速直線運動時の追尾誤差に平均的な偏りが生じないことがよく知られている。
【００２５】
つぎに、式（１０）におけるゲイン行列Ｋ(ｋ)を以下のように表すとする。
【数６】
【００２６】
ここで、Ｋ1(ｋ)は目標の位置推定値に対するゲイン（位置ゲイン）であり、Ｋ2(ｋ)は速度推定値に対するゲイン（速度ゲイン）である。式（２）、式（３）、式（６）、式（７）の設定を行った場合、位置ゲインＫ1(ｋ)、速度ゲインＫ2(ｋ)はともに３行３列の行列である。
【００２７】
この発明の実施の形態による追尾装置では、速度ゲインＫ2(ｋ)を時間変化しない固定値に設定する。これを式（１４）のように表す。
【数７】
【００２８】
いま、目標が一定の加速度ベクトルａc（想定される目標の最大の運動性能から決定する）で運動した場合を考える。このとき、電子情報通信学会論文誌B, Vol.J86-B, No.11, pp.2397-2406, 2003年11月、「線形フィルタによる３次元空間の追尾における定常追従誤差」に示されたように、観測装置の観測誤差が全く存在しない理想条件下を仮定すれば、追尾開始より十分に時間が経過したのちの定常状態における予測位置の誤差ベクトル（定常追従誤差ベクトル）は、次式に示される有限値となる。
【数８】
【００２９】
したがって、想定される目標の最大加速度ベクトルａcに対し、許容する予測位置誤差ベクトルｅ~ｐを決定すれば、式（１５）の関係より、時間変化しない固定値のゲイン（固定ゲイン）Ｋ-2の値を決定することができる。なお、(変数)~は(変数)の上に~を付記したものと同じものを指す。また(変数)-は(変数)の上に−を付記したものと同じものを指す。
【００３０】
つぎに、式（１４）で定義した固定値の速度ゲインＫ-2を式（１３）に使用し、式（８）、式（１１）を用いてフィルタ処理を行った場合の等速直線運動に対する追尾誤差を考える。等速直線運動時は、式（１５）においてａc＝０とおいた場合に相当することから、予測位置に対する定常追従誤差はｅ~ｐ＝０である。しかし、観測装置の観測誤差に起因した雑音成分の追尾誤差が生じる。
【００３１】
この追尾誤差は、予測値ベクトルｘ^(ｋ｜ｋ−１)、平滑値ベクトルｘ^(ｋ｜ｋ)の双方に発生する。そこで、この発明の実施の形態では、この場合の予測値ベクトルの平均２乗誤差と平滑値ベクトルの平均２乗誤差を重み付けした評価値、すなわち式（１６）を最小化するように位置ゲインＫ1(ｋ)を決定する。
【数９】
【００３２】
ここで、ｃ＝１とすれば平滑値ベクトルの平均２乗誤差を最小化するようにＫ1(ｋ)を決定することができ、ｃ＝０とすれば予測値ベクトルの平均２乗誤差を最小化するようにＫ1(ｋ)を決定することができる。
【００３３】
式（１６）を最小化するＫ1(ｋ)の計算式は式（１７）で与えられる。
【数１０】
【００３４】
ここで、Ｐ11(ｋ｜ｋ−１)，Ｐ12(ｋ｜ｋ−１)は、予測誤差共分散行列を式（１８）に示す２行２列のブロック行列で表した場合の各ブロックに相当し、Ｐ11(ｋ｜ｋ−１)は予測位置誤差の共分散行列、Ｐ12(ｋ｜ｋ−１)は予測位置誤差と予測速度誤差の相関行列に相当する。
【数１１】
また、式(８)、式（１１）のフィルタ処理式において、式（１３）、式（１４）、式（１７）のゲイン行列Ｋ(ｋ)を使用した場合、予測誤差共分散行列Ｐ(ｋ｜ｋ−１)、平滑誤差共分散行列Ｐ(ｋ｜ｋ)の逐次計算式は式（１９）、式（２０）で与えられる。
【数１２】
【００３５】
なお、式（２０）は従来のカルマンフィルタの式（１２）と同一である。一方、式（１９）と従来のカルマンフィルタの場合の式（９）とを比較すると、式（１９）にはシステム雑音の共分散行列Ｑ（ｋ）の項がないことが分かる。これは、この発明の実施の形態によるフィルタ法が目標の加速度項を白色雑音と仮定することをやめたことによってもたらされる効果である。
【００３６】
続いて、この発明の実施の形態による追尾装置の構成と動作について説明する。図１は、この発明の追尾装置の構成を表す図である。なお、図では観測値を取得する観測装置１についても図示している。図の追尾装置２において、平滑値ベクトル記憶手段１１は平滑値ベクトルを記憶する部位であって、平滑値ベクトル初期値設定手段１２は平滑値ベクトル記憶手段１１に平滑値ベクトルの初期値を設定する部位である。
【００３７】
予測手段１３は、前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する部位である。また、ゲイン行列設定手段１４は位置ゲインＫ1(ｋ)と速度ゲインＫ2(ｋ)を用いてゲイン行列Ｋ(ｋ)を設定する部位である。
【００３８】
平滑手段１５は、予測手段１３によって算出された予測値ベクトルと観測装置１からの観測値ベクトルを入力し、またゲイン行列設定手段１４よりゲイン行列Ｋ(ｋ)を入力して、平滑値ベクトルを算出する部位である。
【００３９】
また、平滑誤差共分散行列初期値記憶手段１６は、平滑誤差共分散行列Ｐ(ｋ｜ｋ)の値を記憶する平滑値ベクトルを記憶する部位であって、平滑誤差共分散行列初期値設定手段１７は平滑誤差共分散行列初期値記憶手段１６に平滑誤差共分散行列Ｐ(ｋ｜ｋ)の初期値を設定する部位である。
【００４０】
予測誤差共分散行列算出手段１８は、平滑誤差共分散行列初期値記憶手段１６によって記憶された前時刻の平滑誤差共分散行列Ｐ(ｋ｜ｋ)より現時刻の予測誤差共分散行列Ｐ(ｋ｜ｋ−１)を算出する部位である。
【００４１】
平滑誤差共分散行列算出手段１９は、現時刻の予測誤差共分散行列Ｐ(ｋ｜ｋ−１)とゲイン行列設定手段よりのゲイン行列Ｋ(ｋ)を入力し、また観測装置より観測雑音共分散行列Ｒ(ｋ)を入力して、平滑誤差共分散行列Ｐ(ｋ｜ｋ)を算出する部位である。
【００４２】
また、速度ゲイン記憶手段２０は、速度ゲインＫ2(ｋ)を記憶する部位であって、速度ゲイン設定手段２１は、速度ゲイン記憶手段に速度ゲインＫ2(ｋ)を設定する部位である。
【００４３】
位置ゲイン算出手段２２は、予測誤差共分散行列Ｐ(ｋ｜ｋ−１)の値と観測装置からの観測雑音共分散行列Ｒ(ｋ)を入力し、位置ゲインＫ1(ｋ)を算出する部位である。
【００４４】
なお、ゲイン行列設定手段１４、速度ゲイン記憶手段２０、速度ゲイン設定手段２１、位置ゲイン算出手段２２をまとめてゲイン行列算出手段２３と呼ぶ。
【００４５】
上述の各構成部位にあっては、ベクトル値や数値の記憶に供される部位は磁気ディスク装置やランダムアクセスメモリ、フラッシュメモリなどの記憶装置若しくは記憶素子を用いて構成される。またその他の部位は上述のような作用を行う専用の回路若しくはＤＳＰ(Digital Signal Processor)として構成してもよいし、一つ若しくは複数のＣＰＵ(Central Processing Unit)で処理されるソフトウエアプログラムの機能として実装してもよい。
【００４６】
図２は、この発明の追尾方法のフローチャートである。まずステップＳ１０１において
平滑値ベクトル初期値設定手段１２は平滑値ベクトル記憶手段１１に平滑値ベクトルの初期値ｘ^(０｜０)を設定し、また、平滑誤差共分散行列初期値設定手段１７は平滑誤差共分散行列記憶手段１６に平滑誤差共分散行列の初期値Ｐ(０｜０)を設定する。
【００４７】
ステップＳ１０２において、速度ゲイン設定手段２１は、固定値の速度ゲインＫ-2を速度ゲイン記憶手段に設定する。
【００４８】
ステップＳ１０３において、予測手段１３は、平滑値ベクトル記憶手段１１から入力した前時刻の平滑値ベクトルｘ^(ｋ−１｜ｋ−１)を使用し、式（８）に従って、現時刻の予測値ベクトルｘ^(ｋ｜ｋ−１)を算出する。
それとともに、予測誤差共分散行列算出手段１９は、平滑誤差共分散行列記憶手段から入力した前時刻の平滑誤差共分散行列Ｐ(ｋ−１｜ｋ−１)を使用し、式（１９）に従って、現時刻の予測誤差共分散行列Ｐ(ｋ｜ｋ−１)を算出する。
【００４９】
予測手段１３、平滑手段１５、位置ゲイン算出手段２２、行列ゲイン設定手段１４、
平滑誤差共分散行列算出手段１９のいずれかの部位によって追尾終了する。
【００５０】
以降、各サンプリング時刻がｋであるとして説明する。位置ゲイン算出手段２２は、観測装置より観測値ベクトルｚ(ｋ)と観測雑音共分散行列Ｒ(ｋ)を入力すると（ステップＳ１０４）、位置ゲイン算出手段２２が、式（１７）に従い、位置ゲインＫ1(ｋ)を算出する（ステップＳ１０５）。
【００５１】
ステップＳ１０６において、行列ゲイン設定手段１４は、位置ゲインＫ1(ｋ)と速度ゲインＫ-2を用いて、式（２１）に従って、ゲイン行列Ｋ(ｋ)を設定する。
【数１３】
【００５２】
ステップＳ１０７において、平滑手段１５は、式（１１）に従って、前記予測値ベクトルｘ^(ｋ｜ｋ−１)、観測値ベクトルｚ(ｋ)、ゲイン行列Ｋ(ｋ)を使用して、現時刻の平滑値ベクトルｘ^(ｋ｜ｋ)を算出する。この平滑値ベクトルは平滑値ベクトル記憶手段１１に格納する。
【００５３】
一方、平滑誤差共分散行列算出手段１９は、式(２０)に従って、前記予測誤差共分散行列Ｐ(ｋ｜ｋ−１)、観測雑音共分散行列Ｒ(ｋ)、ゲイン行列Ｋ(ｋ)を使用して、現時刻の平滑誤差共分散行列Ｐ(ｋ｜ｋ)を算出する。この平滑誤差共分散行列Ｐ(ｋ｜ｋ)は平滑誤差共分散行列記憶手段に格納する。
【００５４】
以上が時刻ｔ(ｋ)における一連の処理である。続いて、追尾終了かどうかが判定され、追尾終了でない場合（Ｓ１０８：Ｎｏ）はｋの値がインクリメントされた後、ステップＳ１０４から再実行される。また追尾終了の場合（Ｓ１０８：Ｙｅｓ）の場合は終了する。
がなされるまで、これら一連の処理を繰り返して実行する。
【００５５】
なお、式（１７）において、重み係数のパラメータをｃ＝１に設定すると、現時刻の目標の運動を監視するために平滑値ベクトルの追尾精度を重視するような捜索型観測装置（捜索レーダ等）への適用に適したものとなる。一方、ｃ＝０に設定した場合、次時刻の観測領域を決定するために予測値ベクトルの追尾精度を重視する追尾型観測装置（追尾レーダ等）への適用に適したものとなる。
【００５６】
また、これまでの説明では、ｘ−ｙ−ｚの３次元空間における目標の位置と速度を推定する場合を例とした。しかしこの他の場合にも、この発明を適用する方法は容易に類推できよう。例えばｘ−ｙの２次元空間における目標の位置と速度を推定することには、状態ベクトルを式（２２）、状態推移行列を式（２３）、観測値ベクトルを式（２４）、観測行列を式（２５）のように置けば、同様の追尾装置、追尾方法を得ることができる。
【数１４】
【００５７】
このとき、位置ゲインＫ1(ｋ)、速度ゲインＫ-2はともに２行２列の行列である。
さらに、２次元空間における追尾または３次元空間における追尾を、座標軸ごとに独立に実行するため、１次元空間のフィルタを構成することもできる。この場合は、状態ベクトルを式（２６）、状態推移行列を式（２７）、観測値ベクトルを式（２８）、観測行列を式（２９）のように設定することにより、３次元や２次元の追尾装置、追尾方法と同様の装置、方法を得ることができる。
【数１５】
【００５８】
なお、式（２６）〜式（２９）はｘ軸方向の追尾処理を行う場合を例として表したものである。この場合、位置ゲインＫ1(ｋ)、速度ゲインＫ-2はともにスカラとなる。
【００５９】
以上、この発明の追尾装置及び追尾方法によれば、カルマンフィルタの場合のように目標の加速度を白色雑音のシステム雑音としてモデル化することがないため、その雑音共分散行列をパラメータとして設定する必要がない。その代わりに、想定される目標の最大加速度に対して許容される予測位置の定常追従誤差を指定することにより、固定の速度ゲインの値を決定する方法をとっている。さらにその決定された速度ゲインを用いた場合に、等速直線運動時の、観測雑音に起因する追尾誤差の雑音成分を最小化するように位置ゲインを自動的に算出する仕組みをとっている。
【００６０】
この結果、目標の最大の加速度運動に対する追尾誤差を許容範囲内に収めつつ、等速直線運動時の追尾結果の安定化を図ることができ、結果的に容易に最適な追尾性能を実現することができるとの利点を有する。
【産業上の利用可能性】
【００６１】
この発明は、例えば観測値に基づいて移動体の運動諸元を推定する追尾方法及びその装置に適用することが可能である。
【図面の簡単な説明】
【００６２】
【図１】この発明の実施の形態による追尾装置の構成を示すブロック図である。
【図２】この発明の実施の形態による追尾装置の処理のフローチャートである。
【図３】この発明の実施の形態による追尾装置の目標の運動例を示す図である。
【符号の説明】
【００６３】
１３ 予測手段、
１８ 予測誤差共分散行列算出手段、
１９ 平滑誤差共分散行列算出手段、
２３ ゲイン行列算出手段、
１５ 平滑手段。
【技術分野】
【０００１】
この発明は、観測値に基づいて移動体の運動諸元を推定する追尾方法及びその装置に関する。
【背景技術】
【０００２】
観測装置による目標観測結果を用いて目標を追尾する手段として、従来はカルマンフィルタを使用したものが一般的である。このような一般的な追尾装置では、目標の基本的な運動を等速直線運動と仮定して運動モデルを記述しておき、目標の等速直線運動からのずれを表現するための加速度項を白色雑音と仮定して追尾処理を行う（例えば、特許文献１）。
【０００３】
【特許文献１】特開２００５−３３１２４８号「目標追尾装置及び目標追尾方法」
【発明の開示】
【発明が解決しようとする課題】
【０００４】
航空機、船舶、車両などの現実の目標は、直進しながら加速または減速したり、半径がほぼ一定の円弧を描きながら旋回する、といった加速度運動を行うことが普通である。このような場合、目標の加速度ベクトルは一定値あるいは緩やかに時間変化する値であり、白色雑音としてモデル化することは難しい。このために、従来の追尾装置では、結果的に最適性能を実現できないという問題があった。
【０００５】
この発明は、このような課題を解決することを目的としてなされたものであり、目標運動における加速度項を白色雑音と仮定せずに、良好な推定性能を得るものである。
【課題を解決するための手段】
【０００６】
この発明の追尾装置では、目標が所与の一定の加速度（想定される目標の最大運動能力より決定する）で継続的に運動した場合の追従遅れの最大値を所定範囲内に収めるとともに、その一方で加速度が零の場合、すなわち目標が等速直線運動を行っている場合の、観測雑音に起因する追尾誤差を最小化する新たなフィルタを使用するものである。
【発明の効果】
【０００７】
この結果、目標の最大の加速度運動に対する追尾誤差を許容範囲内に収めつつ、等速直線運動時の追尾結果の安定化を図ることができ、結果的に容易に最適な追尾性能を実現することができる
【発明を実施するための最良の形態】
【０００８】
以下、この発明の実施の形態について図を用いて説明する。
実施の形態．
まず初めに、カルマンフィルタを用いた一般的な追尾処理について説明する。カルマンフィルタを使用した追尾処理では、目標の運動モデルを式（１）のように仮定する。ただし、サンプリング時刻（以下、単に時刻）をｔ(ｋ)で表す。
【数１】
【０００９】
カルマンフィルタを用いた一般的な追尾処理では、目標の基本的な運動を等速直線運動と仮定して式（１）の運動モデルを記述している。例えば、ｘ−ｙ−ｚの３次元直交座標系における目標の位置と速度を推定する場合、状態ベクトルｘ(ｋ)、状態推移行列Φを式（２）、（３）、（４）のように定義している。
【数２】
【００１０】
ただし、Ｔはベクトル、行列の転置、ドットは時間微分を表す。また、Ｉn×n，Ｏn×nはそれぞれｎ行ｎ列の単位行列及び零行列であり、τは一定のサンプリング周期を表す。
【００１１】
さらに、カルマンフィルタを用いた追尾処理において、式（１）のｗ(ｋ)は現実の目標運動を等速直線で近似したことで生じるｘ(ｋ)の誤差を表現するために設けられたｘ、ｙ、ｚの各方向の加速度項の雑音成分で構成され、より詳細には平均零ベクトル、共分散行列Ｑ(ｋ)の３変量正規分布に従う白色雑音であると仮定されている。
【００１２】
また追尾処理に供給される観測値の観測モデルは式（５）に従うものと仮定する。
【数３】
【００１３】
たとえば、観測装置によりｘ−ｙ−ｚの３次元直交座標系における目標位置の観測結果が得られるとする。このとき、観測値ベクトルｚ(ｋ)を式（６）、観測行列Ｈを式（７）のように定義する。
【数４】
【００１４】
また式（５）において、ｖ(ｋ)は観測装置の観測誤差を表すための雑音成分であって、平均零ベクトル、共分散行列Ｒ(ｋ)の３変量正規分布に従う白色雑音であると仮定する。
【００１５】
式（１）、式（５）のような運動モデル、観測モデルの仮定に基づいたカルマンフィルタのアルゴリズムによる状態ベクトルの推定処理について、以下に説明する。
【００１６】
なお、以降の説明では、時刻ｔ(ｋ−１)までの観測結果に基づいて時刻ｔ(ｋ)の状態ベクトルを推定した予測値ベクトルをｘ^(ｋ｜ｋ−１)と表し、時刻ｔ(ｋ)までの観測結果に基づいて時刻ｔ(ｋ)の状態ベクトルを推定した平滑値ベクトルをｘ^(ｋ｜ｋ)と表す。なお、(変数)^は(変数)の上に^を付記したベクトルを表している。すなわち、式（８）のような関係が成立する。
【００１７】
さらに、ｘ^(ｋ｜ｋ−１)の誤差共分散行列を表す予測誤差共分散行列をＰ(ｋ｜ｋ−１)、ｘ^(ｋ｜ｋ)の誤差共分散行列を表す平滑誤差共分散行列をＰ(ｋ｜ｋ)と表す。すなわち、式（９）から式（１２）までの関係が成立する。さらに、Ｋ(ｋ)はフィルタのゲイン行列であり、式（２）〜（４），式（６）、（７）の設定を行った場合、６行３列の行列である。さらに、Ｉは６行６列の単位行列である。
【数５】
【００１８】
なお、ｘ^(ｋ｜ｋ)の初期値ｘ^(０｜０)と、Ｐ(ｋ｜ｋ)の初期値Ｐ(０｜０)が別途算出され、与えられるものとする。また、システム雑音共分散行列Ｑ(ｋ)、観測雑音共分散行列Ｒ(ｋ)はパラメータとして与える。
【００１９】
カルマンフィルタは、式（８）、式（１１）により、平均的な偏りのない不偏推定値を算出するとともに、式の（１０）でゲイン行列Ｋ(ｋ)を算出することにより、平均２乗誤差を最小化する最適なフィルタとして知られている。
【００２０】
ここまで説明してきたカルマンフィルタは、目標の運動モデルと観測装置の観測モデルとが正しく表現されていれば、最適な推定性能を与えることができる。しかし、カルマンフィルタでは、式（１）が示すように、目標の等速直線運動からのずれを表現するための加速度項ｗ(ｋ)を白色雑音と仮定する必要がある。
【００２１】
しかしながら、航空機、船舶、車両などの現実の目標が加速度運動を行う場合、図３に示すように、直進しながら加速または減速したり、半径がほぼ一定の円弧を描きながら旋回することが多い。
【００２２】
これらの場合、目標の加速度ベクトルは一定値あるいは緩やかに時間変化する値であり、白色雑音としてモデル化することには無理である。この結果、従来の追尾装置では、所望の追尾精度を得るために必要なシステム雑音共分散行列Ｑ(ｋ)の値をパラメータとして設定することが困難であった。換言すれば、従来の追尾装置では、追尾すべき目標の運動特性に関する知識を基に、パラメータＱ(ｋ)の値を的確に設定することができないため、結果的に最適性能を実現できないという問題があった。
【００２３】
この発明の実施の形態による追尾装置では、上記のような問題を解決するために、目標の加速度項を白色雑音であると仮定することをやめる。以下に、その詳細な方法について説明する。
【００２４】
追尾フィルタの計算式における状態ベクトルの予測式と更新式として、従来のカルマンフィルタと同じように式（８）と式（１１）を使用する。式（８）と式（１１）を用いることにより、等速直線運動時の追尾誤差に平均的な偏りが生じないことがよく知られている。
【００２５】
つぎに、式（１０）におけるゲイン行列Ｋ(ｋ)を以下のように表すとする。
【数６】
【００２６】
ここで、Ｋ1(ｋ)は目標の位置推定値に対するゲイン（位置ゲイン）であり、Ｋ2(ｋ)は速度推定値に対するゲイン（速度ゲイン）である。式（２）、式（３）、式（６）、式（７）の設定を行った場合、位置ゲインＫ1(ｋ)、速度ゲインＫ2(ｋ)はともに３行３列の行列である。
【００２７】
この発明の実施の形態による追尾装置では、速度ゲインＫ2(ｋ)を時間変化しない固定値に設定する。これを式（１４）のように表す。
【数７】
【００２８】
いま、目標が一定の加速度ベクトルａc（想定される目標の最大の運動性能から決定する）で運動した場合を考える。このとき、電子情報通信学会論文誌B, Vol.J86-B, No.11, pp.2397-2406, 2003年11月、「線形フィルタによる３次元空間の追尾における定常追従誤差」に示されたように、観測装置の観測誤差が全く存在しない理想条件下を仮定すれば、追尾開始より十分に時間が経過したのちの定常状態における予測位置の誤差ベクトル（定常追従誤差ベクトル）は、次式に示される有限値となる。
【数８】
【００２９】
したがって、想定される目標の最大加速度ベクトルａcに対し、許容する予測位置誤差ベクトルｅ~ｐを決定すれば、式（１５）の関係より、時間変化しない固定値のゲイン（固定ゲイン）Ｋ-2の値を決定することができる。なお、(変数)~は(変数)の上に~を付記したものと同じものを指す。また(変数)-は(変数)の上に−を付記したものと同じものを指す。
【００３０】
つぎに、式（１４）で定義した固定値の速度ゲインＫ-2を式（１３）に使用し、式（８）、式（１１）を用いてフィルタ処理を行った場合の等速直線運動に対する追尾誤差を考える。等速直線運動時は、式（１５）においてａc＝０とおいた場合に相当することから、予測位置に対する定常追従誤差はｅ~ｐ＝０である。しかし、観測装置の観測誤差に起因した雑音成分の追尾誤差が生じる。
【００３１】
この追尾誤差は、予測値ベクトルｘ^(ｋ｜ｋ−１)、平滑値ベクトルｘ^(ｋ｜ｋ)の双方に発生する。そこで、この発明の実施の形態では、この場合の予測値ベクトルの平均２乗誤差と平滑値ベクトルの平均２乗誤差を重み付けした評価値、すなわち式（１６）を最小化するように位置ゲインＫ1(ｋ)を決定する。
【数９】
【００３２】
ここで、ｃ＝１とすれば平滑値ベクトルの平均２乗誤差を最小化するようにＫ1(ｋ)を決定することができ、ｃ＝０とすれば予測値ベクトルの平均２乗誤差を最小化するようにＫ1(ｋ)を決定することができる。
【００３３】
式（１６）を最小化するＫ1(ｋ)の計算式は式（１７）で与えられる。
【数１０】
【００３４】
ここで、Ｐ11(ｋ｜ｋ−１)，Ｐ12(ｋ｜ｋ−１)は、予測誤差共分散行列を式（１８）に示す２行２列のブロック行列で表した場合の各ブロックに相当し、Ｐ11(ｋ｜ｋ−１)は予測位置誤差の共分散行列、Ｐ12(ｋ｜ｋ−１)は予測位置誤差と予測速度誤差の相関行列に相当する。
【数１１】
また、式(８)、式（１１）のフィルタ処理式において、式（１３）、式（１４）、式（１７）のゲイン行列Ｋ(ｋ)を使用した場合、予測誤差共分散行列Ｐ(ｋ｜ｋ−１)、平滑誤差共分散行列Ｐ(ｋ｜ｋ)の逐次計算式は式（１９）、式（２０）で与えられる。
【数１２】
【００３５】
なお、式（２０）は従来のカルマンフィルタの式（１２）と同一である。一方、式（１９）と従来のカルマンフィルタの場合の式（９）とを比較すると、式（１９）にはシステム雑音の共分散行列Ｑ（ｋ）の項がないことが分かる。これは、この発明の実施の形態によるフィルタ法が目標の加速度項を白色雑音と仮定することをやめたことによってもたらされる効果である。
【００３６】
続いて、この発明の実施の形態による追尾装置の構成と動作について説明する。図１は、この発明の追尾装置の構成を表す図である。なお、図では観測値を取得する観測装置１についても図示している。図の追尾装置２において、平滑値ベクトル記憶手段１１は平滑値ベクトルを記憶する部位であって、平滑値ベクトル初期値設定手段１２は平滑値ベクトル記憶手段１１に平滑値ベクトルの初期値を設定する部位である。
【００３７】
予測手段１３は、前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する部位である。また、ゲイン行列設定手段１４は位置ゲインＫ1(ｋ)と速度ゲインＫ2(ｋ)を用いてゲイン行列Ｋ(ｋ)を設定する部位である。
【００３８】
平滑手段１５は、予測手段１３によって算出された予測値ベクトルと観測装置１からの観測値ベクトルを入力し、またゲイン行列設定手段１４よりゲイン行列Ｋ(ｋ)を入力して、平滑値ベクトルを算出する部位である。
【００３９】
また、平滑誤差共分散行列初期値記憶手段１６は、平滑誤差共分散行列Ｐ(ｋ｜ｋ)の値を記憶する平滑値ベクトルを記憶する部位であって、平滑誤差共分散行列初期値設定手段１７は平滑誤差共分散行列初期値記憶手段１６に平滑誤差共分散行列Ｐ(ｋ｜ｋ)の初期値を設定する部位である。
【００４０】
予測誤差共分散行列算出手段１８は、平滑誤差共分散行列初期値記憶手段１６によって記憶された前時刻の平滑誤差共分散行列Ｐ(ｋ｜ｋ)より現時刻の予測誤差共分散行列Ｐ(ｋ｜ｋ−１)を算出する部位である。
【００４１】
平滑誤差共分散行列算出手段１９は、現時刻の予測誤差共分散行列Ｐ(ｋ｜ｋ−１)とゲイン行列設定手段よりのゲイン行列Ｋ(ｋ)を入力し、また観測装置より観測雑音共分散行列Ｒ(ｋ)を入力して、平滑誤差共分散行列Ｐ(ｋ｜ｋ)を算出する部位である。
【００４２】
また、速度ゲイン記憶手段２０は、速度ゲインＫ2(ｋ)を記憶する部位であって、速度ゲイン設定手段２１は、速度ゲイン記憶手段に速度ゲインＫ2(ｋ)を設定する部位である。
【００４３】
位置ゲイン算出手段２２は、予測誤差共分散行列Ｐ(ｋ｜ｋ−１)の値と観測装置からの観測雑音共分散行列Ｒ(ｋ)を入力し、位置ゲインＫ1(ｋ)を算出する部位である。
【００４４】
なお、ゲイン行列設定手段１４、速度ゲイン記憶手段２０、速度ゲイン設定手段２１、位置ゲイン算出手段２２をまとめてゲイン行列算出手段２３と呼ぶ。
【００４５】
上述の各構成部位にあっては、ベクトル値や数値の記憶に供される部位は磁気ディスク装置やランダムアクセスメモリ、フラッシュメモリなどの記憶装置若しくは記憶素子を用いて構成される。またその他の部位は上述のような作用を行う専用の回路若しくはＤＳＰ(Digital Signal Processor)として構成してもよいし、一つ若しくは複数のＣＰＵ(Central Processing Unit)で処理されるソフトウエアプログラムの機能として実装してもよい。
【００４６】
図２は、この発明の追尾方法のフローチャートである。まずステップＳ１０１において
平滑値ベクトル初期値設定手段１２は平滑値ベクトル記憶手段１１に平滑値ベクトルの初期値ｘ^(０｜０)を設定し、また、平滑誤差共分散行列初期値設定手段１７は平滑誤差共分散行列記憶手段１６に平滑誤差共分散行列の初期値Ｐ(０｜０)を設定する。
【００４７】
ステップＳ１０２において、速度ゲイン設定手段２１は、固定値の速度ゲインＫ-2を速度ゲイン記憶手段に設定する。
【００４８】
ステップＳ１０３において、予測手段１３は、平滑値ベクトル記憶手段１１から入力した前時刻の平滑値ベクトルｘ^(ｋ−１｜ｋ−１)を使用し、式（８）に従って、現時刻の予測値ベクトルｘ^(ｋ｜ｋ−１)を算出する。
それとともに、予測誤差共分散行列算出手段１９は、平滑誤差共分散行列記憶手段から入力した前時刻の平滑誤差共分散行列Ｐ(ｋ−１｜ｋ−１)を使用し、式（１９）に従って、現時刻の予測誤差共分散行列Ｐ(ｋ｜ｋ−１)を算出する。
【００４９】
予測手段１３、平滑手段１５、位置ゲイン算出手段２２、行列ゲイン設定手段１４、
平滑誤差共分散行列算出手段１９のいずれかの部位によって追尾終了する。
【００５０】
以降、各サンプリング時刻がｋであるとして説明する。位置ゲイン算出手段２２は、観測装置より観測値ベクトルｚ(ｋ)と観測雑音共分散行列Ｒ(ｋ)を入力すると（ステップＳ１０４）、位置ゲイン算出手段２２が、式（１７）に従い、位置ゲインＫ1(ｋ)を算出する（ステップＳ１０５）。
【００５１】
ステップＳ１０６において、行列ゲイン設定手段１４は、位置ゲインＫ1(ｋ)と速度ゲインＫ-2を用いて、式（２１）に従って、ゲイン行列Ｋ(ｋ)を設定する。
【数１３】
【００５２】
ステップＳ１０７において、平滑手段１５は、式（１１）に従って、前記予測値ベクトルｘ^(ｋ｜ｋ−１)、観測値ベクトルｚ(ｋ)、ゲイン行列Ｋ(ｋ)を使用して、現時刻の平滑値ベクトルｘ^(ｋ｜ｋ)を算出する。この平滑値ベクトルは平滑値ベクトル記憶手段１１に格納する。
【００５３】
一方、平滑誤差共分散行列算出手段１９は、式(２０)に従って、前記予測誤差共分散行列Ｐ(ｋ｜ｋ−１)、観測雑音共分散行列Ｒ(ｋ)、ゲイン行列Ｋ(ｋ)を使用して、現時刻の平滑誤差共分散行列Ｐ(ｋ｜ｋ)を算出する。この平滑誤差共分散行列Ｐ(ｋ｜ｋ)は平滑誤差共分散行列記憶手段に格納する。
【００５４】
以上が時刻ｔ(ｋ)における一連の処理である。続いて、追尾終了かどうかが判定され、追尾終了でない場合（Ｓ１０８：Ｎｏ）はｋの値がインクリメントされた後、ステップＳ１０４から再実行される。また追尾終了の場合（Ｓ１０８：Ｙｅｓ）の場合は終了する。
がなされるまで、これら一連の処理を繰り返して実行する。
【００５５】
なお、式（１７）において、重み係数のパラメータをｃ＝１に設定すると、現時刻の目標の運動を監視するために平滑値ベクトルの追尾精度を重視するような捜索型観測装置（捜索レーダ等）への適用に適したものとなる。一方、ｃ＝０に設定した場合、次時刻の観測領域を決定するために予測値ベクトルの追尾精度を重視する追尾型観測装置（追尾レーダ等）への適用に適したものとなる。
【００５６】
また、これまでの説明では、ｘ−ｙ−ｚの３次元空間における目標の位置と速度を推定する場合を例とした。しかしこの他の場合にも、この発明を適用する方法は容易に類推できよう。例えばｘ−ｙの２次元空間における目標の位置と速度を推定することには、状態ベクトルを式（２２）、状態推移行列を式（２３）、観測値ベクトルを式（２４）、観測行列を式（２５）のように置けば、同様の追尾装置、追尾方法を得ることができる。
【数１４】
【００５７】
このとき、位置ゲインＫ1(ｋ)、速度ゲインＫ-2はともに２行２列の行列である。
さらに、２次元空間における追尾または３次元空間における追尾を、座標軸ごとに独立に実行するため、１次元空間のフィルタを構成することもできる。この場合は、状態ベクトルを式（２６）、状態推移行列を式（２７）、観測値ベクトルを式（２８）、観測行列を式（２９）のように設定することにより、３次元や２次元の追尾装置、追尾方法と同様の装置、方法を得ることができる。
【数１５】
【００５８】
なお、式（２６）〜式（２９）はｘ軸方向の追尾処理を行う場合を例として表したものである。この場合、位置ゲインＫ1(ｋ)、速度ゲインＫ-2はともにスカラとなる。
【００５９】
以上、この発明の追尾装置及び追尾方法によれば、カルマンフィルタの場合のように目標の加速度を白色雑音のシステム雑音としてモデル化することがないため、その雑音共分散行列をパラメータとして設定する必要がない。その代わりに、想定される目標の最大加速度に対して許容される予測位置の定常追従誤差を指定することにより、固定の速度ゲインの値を決定する方法をとっている。さらにその決定された速度ゲインを用いた場合に、等速直線運動時の、観測雑音に起因する追尾誤差の雑音成分を最小化するように位置ゲインを自動的に算出する仕組みをとっている。
【００６０】
この結果、目標の最大の加速度運動に対する追尾誤差を許容範囲内に収めつつ、等速直線運動時の追尾結果の安定化を図ることができ、結果的に容易に最適な追尾性能を実現することができるとの利点を有する。
【産業上の利用可能性】
【００６１】
この発明は、例えば観測値に基づいて移動体の運動諸元を推定する追尾方法及びその装置に適用することが可能である。
【図面の簡単な説明】
【００６２】
【図１】この発明の実施の形態による追尾装置の構成を示すブロック図である。
【図２】この発明の実施の形態による追尾装置の処理のフローチャートである。
【図３】この発明の実施の形態による追尾装置の目標の運動例を示す図である。
【符号の説明】
【００６３】
１３ 予測手段、
１８ 予測誤差共分散行列算出手段、
１９ 平滑誤差共分散行列算出手段、
２３ ゲイン行列算出手段、
１５ 平滑手段。
【特許請求の範囲】
【請求項１】
目標の位置と速度を推定する追尾方法であって、
第１の時間の平滑値ベクトルから第２の時間の予測値ベクトルを算出する予測ステップと、
第１の時間の平滑誤差共分散行列から第２の時間の予測誤差共分散行列を算出する予測誤差共分散行列算出ステップと、
第２の時間の予測誤差共分散行列と第２の時間のゲイン行列と所定の観測雑音共分散行列とに基づいて第２の時間の平滑誤差共分散行列を算出する平滑誤差共分散行列算出ステップと、
第２の時間の予測誤差共分散行列と前記所定の観測雑音共分散行列から、第２の時間の予測値ベクトルに含まれる目標の位置についてのゲインである位置ゲインと第２の時間の予測値ベクトルに含まれる目標の速度についてのゲインである速度ゲインとを含む第２の時間のゲイン行列を算出するゲイン行列算出ステップと、
第２の時間の観測値を取得し、取得した観測値と第２の時間の予測値ベクトルと第２の時間のゲイン行列とに基づいて第２の時間の平滑値ベクトルを算出する平滑ステップと、
を有する追尾方法において、
前記ゲイン行列算出ステップで、前記速度ゲインとして固定値のゲインを含む第２の時間のゲイン行列を算出することを特徴とする追尾方法。
【請求項２】
ゲイン行列算出ステップで、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項１に記載の追尾方法。
【請求項３】
ゲイン行列算出ステップで、目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項１に記載の追尾方法。
【請求項４】
ゲイン行列算出ステップで、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差と前記目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差との重み付け平均を最小化するような位置ゲインを算出することを特徴とした請求項１に記載の追尾方法。
【請求項５】
目標の位置と速度を推定する追尾装置であって、
第１の時間の平滑値ベクトルから第２の時間の予測値ベクトルを算出する予測手段と、
第１の時間の平滑誤差共分散行列から第２の時間の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、
第２の時間の予測誤差共分散行列と第２の時間のゲイン行列と所定の観測雑音共分散行列とに基づいて第２の時間の平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段と、
第２の時間の予測誤差共分散行列と前記所定の観測雑音共分散行列から、第２の時間の予測値ベクトルに含まれる目標の位置についてのゲインである位置ゲインと第２の時間の予測値ベクトルに含まれる目標の速度についてのゲインである速度ゲインとを含む第２の時間のゲイン行列を算出するゲイン行列算出手段と、
第２の時間の観測値を取得し、取得した観測値と第２の時間の予測値ベクトルと第２の時間のゲイン行列とに基づいて第２の時間の平滑値ベクトルを算出する平滑手段と、
を備えた追尾装置において、
前記ゲイン行列算出手段は、前記速度ゲインとして固定値のゲインを含む第２の時間のゲイン行列を算出することを特徴とする追尾装置。
【請求項６】
ゲイン行列算出手段は、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項５に記載の追尾装置。
【請求項７】
ゲイン行列算出手段は、目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項５に記載の追尾装置。
【請求項８】
ゲイン行列算出手段は、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差と前記目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差との重み付け平均を最小化するような位置ゲインを算出することを特徴とした請求項５に記載の追尾装置。
【請求項１】
目標の位置と速度を推定する追尾方法であって、
第１の時間の平滑値ベクトルから第２の時間の予測値ベクトルを算出する予測ステップと、
第１の時間の平滑誤差共分散行列から第２の時間の予測誤差共分散行列を算出する予測誤差共分散行列算出ステップと、
第２の時間の予測誤差共分散行列と第２の時間のゲイン行列と所定の観測雑音共分散行列とに基づいて第２の時間の平滑誤差共分散行列を算出する平滑誤差共分散行列算出ステップと、
第２の時間の予測誤差共分散行列と前記所定の観測雑音共分散行列から、第２の時間の予測値ベクトルに含まれる目標の位置についてのゲインである位置ゲインと第２の時間の予測値ベクトルに含まれる目標の速度についてのゲインである速度ゲインとを含む第２の時間のゲイン行列を算出するゲイン行列算出ステップと、
第２の時間の観測値を取得し、取得した観測値と第２の時間の予測値ベクトルと第２の時間のゲイン行列とに基づいて第２の時間の平滑値ベクトルを算出する平滑ステップと、
を有する追尾方法において、
前記ゲイン行列算出ステップで、前記速度ゲインとして固定値のゲインを含む第２の時間のゲイン行列を算出することを特徴とする追尾方法。
【請求項２】
ゲイン行列算出ステップで、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項１に記載の追尾方法。
【請求項３】
ゲイン行列算出ステップで、目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項１に記載の追尾方法。
【請求項４】
ゲイン行列算出ステップで、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差と前記目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差との重み付け平均を最小化するような位置ゲインを算出することを特徴とした請求項１に記載の追尾方法。
【請求項５】
目標の位置と速度を推定する追尾装置であって、
第１の時間の平滑値ベクトルから第２の時間の予測値ベクトルを算出する予測手段と、
第１の時間の平滑誤差共分散行列から第２の時間の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、
第２の時間の予測誤差共分散行列と第２の時間のゲイン行列と所定の観測雑音共分散行列とに基づいて第２の時間の平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段と、
第２の時間の予測誤差共分散行列と前記所定の観測雑音共分散行列から、第２の時間の予測値ベクトルに含まれる目標の位置についてのゲインである位置ゲインと第２の時間の予測値ベクトルに含まれる目標の速度についてのゲインである速度ゲインとを含む第２の時間のゲイン行列を算出するゲイン行列算出手段と、
第２の時間の観測値を取得し、取得した観測値と第２の時間の予測値ベクトルと第２の時間のゲイン行列とに基づいて第２の時間の平滑値ベクトルを算出する平滑手段と、
を備えた追尾装置において、
前記ゲイン行列算出手段は、前記速度ゲインとして固定値のゲインを含む第２の時間のゲイン行列を算出することを特徴とする追尾装置。
【請求項６】
ゲイン行列算出手段は、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項５に記載の追尾装置。
【請求項７】
ゲイン行列算出手段は、目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差を最小化するような位置ゲインを算出し、この位置ゲインを含む第２の時間のゲイン行列を算出することを特徴とした請求項５に記載の追尾装置。
【請求項８】
ゲイン行列算出手段は、目標が等速直線運動する場合の第２の時間の平滑値ベクトルの平均２乗誤差と前記目標が等速直線運動する場合の第２の時間の予測値ベクトルの平均２乗誤差との重み付け平均を最小化するような位置ゲインを算出することを特徴とした請求項５に記載の追尾装置。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２００８−７６０９６（Ｐ２００８−７６０９６Ａ）
【公開日】平成２０年４月３日（２００８．４．３）
【国際特許分類】
【出願番号】特願２００６−２５２９７８（Ｐ２００６−２５２９７８）
【出願日】平成１８年９月１９日（２００６．９．１９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２０年４月３日（２００８．４．３）
【国際特許分類】
【出願日】平成１８年９月１９日（２００６．９．１９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]