推定装置及びコンピュータプログラム及び推定方法
【課題】処理時間を低減しつつ目標航跡を精度良く抽出する。
【解決手段】航跡候補生成部１４０は、目標航跡を記述するパラメータのうちいくつかを固定した上でパラメータの候補を多数生成する。相関ゲート設定部１５０は、航跡候補生成部１４０が生成したパラメータ候補におけるいくつかの固定されたパラメータと目標の真のパラメータとの誤差を最大限考慮した相関ゲートを設定する。概探索部１６０は、航跡候補生成部１４０が生成したパラメータ候補と、観測データ管理部１３０が取り出した観測データとから、相関ゲート設定部１５０が設定した相関ゲートを用いてパラメータ候補の尤度を計算し、尤度が最大となるパラメータ候補を抽出する。詳細探索部１７０は、概探索部１６０が抽出したパラメータ候補を初期値として、尤度が大きくなる方向にパラメータを更新し、相関ゲートが小さくなるように更新して、尤度が最大となるパラメータを探索する。
【解決手段】航跡候補生成部１４０は、目標航跡を記述するパラメータのうちいくつかを固定した上でパラメータの候補を多数生成する。相関ゲート設定部１５０は、航跡候補生成部１４０が生成したパラメータ候補におけるいくつかの固定されたパラメータと目標の真のパラメータとの誤差を最大限考慮した相関ゲートを設定する。概探索部１６０は、航跡候補生成部１４０が生成したパラメータ候補と、観測データ管理部１３０が取り出した観測データとから、相関ゲート設定部１５０が設定した相関ゲートを用いてパラメータ候補の尤度を計算し、尤度が最大となるパラメータ候補を抽出する。詳細探索部１７０は、概探索部１６０が抽出したパラメータ候補を初期値として、尤度が大きくなる方向にパラメータを更新し、相関ゲートが小さくなるように更新して、尤度が最大となるパラメータを探索する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、入力データの時系列から目標航跡を抽出することにより目標の航跡を推定し目標を追尾する推定装置（追尾装置）に関する。
【背景技術】
【０００２】
センサによって取得された、多数の不要信号を含む入力データの時系列から、目標航跡を抽出する方式として、ＭＬ−ＰＤＡ（Ｍａｘｉｍｕｍ Ｌｉｋｅｌｉｈｏｏｄ Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｄａｔａ Ａｓｓｏｃｉａｔｉｏｎ）がある。ＭＬ−ＰＤＡでは、目標の位置や速度などをあらかじめ仮定し、最尤法により目標航跡を抽出する。
ＭＬ−ＰＤＡにおいて、パラメータ探索を効率化する方式として、格子点探索方式による概探索と、概探索の結果を初期値とする準Ｎｅｗｔｏｎ法等の最適化アルゴリズムによる詳細探索とを組み合わせる方式がある。
更に、概探索を効率化する方式として、最初は粗く格子点を生成し、評価関数が予め設定した閾値以上となった格子点を含む領域において再び格子点を生成するということを繰り返す方式がある。
また、航跡の位置ベクトルと観測値ベクトルとの差が小さいほど評価関数値が増大することを利用して、格子点の位置成分を観測値ベクトルに置き換えることにより、速度や加速度など、直接観測できない高次のパラメータについてのみ格子点を生成する方式がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００８−９６１３７号公報
【非特許文献】
【０００４】
【非特許文献１】Ｔ．Ｋｉｒｕｂａｒａｊａｎ他「Ｐａｓｓｉｖｅ ｒａｎｇｉｎｇ ｏｆ ａ ｌｏｗ ｏｂｓｅｒｖａｂｌｅ ｂａｌｌｉｓｔｉｃ ｍｉｓｓｉｌｅ ｉｎ ａ ｇｒａｖｉｔａｔｉｏｎａｌ ｆｉｅｌｄ」ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍｓ，ｖｏｌ．３７，ｎｏ．２，４８１〜４９４ページ、２００１年。
【非特許文献２】Ｗ．Ｒ．Ｂｌａｎｄｉｎｇ他「Ｄｉｒｅｃｔｅｄ ｓｕｂｓｐａｃｅ ｓｅａｒｃｈ ＭＬ−ＰＤＡ ｗｉｔｈ ａｐｐｌｉｃａｔｉｏｎ ｔｏ ａｃｔｉｖｅ ｓｏｎａｒ ｔｒａｃｋｉｎｇ」ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍｓ，ｖｏｌ．４４，ｎｏ．１，２０１〜２１６ページ、２００８年。
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ＭＬ−ＰＤＡを用いて目標を追尾する場合、例えば目標が平面内を等速直線運動しているとすると、推定すべきパラメータ（状態量）は、目標の初期位置ベクトルと速度ベクトルであり、各々２次元の計４次元空間から尤度が最大となるパラメータを探索する。一方、目標が平面内を等加速度運動しているとすると、推定すべきパラメータに加速度ベクトルが加わるため、６次元空間からパラメータを探索する必要がある。目標が空間内を運動しているとすると、等速直線運動の場合で６次元空間、等加速度運動の場合で９次元空間からパラメータを探索する必要がある。
したがって、推定すべきパラメータの数が増えると、追尾に必要な計算量が多くなり、処理に時間がかかる。
【０００６】
格子点探索方式による概探索と、概探索の結果を初期値とする最適化アルゴリズムによる詳細探索とを組み合わせる方式において、概探索の処理時間は、生成する格子点数に依存する。また、詳細探索は、一般に概探索に比べて少ない処理で済む。探索精度を向上させるためには、概探索において格子点の数を十分多く生成する必要がある。格子点を増やすと、計算量が多くなるので、処理に時間がかかる。
【０００７】
粗く格子点を生成し、段階的に格子点生成領域を絞り込む格子点探索方式を用いる場合、概探索における推定精度を見積もることはできるが、探索空間の次元が減るわけではないため、探索の処理に時間がかかる。
また、格子点の位置成分を観測値ベクトルに置き換える格子点探索方式を用いる場合、位置成分の候補数が観測値数に依存するため、観測値数が少ない場合は格子点数を減らすことができるが、高次のパラメータについての格子点数が減るわけではないため、高次のパラメータ数が多い場合は探索の処理に時間がかかる。
【０００８】
この発明は、例えば上記のような課題を解決するためになされたものであり、処理時間を低減しつつ目標航跡を精度良く抽出することを目的とする。
【課題を解決するための手段】
【０００９】
この発明にかかる推定装置は、データを処理する処理装置と、候補算出部と、相関部と、尤度算出部と、選択部と、推定値更新部と、推定値出力部とを有し、
上記候補算出部は、目標の複数の状態量のうち所定の第一の状態量を除く他の状態量について、上記処理装置を用いて、上記他の状態量の推定値の複数の候補値を算出し、
上記相関部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出し、
上記尤度算出部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記相関部が仮定したモデル化誤差上限値と、上記相関部が抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出し、
上記候補選択部は、上記処理装置を用いて、上記尤度算出部が算出した尤度に基づいて、上記候補算出部が算出した複数の候補値のなかから、上記他の状態量の推定値を選択し、
上記推定値更新部は、上記処理装置を用いて、上記相関部が仮定した第一の状態量の推定値と上記候補選択部が選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする。
【発明の効果】
【００１０】
この発明にかかる推定装置によれば、処理時間を低減しつつ目標航跡を精度良く抽出することができる。
【図面の簡単な説明】
【００１１】
【図１】実施の形態１における追尾システム８００の全体構成の一例を示すシステム構成図。
【図２】実施の形態１における追尾装置１００のハードウェア資源の一例を示すハードウェア構成図。
【図３】実施の形態１における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図４】実施の形態１における追尾処理Ｓ６００の流れの一例を示すフローチャート図。
【図５】目標の航跡の一例を示す図。
【図６】実施の形態１における概探索ステップＳ６２０における相関ゲートと、得られる航跡との関係の一例を示す図。
【図７】実施の形態１における詳細探索ステップＳ６３０における更新前の相関ゲート及び航跡と更新後の相関ゲート及び航跡との関係の一例を示す図。
【図８】実施の形態１における詳細探索ステップＳ６３０により最終的に得られる相関ゲートと航跡との関係の一例を示す図。
【図９】実施の形態３における量子化誤差を説明するための図。
【図１０】実施の形態６における追尾処理Ｓ６００の流れの一例を示すフローチャート図。
【図１１】実施の形態７における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図１２】実施の形態７における追尾処理Ｓ６００の流れの一例を示すフローチャート図。
【発明を実施するための形態】
【００１２】
実施の形態１．
実施の形態１について、図１〜図８を用いて説明する。
【００１３】
図１は、この実施の形態における追尾システム８００の全体構成の一例を示すシステム構成図である。
追尾システム８００（推定システム）は、目標７００を観測し、観測した目標７００の航跡を推定して、目標７００を追尾する。追尾システム８００は、センサ８１０と、追尾装置１００とを有する。
センサ８１０（観測装置）は、目標を観測し、観測した観測結果を表わす信号を出力する。センサ８１０は、連続して、もしくは繰り返し、観測を行う。なお、センサ８１０は、追尾装置１００の一部であってもよい。
追尾装置１００（推定装置、航跡推定装置）は、センサ８１０が観測した観測結果に基づいて、目標７００の航跡を推定する。
【００１４】
図２は、この実施の形態における追尾装置１００のハードウェア資源の一例を示すハードウェア構成図である。
追尾装置１００は、例えば、コンピュータである。追尾装置１００は、例えば、処理装置９１１と、入力装置９１２と、出力装置９１３と、記憶装置９１４とを有する。
処理装置９１１は、記憶装置９１４が記憶したプログラムを実行することにより、データを処理し、追尾装置１００全体を制御する。
記憶装置９１４は、処理装置９１１が実行するプログラムや、処理装置９１１が処理するデータなどを記憶する。記憶装置９１４は、例えば、揮発性メモリ、不揮発性メモリ、半導体メモリ、磁気ディスク装置、光ディスク装置などである。
入力装置９１２は、追尾装置１００の外部から信号を入力し、入力した信号を処理装置９１１が処理できるデータに変換する。入力装置９１２が変換したデータは、処理装置９１１が直接処理する構成でもよいし、一時的に記憶装置９１４が記憶する構成でもよい。入力装置９１２は、例えば、アナログ信号をデジタルデータに変換するアナログデジタル変換装置、他の装置が送信した信号を受信して復調する受信装置、操作者の操作を入力するキーボードやマウスなどの操作入力装置などである。
出力装置９１３は、処理装置９１１が処理したデータや記憶装置９１４が記憶したデータなどを変換して、追尾装置１００の外部に出力する。出力装置９１３は、例えば、デジタルデータをアナログ信号に変換するデジタルアナログ変換装置、他の装置に対する信号を送信する送信装置、操作者に対して情報を提示する表示装置・音声出力装置などである。
以下に説明する追尾装置１００の機能ブロックは、記憶装置９１４が記憶したプログラムを処理装置９１１が実行することにより実現される。なお、これは一例であり、追尾装置１００の機能ブロックは、他の方式で実現したものであってもよい。追尾装置１００の機能ブロックは、例えば、アナログ回路やデジタル回路などの電気的構成で実現したものであってもよいし、機械的構成など他の構成により実現したものであってもよい。また、追尾装置１００は、物理的に１つの装置である必要はなく、物理的に複数の装置が、それぞれ追尾装置１００の機能ブロックのうちのいくつかを実現し、全体として追尾装置１００として機能する構成であってもよい。
【００１５】
図３は、この実施の形態における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図である。
追尾装置１００は、例えば、観測データ抽出部１１０と、観測データ蓄積部１２０と、観測データ管理部１３０と、航跡候補生成部１４０と、相関ゲート設定部１５０と、概探索部１６０と、詳細探索部１７０とを有する。
【００１６】
観測データ抽出部１１０は、入力装置９１２を用いて、センサ８１０が出力した信号（入力データ）を入力する。観測データ抽出部１１０は、処理装置９１１を用いて、入力した信号から、目標を観測した観測値（観測データ）を抽出する。なお、センサ８１０が出力する信号は、ノイズなどの影響による不要信号を含むので、観測データ抽出部１１０が抽出する観測値は、必ずしも目標を観測した観測値ではない可能性がある。
観測データ蓄積部１２０は、記憶装置９１４を用いて、観測データ抽出部１１０が抽出した観測値を、センサ８１０が観測した観測時刻（フレーム番号）と対応づけて蓄積して記憶する。
観測データ管理部１３０は、処理装置９１１を用いて、目標の航跡の推定に使用する観測値の観測時刻の範囲を決定する。観測データ管理部１３０は、観測データ蓄積部１２０が記憶した観測値のなかから、観測時刻が決定した範囲内である観測値（観測データ時系列）を抽出する。
航跡候補生成部１４０は、処理装置９１１を用いて、目標の航跡の候補を生成する。航跡候補生成部１４０は、複数の候補（航跡候補群）を生成する。
相関ゲート設定部１５０は、処理装置９１１を用いて、相関ゲート行列を設定・更新する。
概探索部１６０は、処理装置９１１を用いて、観測データ管理部１３０が抽出した観測値と、相関ゲート設定部１５０が設定した相関ゲート行列とに基づいて、航跡候補生成部１４０が生成した候補のなかから、実際の目標の航跡に近い航跡（航跡候補）を抽出する。
詳細探索部１７０は、処理装置９１１を用いて、観測データ管理部１３０が抽出した観測値と、相関ゲート設定部１５０が設定した相関ゲート行列とに基づいて、概探索部１６０が抽出した航跡から出発して、更に実際の目標の航跡に近い航跡を算出する。詳細探索部１７０は、相関ゲート設定部１５０が更新した相関ゲート行列に基づいて、更新した航跡から更に実際の目標の航跡に近い航跡を算出する。詳細探索部１７０は、航跡が収束するまでこれを繰り返す。詳細探索部１７０は、処理装置９１１を用いて、算出した航跡（航跡推定値）を出力する。
【００１７】
図４は、この実施の形態における追尾処理Ｓ６００の流れの一例を示すフローチャート図である。
追尾処理Ｓ６００（推定方法、航跡推定方法）において、追尾装置１００は、センサ８１０が観測した観測結果に基づいて、目標を追尾し、目標の航跡を推定する。追尾処理Ｓ６００は、例えば、データ入力ステップＳ６１１と、観測データ抽出・蓄積ステップＳ６１２と、使用フレーム選択ステップＳ６１３と、相関ゲート設定ステップＳ６１４と、航跡候補設定ステップＳ６１５と、概探索ステップＳ６２０と、詳細探索ステップＳ６３０と、相関ゲート更新ステップＳ６４３と、ゲーティングステップＳ６４４と、尤度計算ステップＳ６４５と、航跡推定値出力ステップＳ６５１とを有する。追尾装置１００は、追尾処理Ｓ６００をデータ入力ステップＳ６１１から開始する。
【００１８】
まず、データ入力ステップＳ６１１において、センサ８１０は、データを取得する。センサ８１０は、例えば、レーダ、赤外線センサ、光学カメラなどである。
次に、観測データ抽出・蓄積ステップＳ６１２において、観測データ抽出部１１０は、入力データから観測データを抽出する。観測データ蓄積部１２０は、観測データを観測された時刻ごとに蓄積する。観測データは、例えば、２次元画像の位置座標である。観測データを抽出する方式としては、例えば、輝度値が予め設定した閾値を超えた画素を抽出する方式、輝度値が上位の画素を一定数個抽出する方式などがある。
【００１９】
次に、使用フレーム選択ステップＳ６１３において、観測データ管理部１３０は、航跡の推定に使用する時刻フレーム（観測時刻）を任意に選択する。選択された時刻フレームをｔ１、ｔ２、…、ｔＮとする。ただし、ｕ＜ｖなる任意のｕとｖについてｔｕ＜ｔｖであるとする。
観測データ管理部１３０は、観測データ蓄積部１２０が蓄積した観測データ群から前記時刻フレームにおける式（１）で表わされる観測データ時系列ＺＮを取り出す。
【数１１】
ただし、Ｚｋは、時刻ｔｋにおける観測データである。時刻ｔｋにおける観測データＺｋは、Ｍｋ個の観測値ベクトルから構成される。また、ｚｋ，ｊは、時刻ｔｋにおける観測値ベクトルである。ｘｋ，ｊ，ｙｋ，ｊは、それぞれｘ座標、ｙ座標を表わす。
【００２０】
次に、相関ゲート設定ステップＳ６１４において、相関ゲート設定部１５０は、式（２）で表わされる相関ゲート行列Ｓｋを設定する。
【数１２】
ただし、Ｒｋは、観測雑音共分散行列である。観測雑音共分散行列Ｒｋは、正定値対称行列であり、例えば、観測値ベクトルの観測精度の２乗値が対角成分に並んだ行列である。
Ｑｋは、モデル化誤差共分散行列である。モデル化誤差共分散行列Ｑｋは、半正定値対称行列であり、例えば、式（３）で表わされる、想定される最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を加速度として目標が移動した場合の目標の変位量の２乗値が対角成分に並んだ行列である。
【数１３】
ただし、ｔＰは、目標の位置、速度、加速度の初期値を定める基準時刻である。基準時刻ｔＰは、例えば、観測開始時刻ｔ１や、最新時刻ｔＮなどである。
また、最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）の値は事前に設定しておく。最大加速度誤差ベクトルａｍａｘの値は、例えば、推定したい目標の種類に応じて設定した値を用いる。
【００２１】
次に、航跡候補設定ステップＳ６１５において、航跡候補生成部１４０は、概探索のための航跡候補群を作成する。ここで、航跡は、基準時刻ｔＰにおける目標の位置ベクトル、速度ベクトル、加速度ベクトルを成分に持つ状態ベクトルｘＰとして、式（４）で記述される。
【数１４】
ただし、（ｘＰ，ｙＰ）は、基準時刻ｔＰにおける目標の位置ベクトルである。（ｘＰドット，ｙＰドット）は、基準時刻ｔＰにおける目標の速度ベクトルである。（ｘＰツードット，ｙＰツードット）は、基準時刻ｔＰにおける目標の加速度ベクトルである。目標の位置ベクトル、速度ベクトル、及び加速度ベクトルは、いずれも、ｘｙ座標で表わす。また、上付きのＴは行列の転置を表わす。
【００２２】
航跡候補生成部１４０は、加速度ベクトルの候補を、ある固定値ａｆｉｘ＝（ａｘｆｉｘ，ａｙｆｉｘ）とした上で、あらかじめ定められた範囲における航跡候補、すなわち状態ベクトル候補を複数個生成する。状態ベクトル候補を生成する範囲は、例えば、位置成分は、センサの探知可能領域の範囲、速度成分は、目標の想定される移動方向に沿った値の範囲である。ここで、航跡候補生成部１４０は、位置成分、速度成分が前記範囲内で等間隔になるように生成する構成であってもよいし、乱数を用いて不等間隔に生成する構成であってもよい。また、航跡候補生成部１４０は、位置成分の候補については、観測値ベクトルを使う構成であってもよい。
また、加速度ベクトル候補の固定値ａｆｉｘは、事前に設定しておく。加速度ベクトル候補の固定値ａｆｉｘは、例えば、０［ｍ／ｓ２］である。あるいは、航跡候補生成部１４０は、前回の推定結果の加速度ベクトル推定値を、加速度ベクトル候補の固定値ａｆｉｘとして用いる構成であってもよい。
【００２３】
次に、概探索ステップＳ６２０において、概探索部１６０は、概探索をする。概探索ステップＳ６２０は、状態遷移ステップＳ６２３と、ゲーティングステップＳ６２４と、尤度計算ステップＳ６２５と、最大尤度比航跡候補抽出ステップＳ６２７とを有する。概探索部１６０は、航跡候補生成部１４０が生成した航跡候補それぞれについて、状態遷移ステップＳ６２３、ゲーティングステップＳ６２４、尤度計算ステップＳ６２５を実行する。
【００２４】
概探索ステップＳ６２０の状態遷移ステップＳ６２３において、概探索部１６０は、航跡候補ごとに、式（５）にしたがって、各時刻ｔｋにおける状態ベクトルｘｋを計算する。
【数１５】
ただし、Ｉ２は２行２列の単位行列である。０２は、２行２列の零行列である。
【００２５】
次に、概探索ステップＳ６２０のゲーティングステップＳ６２４において、概探索部１６０は、航跡候補ごとに、各時刻ｔｋにおける状態ベクトルｘｋの位置成分と、相関ゲート設定部１５０が設定した相関ゲート行列Ｓｋに基づいて、楕円形状の相関ゲートＧｋを、式（６）を使って設定する。
【数１６】
ただし、Ｈは、状態ベクトルから位置成分を取り出す観測行列である。また、ｄは、事前に設定するゲートサイズパラメータである。ゲートサイズパラメータｄの値は、自由度２のカイ２乗分布より定まる正定数である。上付きのＴは、転置を表わす。上付きの−１は、逆行列を表わす。
【００２６】
次に、概探索ステップＳ６２０の尤度計算ステップＳ６２５において、概探索部１６０は、航跡候補ごとに、各時刻ｔｋにおける状態ベクトルｘｋと、相関ゲートＧｋに含まれる観測値ベクトルｚｋ，ｊとに基づいて、式（７）を使って尤度φを計算する。
【数１７】
ただし、ＰＤは、事前に設定する探知確率パラメータである。ＰＤは、０＜ＰＤ＜１なる定数である。また、βは、事前に設定する不要信号密度パラメータである。βは、正定数である。また、ｍｋは時刻ｔｋにおける観測データＺｋを構成するＭｋ個の観測値ベクトルのうち、式（６）の相関ゲートＧｋに含まれる観測値ベクトルの個数である。また、ｚｋ，ｊは、前記観測データＺｋを構成する観測値ベクトルのうち、式（６）の相関ゲートＧｋに含まれるものである。ｌｎは、自然対数（ネイピア数を底とする対数関数）を表わす。ｄｅｔは、行列式を表わす。ｅｘｐは、ネイピア数を底とする指数関数を表わす。
【００２７】
なお、式（６）及び式（７）は、目標の位置観測誤差の分布が正規分布で与えられる場合についての相関ゲート及び尤度の計算式の例である。観測誤差の分布が正規分布以外である場合などにおいては、相関ゲートは楕円形状以外の形状である構成であってもよい。また、尤度の計算式は任意に設定する構成であってもよい。例えば、尤度の計算式は、観測値ベクトルと状態ベクトルの位置成分との距離の逆２乗値を用いる構成であってもよい。また、尤度の計算式は、観測データの振幅情報や輝度情報に基づくものを用いる構成であってもよい。
【００２８】
概探索部１６０は、状態遷移ステップＳ６２３、ゲーティングステップＳ６２４、尤度計算ステップＳ６２５を繰り返し、航跡候補生成部１４０が生成したすべての航跡候補を処理する。
【００２９】
次に、概探索ステップＳ６２０の最大尤度比航跡候補抽出ステップＳ６２７において、概探索部１６０は、航跡候補の中から、尤度φが最大となるものを抽出する。そして、詳細探索部１７０は、最大尤度となる航跡候補（状態ベクトル候補）を、状態ベクトルの初期値として入力する。
【００３０】
次に、詳細探索ステップＳ６３０において、詳細探索部１７０は、詳細探索をする。詳細探索ステップＳ６３０は、探索方向計算ステップＳ６３１と、航跡更新ステップＳ６３２と、状態遷移ステップＳ６３３と、ゲーティングステップＳ６３４と、尤度計算ステップＳ６３５と、探索収束条件分岐ステップＳ６３６とを有する。
【００３１】
詳細探索ステップＳ６３０の探索方向計算ステップＳ６３１において、詳細探索部１７０は、前回の探索における状態ベクトルを起点として、より尤度φが大きくなる方向ベクトルを計算する。探索方向の計算方式は、例えば、最急降下法、Ｎｅｗｔｏｎ法、準Ｎｅｗｔｏｎ法、共役勾配法などがある。
【００３２】
次に、詳細探索ステップＳ６３０の航跡更新ステップＳ６３２において、詳細探索部１７０は、前回の探索における状態ベクトルを、探索方向計算ステップＳ６３１で計算した方向ベクトルを用いて、前回の探索よりも尤度の大きい状態ベクトルに更新する。
【００３３】
次に、詳細探索ステップＳ６３０の状態遷移ステップＳ６３３、ゲーティングステップＳ６３４及び尤度計算ステップＳ６３５において、詳細探索部１７０は、状態遷移、ゲーティング、尤度計算を行う。状態遷移ステップＳ６３３、ゲーティングステップＳ６３４及び尤度計算ステップＳ６３５における詳細探索部１７０の動作は、概探索の対応するステップ（状態遷移ステップＳ６２３、ゲーティングステップＳ６２４及び尤度計算ステップＳ６２５）における概探索部１６０の動作と同一である。
【００３４】
そして、詳細探索ステップＳ６３０の探索収束条件分岐ステップＳ６３６において、詳細探索部１７０は、探索の収束を判定する。探索の収束判定方式には、例えば、尤度勾配ベクトルのノルムが十分小さい所定の閾値より小さくなった場合に収束したと判定する方式、前回の探索の尤度と今回の探索の尤度との差が十分小さい所定の閾値より小さくなった場合に収束したと判定する方式、探索収束条件分岐ステップＳ６３６を通過した回数があらかじめ定めた上限値を超えた場合に収束したと判定する方式などがある。
探索が収束したと判定しなかった場合、詳細探索部１７０は、相関ゲート更新ステップＳ６４３へ処理を進める。
探索が収束したと判定した場合、詳細探索部１７０は、航跡推定値出力ステップＳ６５１へ処理を進める。
【００３５】
相関ゲート更新ステップＳ６４３において、相関ゲート設定部１５０は、相関ゲート行列Ｓｋを更新する。例えば、相関ゲート設定部１５０は、探索によって得られた加速度ベクトル推定値ａｅｓｔ＝（ａｘｅｓｔ，ａｙｅｓｔ）、加速度ベクトル固定値ａｆｉｘ＝（ａｘｆｉｘ，ａｙｆｉｘ）、及び想定される最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を用いて、式（８）のように、相関ゲート行列Ｓｋを更新する。
【数１８】
【００３６】
最大加速度誤差ベクトルａｍａｘは、加速度ベクトル固定値ａｆｉｘと実際の目標加速度ベクトルとの差の最大許容量を表すものである。また、加速度ベクトル固定値ａｆｉｘと加速度ベクトル推定値ａｅｓｔとの差は、加速度ベクトル固定値ａｆｉｘが実際の目標加速度ベクトルに近づくための補正量を表すものである。したがって、前記補正量の分だけ加速度誤差が減少したと考えられる。そこで、前記最大許容量から前記補正量を減算することにより、モデル化誤差共分散行列Ｑｋの大きさを小さくする。そして、更新後の加速度誤差最大許容量（ａハットｘｍａｘ，ａハットｙｍａｘ）を新たな最大加速度誤差ベクトルａハットｍａｘとして設定する。ただし、式（９）の右辺が負値となる場合、相関ゲート設定部１５０は、更新後の加速度誤差最大許容量ａハットｘｍａｘあるいはａハットｙｍａｘを、０［ｍ／ｓ２］とする。
【００３７】
そして、ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５において、詳細探索部１７０は、更新された相関ゲート行列Ｓｋを用いて、ゲーティング及び尤度計算を行う。ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５における詳細探索部１７０の動作は、詳細探索ステップＳ６３０のゲーティングステップＳ６３４及び尤度計算ステップＳ６３５における動作と同様である。
そして、詳細探索部１７０は、再び、詳細探索ステップＳ６３０に処理を戻し、探索方向計算ステップＳ６３１以降の動作を行う。
【００３８】
探索収束条件分岐ステップＳ６３６で探索が収束したと判定した場合、航跡推定値出力ステップＳ６５１において、詳細探索部１７０は、航跡推定値として探索した状態ベクトルを出力する。
【００３９】
図５は、目標の航跡の一例を示す図である。
例えば、目標が一定の加速度で等加速度運動をしているものとする。航跡候補生成部１４０が生成した航跡の候補における加速度は、実際の目標の加速度と異なっている。
【００４０】
図６は、この実施の形態における概探索ステップＳ６２０における相関ゲートと、得られる航跡との関係の一例を示す図である。
この例において、基準時刻ｔＰは、時刻ｔ１である。観測時刻と基準時刻ｔＰとの差が大きくなるにつれて、相関ゲートが大きくなる。これにより、加速度の固定値と実際の目標の加速度との間の差が大きい場合でも、観測値が相関ゲートに入るので、どの航跡の候補が実際の航跡に最も近いかを判定することができる。
【００４１】
図７は、この実施の形態における詳細探索ステップＳ６３０における更新前の相関ゲート及び航跡と更新後の相関ゲート及び航跡との関係の一例を示す図である。
詳細探索部１７０は、基準時刻における目標の位置、速度及び加速度の推定値を更新することにより、推定航跡を実際の目標の航跡に近づける。これにより、相関ゲートを小さくしても、観測値が相関ゲートに入るようになる。
相関ゲート設定部１５０が小さくした相関ゲートに基づいて、詳細探索部１７０が航跡を更新することにより、更に、推定航跡を実際の目標の航跡に近づけることができる。
【００４２】
図８は、この実施の形態における詳細探索ステップＳ６３０により最終的に得られる相関ゲートと航跡との関係の一例を示す図である。
航跡の更新と相関ゲートの更新とを、航跡が収束するまで繰り返すことにより、最終的に、実際の目標の航跡にほぼ一致する航跡を推定することができる。
【００４３】
この実施の形態における追尾装置１００は、入力データの時系列から目標航跡を抽出することによって目標を追尾する。
追尾装置１００は、観測データ抽出部１１０と、観測データ蓄積部１２０と、観測データ管理部１３０と、航跡候補生成部１４０と、相関ゲート設定部１５０と、概探索部１６０と、詳細探索部１７０とを有する。
前記観測データ抽出部１１０は、時刻ごとに入力データから観測データを抽出する。
前記観測データ蓄積部１２０は、前記観測データを時刻ごとに蓄積する。
前記観測データ管理部１３０は、指定された複数の時刻の観測データを前記観測データ蓄積部１２０から取り出す。
前記航跡候補生成部１４０は、目標航跡を記述するパラメータのうちいくつかを固定した上でパラメータの候補を多数生成する。
前記相関ゲート設定部１５０は、前記航跡候補生成部１４０で生成されたパラメータ候補におけるいくつかの固定されたパラメータと目標の真のパラメータとの誤差を最大限考慮した相関ゲートを設定する。
前記概探索部１６０は、前記航跡候補生成部１４０で生成されたパラメータ候補と、前記観測データ管理部１３０で取り出された観測データとから、前記相関ゲート設定部１５０で設定された相関ゲートを用いて前記パラメータ候補の尤度を計算するとともに、前記尤度が最大となるパラメータ候補を抽出する。
前記詳細探索部１７０は、前記概探索部１６０で抽出された尤度が最大となるパラメータ候補を初期値として、前記尤度が大きくなる方向にパラメータを更新しながら、前記相関ゲートをより小さくなるように更新して、前記尤度が最大となるパラメータを探索することにより、目標航跡を記述するパラメータの推定値を出力する。
【００４４】
前記詳細探索部１７０は、前記尤度が大きくなる方向にパラメータを更新するたびに、前記相関ゲートをより小さくなるように更新することを繰り返す。
【００４５】
前記航跡候補生成部１４０は、航跡を記述するパラメータとして目標の位置、速度、加速度を設定するとともに、固定パラメータとして加速度を選択する。
前記相関ゲート設定部１５０は、前記航跡候補生成部１４０で固定された加速度と目標の真の加速度との誤差を最大限考慮した相関ゲートを設定する。
【００４６】
前記相関ゲート設定部１５０は、目標の観測誤差共分散行列と、目標の真の加速度との最大誤差許容量を考慮した共分散行列を用いた式（２）のような相関ゲート行列を用いて相関ゲートを規定する。
【００４７】
前記相関ゲート設定部１５０は、目標の真の加速度との最大誤差許容量を考慮した共分散行列として式（３）を用いる。
【００４８】
前記概探索部１６０及び前記詳細探索部１７０は、前記相関ゲート行列を用いて相関ゲートを式（６）のように設定し、パラメータの尤度を式（７）のように計算する。
【００４９】
前記詳細探索部１７０は、前記相関ゲートを式（８）のように更新する。
【００５０】
この実施の形態における追尾装置１００は、設定した加速度固定値と実際の目標加速度との差をモデル化誤差として表現し、モデル化誤差を考慮した相関ゲートを設定して概探索を行う。これにより、設定した加速度固定値が実際と異なっていても目標観測値を相関ゲート内に捕捉することができる。
さらに、この実施の形態における追尾装置１００は、概探索においては位置ベクトルと速度ベクトルのみを探索し、加速度ベクトルのような高次のパラメータについては探索を行わない。これにより、加速度ベクトルも探索する場合に比べて航跡候補数を減らすことができ、処理時間を削減することができる。
さらに、この実施の形態における追尾装置１００は、詳細探索においては加速度誤差を表すモデル化誤差共分散行列の大きさを、推定結果の加速度を基に計算した補正量を用いて段階的に小さくしながら、加速度ベクトルのような高次のパラメータも含めて探索を行う。これにより、相関ゲートを目標観測値の存在する近傍に絞り込みつつ、実際の目標の位置ベクトル、速度ベクトル、及び加速度ベクトルを精度良く推定することが可能となる。
【００５１】
なお、この実施の形態では、平面内を等加速度運動する目標を追尾する構成について説明したが、追尾装置１００は、他の運動モデルに基づいて目標を追尾する構成であってもよい。例えば、目標の位置、速度、加速度などを二次元ベクトルで表わすのではなく、三次元ベクトルで表わす構成であってもよい。あるいは、目標が、等加速度運動ではなく、例えば等速円運動など他の運動をしていると仮定して、目標を追尾する構成であってもよい。
【００５２】
実施の形態２．
実施の形態２について、説明する。
なお、実施の形態１と共通する部分については、説明を省略する。
【００５３】
実施の形態１における相関ゲート設定部１５０は、式（８）のように加速度ベクトル推定結果を用いて相関ゲート行列Ｓｋを更新する。これに対し、この実施の形態における相関ゲート設定部１５０は、加速度ベクトル推定結果を用いずに、相関ゲート行列Ｓｋを更新する。例えば、相関ゲート設定部１５０は、想定される最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を用いて、式（１０）のように、相関ゲート行列Ｓｋを更新する。
【数１９】
ただし、γは、事前に設定する減衰係数パラメータである。γは、０＜γ＜１なる定数である。また、ｎは、詳細探索の反復回数（探索収束条件分岐ステップＳ６３６を通過した回数）である。
【００５４】
この実施の形態における追尾装置１００において、前記詳細探索部１７０は、前記相関ゲートを式（１０）のように更新する。
【００５５】
この実施の形態における追尾装置１００は、式（１０）による相関ゲート行列の更新をして、加速度ベクトルの推定結果に関わらず、一定率でモデル化誤差共分散行列Ｑｋの大きさを減少させる。これにより、詳細探索における相関ゲートの絞り込みを容易にすることができる。
【００５６】
実施の形態３．
実施の形態３について、図９を用いて、説明する。
なお、実施の形態１及び実施の形態２と共通する部分については、説明を省略する。
【００５７】
実施の形態１における概探索部１６０は、式（２）のような相関ゲート行列を設定する。これに対し、この実施の形態における概探索部１６０は、格子点探索方式によって概探索を行う場合、探索空間を量子化することと同等であることから、状態ベクトル候補には観測誤差の他に量子化誤差が含まれると考え、式（１１）のような相関ゲート行列を設定する。
【数２０】
ただし、Ｑ’ｋは量子化誤差共分散行列である。量子化誤差共分散行列Ｑ’ｋは、一定の半正定値対称行列である。
【００５８】
図９は、この実施の形態における量子化誤差を説明するための図である。
例えば、航跡候補生成部１４０が生成する航跡の候補において、所定の間隔を有する複数の値を、位置や速度の推定値の候補とする場合、尤度が最も高い航跡の候補を選択しても、実際の目標の航跡の位置や速度との間にずれが生じる。これが量子化誤差である。
したがって、位置や速度の推定値の候補の間の間隔から、量子化誤差の絶対値の最大値を求めることができる。
概探索部１６０は、このようにして算出した量子化誤差の絶対値の最大値に基づいて、量子化誤差協分散行列Ｑ’ｋを算出する。
【００５９】
詳細探索で探索を進めるごとに、量子化誤差の影響は小さくなる。そこで、相関ゲート設定部１５０は、例えば、式（１０）のモデル化誤差共分散行列の更新式と同様に、量子化誤差共分散行列に詳細探索の反復回数に応じて一定の減衰係数を乗ずる。相関ゲート設定部１５０は、例えば、次の式により、相関ゲート行列Ｓｋを更新する。
【数２１】
ただし、ζは、事前に設定する減衰係数パラメータである。ζは、０＜ζ＜１なる定数である。また、ｎは、詳細探索の反復回数（探索収束条件分岐ステップＳ６３６を通過した回数）である。
【００６０】
あるいは、相関ゲート設定部１５０は、例えば、次の式により、相関ゲート行列Ｓｋを更新する。
【数２２】
【００６１】
この実施の形態における追尾装置１００において、前記相関ゲート設定部１５０は、目標の観測誤差共分散行列と、目標の真の加速度との最大誤差許容量を考慮した共分散行列に加えて、さらに概探索におけるパラメータ候補の量子化誤差を考慮した誤差共分散行列を用いた式（１１）のような相関ゲート行列を用いて相関ゲートを規定する。
【００６２】
この実施の形態における追尾装置１００は、概探索において量子化誤差の影響を考慮することにより、目標航跡を精度良く抽出することができる。
【００６３】
実施の形態４．
実施の形態４について、説明する。
なお、実施の形態１〜実施の形態３と共通する部分については、説明を省略する。
【００６４】
実施の形態１における相関ゲート設定部１５０は、最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を事前に設定する。これに対し、この実施の形態における相関ゲート設定部１５０は、相関ゲートの大きさに上限値を事前に設定することにより、最大加速度誤差ベクトルａｍａｘの値を計算する。例えば、式（６）のように相関ゲートＧｋを設定する場合、相関ゲートＧｋの大きさＶｋは、式（１２）で計算することができる。
【数２３】
ただしｄｅｔは、行列式を表わす。相関ゲート設定部１５０は、このＶｋが事前に設定した上限値を超えないような最大加速度誤差ベクトルａｍａｘの値を求める。
【００６５】
この実施の形態における追尾装置１００において、前記相関ゲート設定部１５０は、前記相関ゲートの大きさに上限値を設定することにより、目標の真の加速度との最大誤差許容量を計算する。
【００６６】
これにより、最大加速度誤差ベクトルの値を相関ゲートの大きさから直感的に設定することが可能となる。
【００６７】
実施の形態５．
実施の形態５について、説明する。
なお、実施の形態１〜実施の形態４と共通する部分については、説明を省略する。
【００６８】
実施の形態１における観測データ管理部１３０は、使用フレーム選択ステップＳ６１３において、航跡の推定に使用する時刻フレームを任意に選択する。これに対し、この実施の形態における観測データ管理部１３０は、使用する時刻フレームの長さに上限値を設定した上で、上限値を超えないように時刻フレームを選択する。
【００６９】
あるいは、観測データ蓄積部１２０は、あらかじめ相関ゲートの大きさの上限を定めておき、相関ゲートの大きさが、あらかじめ定めた上限を超えないよう、時刻フレームの長さを設定する構成であってもよい。
実施の形態４で述べたように、相関ゲートの大きさは、式（１２）を使って計算することができる。実施の形態４では、最大加速度誤差ベクトルを変えることにより相関ゲートの大きさを変える構成を説明したが、相関ゲートの大きさは、基準時刻からの経過時間によっても変化する。例えば、観測データ蓄積部１２０は、最大加速度誤差ベクトルをあらかじめ定めた値に設定したうえで、相関ゲートの大きさが上限を超えないよう、基準時刻からの経過時間の絶対値の最大値を算出する。
【００７０】
あるいは、観測データ蓄積部１２０は、例えば、相関ゲートに含まれる観測値ベクトルの数に上限値を設定し、相関ゲートに含まれる観測値ベクトルの数が上限値を超えないよう、時刻フレームの長さを設定する構成であってもよい。
例えば、観測データ蓄積部１２０は、基準時刻ｔＰから最も離れた観測時刻ｔｘ（例えば、ｔＰ＝ｔ１とするとｔｘ＝ｔＮである。）において、推定結果に用いた相関ゲートに含まれる観測値ベクトルの数ｍｘが事前に設定した閾値Ｍを超えたときの時刻フレーム数Ｎを上限値として設定する。
【００７１】
この実施の形態における追尾装置１００において、前記観測データ管理部１３０は、前記相関ゲートの大きさを基に、使用する時刻の長さに上限値を指定する。
【００７２】
前記観測データ管理部１３０は、前記詳細探索部１７０が出力した結果における最も相関ゲートの大きい時刻について、相関ゲートに含まれる観測データの数が事前に設定した閾値を超えたときに使用していた時刻の長さを上限値とする。
【００７３】
これにより、使用する時刻フレームが長くなることで、基準時刻から最も離れた時刻における相関ゲートが大きくなりすぎて、目標以外の観測値ベクトルが推定結果に与える影響が大きくなることを緩和することができる。
【００７４】
実施の形態６．
実施の形態６について、図１０を用いて説明する。
なお、実施の形態１〜実施の形態５と共通する部分については、説明を省略する。
【００７５】
図１０は、この実施の形態における追尾処理Ｓ６００の流れの一例を示すフローチャート図である。
この実施の形態における追尾処理Ｓ６００は、実施の形態１における追尾処理Ｓ６００のステップに加えて、相関ゲート収束条件分岐ステップＳ６４１を有する。相関ゲート更新ステップＳ６４３は、相関ゲート収束条件分岐ステップＳ６４１の次段にある。
【００７６】
ある相関ゲート行列Ｓを用いたときの状態ベクトルｘの尤度をφ（ｘ；Ｓ）と記述する。詳細探索ステップＳ６３０の航跡更新ステップＳ６３２における更新前の状態ベクトルをｘａ、更新後の状態ベクトルをｘｂ、相関ゲート更新ステップＳ６４３における更新前の相関ゲート行列をＳａ、更新後の相関ゲート行列をＳｂとする。
航跡更新ステップＳ６３２において詳細探索部１７０は尤度が大きくなるように状態ベクトルを更新するので、φ（ｘａ；Ｓａ）≦φ（ｘｂ；Ｓａ）が成り立つ。
しかし、相関ゲート更新ステップＳ６４３において相関ゲート設定部１５０は尤度を考慮せずに相関ゲート行列を更新するので、φ（ｘｂ；Ｓａ）＞φ（ｘｂ；Ｓｂ）となる可能性がある。このため、φ（ｘａ；Ｓａ）≦φ（ｘｂ；Ｓｂ）となるとは限らない。
したがって、航跡を更新するたびに相関ゲートを更新すると、尤度が大きくならず、正しい航跡を推定できない可能性がある。詳細探索ステップＳ６３０の探索方向計算ステップＳ６３１における方向ベクトルが計算できなくなり、真の目標航跡に向けた状態ベクトルの更新が行われない可能性がある。
【００７７】
これを回避するため、詳細探索ステップＳ６３０の探索収束条件分岐ステップＳ６３６において、探索が収束していないと判定した場合、相関ゲート行列を更新せず、一定の相関ゲート行列を保ったまま、詳細探索部１７０は、探索方向計算ステップＳ６３１に処理を戻す。
探索が収束した場合、詳細探索部１７０は、相関ゲート収束条件分岐ステップＳ６４１に処理を進める。
【００７８】
相関ゲート収束条件分岐ステップＳ６４１において、相関ゲート設定部１５０は、相関ゲートが収束したか否かを判定する。相関ゲートの収束判定方式には、例えば、前回の相関ゲート行列の行列式と今回の相関ゲート行列の行列式との差が十分小さい所定の閾値よりも小さくなった場合に収束したと判定する方式、相関ゲート収束条件分岐ステップＳ６４１を通過した回数があらかじめ定めた上限値を超えた場合に収束したと判定する方式などがある。
相関ゲートが収束したと判定した場合、相関ゲート設定部１５０は、航跡推定値出力ステップＳ６５１へ処理を進める。
相関ゲートが収束したと判定しなかった場合、相関ゲート設定部１５０は、相関ゲート更新ステップＳ６４３へ処理を進める。
【００７９】
相関ゲート更新ステップＳ６４３において、相関ゲート設定部１５０は、例えば実施の形態１と同様、式（８）及び式（９）により、相関ゲート行列を更新する。なお、相関ゲート設定部１５０は、実施の形態２や実施の形態３で説明した方式など、他の方式で相関ゲート行列を更新する構成でもよい。
【００８０】
ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５において、詳細探索部１７０は、更新された相関ゲート行列を用いて、ゲーティング及び尤度計算を行う。ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５における詳細探索部１７０の動作は、詳細探索ステップＳ６３０のゲーティングステップＳ６３４及び尤度計算ステップＳ６３５における動作と同様である。
そして、詳細探索部１７０は、再び、詳細探索ステップＳ６３０に処理を戻し、探索方向計算ステップＳ６３１以降の動作を行う。
【００８１】
相関ゲート収束条件分岐ステップＳ６４１で相関ゲートが収束したと判定した場合、航跡推定値出力ステップＳ６５１において、詳細探索部１７０は、航跡推定値として探索した状態ベクトルを出力する。
【００８２】
この実施の形態における追尾装置１００において、前記詳細探索部１７０は、前記尤度が大きくなる方向にパラメータを更新することを繰り返し、尤度が十分大きくなった段階で前記相関ゲートをより小さくなるように更新して、再び尤度が大きくなる方向にパラメータを更新することを繰り返す。
【００８３】
この実施の形態における追尾装置１００は、相関ゲートの更新により尤度が減少する可能性を回避することができ、詳細探索における相関ゲートの絞り込みを容易にすることができる。
【００８４】
実施の形態７．
実施の形態７について、図１１〜図１２を用いて説明する。
なお、実施の形態１〜実施の形態６と共通する部分については、同一の符号を付し、説明を省略する。
【００８５】
図１１は、この実施の形態における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図である。
追尾装置１００（推定装置、航跡推定装置）は、観測値取得部１１１と、観測値記憶部１２１と、候補算出部１４１と、候補記憶部１４２と、誤差上限初期化部１５１と、誤差上限値記憶部１５２と、ゲート行列算出部１５３と、相関部１６１と、尤度算出部１６２と、選択部１６３と、推定値記憶部１７１と、推定値更新部１７２と、収束判定部１７３と、誤差上限値更新部１７４と、終了判定部１７５と、推定値出力部１７６とを有する。
【００８６】
観測値取得部１１１は、入力装置９１２を用いて、センサ８１０が出力した信号を入力する。観測値取得部１１１は、処理装置９１１を用いて、入力した信号に基づいて、センサ８１０が目標を観測した観測値を取得する。観測値取得部１１１は、処理装置９１１を用いて、取得した観測値を表わす観測値データを生成する。
【００８７】
例えば、センサ８１０は、レーダである。センサ８１０は、電波を放射し、放射した電波が目標に当たって反射した反射波を受信する。センサ８１０は、受信した反射波の強さを表わす信号を出力する。センサ８１０は、電波を放射する方向を一次元的に走査する。これにより、センサ８１０が出力した信号から、目標が存在する方向を判別できる。また、電波を放射してから反射波を受信するまでの経過時間に基づいて、センサ８１０から目標までの距離を判別できる。
例えば、観測値取得部１１１は、処理装置９１１を用いて、センサ８１０が出力した信号が表わす反射波の強さが所定の閾値より大きくなったとき、センサ８１０が目標を観測したと判定する。観測値取得部１１１は、処理装置９１１を用いて、反射波の強さが閾値より大きくなったタイミングなどに基づいて、センサ８１０が観測した目標が存在する方向や、センサ８１０から目標までの距離などを算出する。観測値取得部１１１は、処理装置９１１を用いて、算出した目標の方向と、目標までの距離とに基づいて、センサ８１０の位置を原点とした直交座標系における目標の位置（例えばＸ座標及びＹ座標）を算出する。観測値取得部１１１は、処理装置９１１を用いて、算出した目標の位置を観測値とする観測値データを生成する。
【００８８】
あるいは、例えば、センサ８１０は、光学カメラである。センサ８１０は、目標が放射する可視光線や赤外線などの光を受光する。センサ８１０は、受光した光の強さを画素の輝度とする２次元画像を生成する。センサ８１０は、生成した画像を表わす信号を出力する。
例えば、観測値取得部１１１は、処理装置９１１を用いて、センサ８１０が出力した信号が表わす画像において、輝度が高い順に所定の数の画素を抽出し、抽出した画素に目標が観測されたと判定する。なお、輝度が高い画素が連続している場合、観測値取得部１１１は、１つの目標とみなし、そのなかで一番輝度が高い画素だけを抽出する構成であってもよい。観測値取得部１１１は、処理装置９１１を用いて、抽出した画素の画像内における位置（例えばＸ座標及びＹ座標）を算出する。観測値取得部１１１は、処理装置９１１を用いて、算出した画素の位置を観測値とする観測値データを生成する。
【００８９】
なお、センサ８１０は、目標の位置を二次元的に観測する構成に限らず、目標の位置を三次元的に観測する構成であってもよいし、ドップラー効果などに基づいて目標の速度を観測する構成であってもよい。
【００９０】
センサ８１０が出力する信号には、ノイズなどの影響による不要信号が含まれている。このため、観測値取得部１１１が抽出する観測値は、必ずしも目標を観測したものではない可能性がある。逆に、目標が存在しても、観測された信号が弱い場合は、ノイズなどに紛れて、観測値取得部１１１が観測値として抽出しない可能性がある。
【００９１】
観測値記憶部１２１は、記憶装置９１４を用いて、観測値取得部１１１が生成した観測値データを蓄積して記憶する。センサ８１０は、連続して、もしくは繰り返し、観測を行うので、時間の経過に伴い、観測値記憶部１２１が記憶する観測値データは増えていく。観測値記憶部１２１は、センサ８１０が目標を観測した時刻がわかるように、観測値データを記憶する。例えば、観測値記憶部１２１は、処理装置９１１を用いて、観測値データが表わす観測値の観測時刻やフレーム番号を表わすデータを生成して観測値データに付加し、記憶装置９１４を用いて、生成したデータ付加した観測値データを記憶する。あるいは、観測値記憶部１２１は、処理装置９１１を用いて、観測値データが表わす観測値の観測時刻やフレーム番号ごとに観測値データを分類し、記憶装置９１４を用いて、分類した観測値データを記憶する。なお、観測値記憶部１２１は、観測時刻から所定時間以上経過した観測値データを破棄し、観測時刻からの経過時間が所定時間内である観測値データのみを保持する構成であってもよい。
【００９２】
追尾装置１００は、所定の運動モデルに基づいて目標を追尾する。例えば、目標の状態量として、時刻ｔにおける三次元空間内における位置を表わす位置ベクトルｐ（ｔ）、速度を表わす速度ベクトルｐドット（ｔ）、加速度を表わす加速度ベクトルｐツードット（ｔ）の三つの三次元ベクトルを考える場合、目標の状態量ベクトルｘ（ｔ）は、この三つの三次元ベクトルの成分を成分とする九次元ベクトルで表わすことができる。
【数２４】
ただし、ｐｘ（ｔ）は、時刻ｔにおける目標の位置の三次元直交座標系におけるＸ座標である。ｐｙ（ｔ）は、時刻ｔにおける目標の位置の三次元直交座標系におけるＹ座標である。ｐｚ（ｔ）は、時刻ｔにおける目標の位置の三次元直交座標系におけるＺ座標である。
ｖｘ（ｔ）は、時刻ｔにおける目標の速度の三次元直交座標系におけるＸ方向成分である。ｖｙ（ｔ）は、時刻ｔにおける目標の速度の三次元直交座標系におけるＹ方向成分である。ｖｚ（ｔ）は、時刻ｔにおける目標の速度の三次元直交座標系におけるＺ方向成分である。
ａｘ（ｔ）は、時刻ｔにおける目標の加速度の三次元直交座標系におけるＸ方向成分である。ａｙ（ｔ）は、時刻ｔにおける目標の加速度の三次元直交座標系におけるＹ方向成分である。ａｚ（ｔ）は、時刻ｔにおける目標の加速度の三次元直交座標系におけるＺ方向成分である。
【００９３】
目標が等加速度運動をしているという運動モデルを用いる場合、目標の航跡は、ある基準時刻ｔＰにおける状態量ベクトルｘ（ｔＰ）で表わすことができる。このとき、基準時刻ｔＰから時間Δｔが経過した時刻ｔ＝ｔＰ＋Δｔにおける目標の状態量ベクトルｘ（ｔＰ＋Δｔ）は、次の式で求めることができる。
【数２５】
ただし、Ｆ（Δｔ）は、時間Δｔの経過に伴う状態量ベクトルの変化を表わす遷移行列である。遷移行列Ｆ（Δｔ）は、例えば、次の式で表わされる。
【数２６】
【００９４】
例えば、センサ８１０が光学カメラである場合、時刻ｔにおける観測値を表わす観測値ベクトルｚ（ｔ）と、その時刻ｔにおける目標の状態量ベクトルｘ（ｔ）との間には、例えば、次のような関係が成り立つ。
【数２７】
ただし、ｐＸ及びｐＹは、目標が観測されたと観測値取得部１１１が判定した画素の二次元座標である。画素の座標は、センサ８１０が撮影した画像の中心を原点とする。また、状態量ベクトルにおける三次元座標系は、センサ８１０の撮影方向をＺ軸とし、センサ８１０が撮影した画像上の横方向をＸ軸、縦方向をＹ軸とする。また、ｖは、観測誤差を表わす二次元ベクトル（観測誤差ベクトル）である。ｈは、観測関数を表わす。観測関数ｈは、例えば、次の式で表わされる。
【数２８】
【００９５】
候補算出部１４１は、処理装置９１１を用いて、目標の航跡の候補を算出する。候補算出部１４１は、処理装置９１１を用いて、複数の候補を算出する。例えば、候補算出部１４１は、処理装置９１１を用いて、目標の航跡を表わす状態量ベクトルの九つの成分それぞれについて、１つ以上の候補を算出し、それを組み合わせることにより、複数の航跡の候補を算出する。
候補記憶部１４２は、記憶装置９１４を用いて、候補算出部１４１が算出した目標の航跡の候補を表わす航跡候補データを記憶する。
【００９６】
例えば、候補算出部１４１は、次のようにして、複数の航跡の候補を算出する。
【００９７】
候補算出部１４１は、観測値記憶部１２１が記憶した観測値データに基づいて、処理装置９１１を用いて、基準時刻ｔＰを決定する。例えば、候補算出部１４１は、観測値記憶部１２１が記憶した観測値データのなかから、観測時刻が最も新しい観測値データを抽出し、抽出した観測値データの観測時刻を基準時刻ｔＰとする。
【００９８】
候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰにおける目標の加速度ベクトルの推定値の候補として、一つの加速度ベクトルを設定する。目標の加速度ベクトルの推定値の候補を一つの加速度ベクトルに固定することにより、実際の目標の加速度ベクトルとの差がモデル化誤差となる。モデル化誤差の期待値は、設定した加速度ベクトルが、想定される実際の目標の加速度ベクトルの確率分布に基づく平均（期待値）に一致する場合に最小になる。想定される実際の目標の加速度ベクトルの確率分布について事前の情報がない場合、候補算出部１４１は、例えば、目標の加速度ベクトルの推定値の候補として、ゼロベクトルを設定する。あるいは、前回推定した航跡がある場合など、想定される実際の目標の加速度ベクトルの確率分布について事前の情報がある場合、候補算出部１４１は、目標の加速度ベクトルの推定値の候補として、例えば、事前の情報に基づいて加速度ベクトルの平均値（期待値）を算出する。例えば、候補算出部１４１は、前回推定した航跡を表わす状態量ベクトルから加速度ベクトルを取り出して、今回の目標の加速度ベクトルの推定値の候補とする。
【００９９】
候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰにおける目標の速度ベクトルの推定値の候補として、複数の速度ベクトルを設定する。例えば、候補算出部１４１は、想定される実際の目標の速度ベクトルの分布範囲のなかから、所定の数の速度ベクトルを、なるべく均等に分布するよう選択して、目標の速度ベクトルの推定値の候補とする。想定される実際の目標の速度ベクトルの分布範囲について事前の情報がない場合、候補算出部１４１は、目標の速度ベクトルの推定値の候補として、例えば、ゼロベクトルを中心とし、絶対値が所定の想定最大速度を超えない球状の範囲内から、所定の数の速度ベクトルを選択する。あるいは、前回推定した航跡がある場合など、想定される実際の目標の速度ベクトルの分布範囲について事前の情報がある場合、候補算出部１４１は、目標の速度ベクトルの推定値の候補として、例えば、事前の情報に基づいて目標の速度ベクトルの分布範囲を求め、その範囲内から、所定の数の速度ベクトルを選択する。
【０１００】
候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補として、複数の位置ベクトルを設定する。例えば、候補算出部１４１は、処理装置９１１を用いて、観測値記憶部１２１が記憶した観測値データのなかから、観測時刻と基準時刻ｔＰとの差が所定の閾値以内である観測値データを抽出する。閾値は、目標が存在しても観測されない欠測の可能性を考慮して、その時間の間に少なくとも１回は、目標が観測される時間に設定しておく。例えば、欠測の可能性が１０％ある場合、センサ８１０が少なくとも３回は観測を行う時間を閾値とすれば、その間に目標が１回も観測されない可能性は、０．１％以下である。
候補算出部１４１は、処理装置９１１を用いて、抽出した観測値データに基づいて、観測時刻ｔにおける目標の位置ベクトルの推定値の候補を算出する。例えば、候補算出部１４１は、処理装置９１１を用いて、次の式の右辺を計算することにより、観測時刻ｔにおける目標の位置ベクトルの推定値の候補を算出する。
【数２９】
ただし、ｐｅ（ｔ）は、候補算出部１４１が抽出した観測値データの観測時刻ｔにおける目標の位置ベクトルの推定値の候補である。ｐＸ及びｐＹは、候補算出部１４１が抽出した観測値データが表わす座標である。ｐｚｅは、目標の位置のＺ座標（センサ８１０から目標までの距離）の推定値の候補である。例えば、センサ８１０の観測可能範囲に基づいて、想定される目標の位置のＺ座標の範囲をあらかじめ定めておき、候補算出部１４１は、想定される範囲内から、複数のＺ座標の推定値の候補を選択する。候補算出部１４１は、選択したＺ座標の推定値の候補に基づいて、観測時刻ｔにおける目標の位置のＸ座標及びＹ座標の推定値の候補を算出する。候補算出部１４１は、目標の位置ベクトルの推定値の候補を、抽出した観測値データの数と選択したＺ座標の推定値の候補の数との積に等しい数だけ算出する。
【０１０１】
候補算出部１４１は、処理装置９１１を用いて、算出した目標の速度ベクトルの推定値の候補に基づいて、目標の位置ベクトルの推定値の候補の時刻合わせを行い、時刻ｔにおける目標の位置ベクトルの推定値の候補を、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補へ変換する。例えば、候補算出部１４１は、処理装置９１１を用いて、次の式の右辺を計算することにより、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補を算出する。
【数３０】
ただし、ｐｅ（ｔＰ）は、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補である。ｐｅ（ｔ）は、候補算出部１４１が抽出した観測値データの観測時刻ｔにおける目標の位置ベクトルの推定値の候補である。ｐドットｅ（ｔＰ）は、候補算出部１４１が算出した基準時刻ｔＰにおける目標の速度ベクトルの推定値の候補である。Δｔは、観測時刻ｔから基準時刻ｔＰを差し引いた差である。
【０１０２】
候補算出部１４１は、処理装置９１１を用いて、算出した基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補と、算出した基準時刻ｔＰにおける目標の速度ベクトルの候補と、算出した基準時刻ｔＰにおける目標の加速度ベクトルの候補とを組み合わせて、目標の航跡を表わす状態量ベクトルの推定値の候補を生成する。
【数３１】
ただし、ｘｅ（ｔＰ）は、基準時刻ｔＰにおける目標の状態量ベクトルの推定値の候補である。ｐツードットｅ（ｔＰ）は、候補算出部１４１が算出した基準時刻ｔＰにおける目標の加速度ベクトルの推定値の候補である。
候補算出部１４１は、状態量ベクトルの推定値の候補を、算出した目標の位置ベクトルの推定値の候補の数と、算出した目標の速度ベクトルの推定値の候補の数との積に等しい数だけ算出する。
【０１０３】
候補算出部１４１は、処理装置９１１を用いて、算出した目標の状態量ベクトルの推定値の候補のなかに重複するものがあれば統合する。例えば、候補算出部１４１は、処理装置９１１を用いて、算出した状態量ベクトルの推定値の候補の間のユークリッド距離を算出し、算出したユークリッド距離が所定の閾値以内である場合に、重複と判断する。候補算出部１４１は、処理装置９１１を用いて、重複と判定した２つの位置ベクトルの推定値の候補を平均して、１つに統合する。３つ以上の候補が重複している可能性がある場合、候補算出部１４１は、例えば、距離が最も近い２つの候補を統合し、統合後の新しい候補と、他の候補との間の距離を算出して、重複しているか否かを判定する。
【０１０４】
なお、目標が観測された観測時刻ｔと基準時刻ｔＰとの間の経過時間Δｔが短い場合、経過時間Δｔの間における目標の移動を考慮せず、観測時刻ｔにおける目標の位置ベクトルの推定値の候補を、そのまま、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補とする構成であってもよい。
【０１０５】
誤差上限値記憶部１５２は、記憶装置９１４を用いて、候補算出部１４１が算出した目標の状態量ベクトルの推定値の候補において候補を複数算出しなかった状態量について、その状態量の推定値の候補を固定したことによるモデル化誤差の上限値を記憶する。例えば、候補算出部１４１が加速度ベクトルの推定値の候補を固定した場合、誤差上限値記憶部１５２は、実際の加速度ベクトルと、候補算出部１４１が固定した加速度ベクトルの推定値の候補との間の差をモデル化誤差とし、その上限値を記憶する。誤差上限値記憶部１５２が記憶するモデル化誤差の上限値は、誤差上限初期化部１５１や誤差上限値更新部１７４が算出する。例えば、誤差上限値記憶部１５２は、次の式で表わされる最大加速度誤差ベクトルを、モデル化誤差上限値ベクトルとして記憶する。
【数３２】
ただし、Δｐツードットｍａｘは、最大加速度誤差ベクトルである。Δａｘｍａｘは、最大加速度誤差ベクトルのＸ方向成分である。Δａｙｍａｘは、最大加速度誤差ベクトルのＹ方向成分である。Δａｚｍａｘは、最大加速度誤差ベクトルのＺ方向成分である。
【０１０６】
誤差上限初期化部１５１は、処理装置９１１を用いて、誤差上限値記憶部１５２が記憶するモデル化誤差の上限値を初期化する。例えば、想定される目標の加速度ベクトルの確率分布について事前の情報がなく、候補算出部１４１が目標の加速度ベクトルの推定値の候補としてゼロベクトルを設定した場合、誤差上限初期化部１５１は、目標の想定最大加速度を、モデル化誤差の上限値とする。あるいは、例えば、想定される目標の加速度ベクトルの確率分布について事前の情報があり、候補算出部１４１がその期待値を目標の加速度ベクトルの推定値の候補として設定した場合、誤差上限初期化部１５１は、想定される目標の加速度ベクトルの確率分布に基づいて、モデル化誤差の上限値を算出する。誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限初期化部１５１が算出したモデル化誤差の上限値を、モデル化誤差の上限値の初期値として記憶する。
【０１０７】
ゲート行列算出部１５３は、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値に基づいて、ゲート行列を算出する。ゲート行列は、観測時刻ごとに異なるので、ゲート行列算出部１５３は、相関処理の対象となる観測値データの観測時刻ごとに、ゲート行列を算出する。例えば、ゲート行列算出部１５３は、処理装置９１１を用いて、観測値記憶部１２１が記憶した観測値データのなかから、相関処理の対象となる観測値データを抽出する。ゲート行列算出部１５３は、抽出した観測値データの観測時刻ｔに基づいて、ゲート行列を算出する。
例えば、ゲート行列算出部１５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、モデル化誤差共分散行列を算出する。
【数３３】
ただし、Ｑ（ｔＰ＋Δｔ）は、基準時刻ｔＰから時間Δｔが経過した時刻におけるモデル化誤差共分散行列である。ｄｉａｇは、ｎ次元ベクトルのｉ番目の成分をｉ行ｉ列成分とするｎ次の対角行列を表わす。
ゲート行列算出部１５３は、算出したモデル化誤差共分散行列に基づいて、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔにおけるゲート行列を算出する。
【０１０８】
Ｓ（ｔ）＝Ｒ（ｔ）＋Ｑ（ｔ）
ただし、Ｓ（ｔ）は、ゲート行列算出部１５３が抽出した観測値データの観測時刻ｔにおけるゲート行列である。Ｒ（ｔ）は、その観測時刻ｔにおける観測値データの観測誤差共分散行列である。Ｑ（ｔ）は、その観測時刻ｔについてゲート行列算出部１５３が算出したモデル化誤差共分散行列である。Ｓ（ｔ）、Ｒ（ｔ）、Ｑ（ｔ）は、いずれも、観測値ベクトルｚ（ｔ）と同じ次数の正方行列である。
【０１０９】
相関部１６１は、候補記憶部１４２が記憶した目標の航跡の候補それぞれについて、処理装置９１１を用いて、ゲート行列算出部１５３が算出したゲート行列に基づいて、観測値記憶部１２１が記憶した観測値データのなかから、その航跡の目標を観測した観測値であると考えられる観測値を表わす観測値データを抽出する。
例えば、相関部１６１は、次のようにして、観測値データを抽出する。
【０１１０】
相関部１６１は、候補記憶部１４２が記憶した目標の航跡の候補それぞれについて、処理装置９１１を用いて、観測値記憶部１２１が記憶した観測値データのうち相関処理の対象となる観測値データの観測時刻ｔに基づいて、時刻ｔにおける目標の状態量ベクトルの推定値を算出する。相関部１６１は、例えば、次の式の右辺を計算することにより、時刻ｔにおける目標の状態量ベクトルの推定値を算出する。
【０１１１】
ｘｅ（ｔ）＝Ｆ（Δｔ）ｘｅ（ｔＰ）
ただし、ｘｅ（ｔ）は、基準時刻ｔＰから時間Δｔが経過した時刻ｔ＝ｔＰ＋Δｔにおける目標の状態量ベクトルの推定値である。ｘｅ（ｔＰ）は、候補記憶部１４２が記憶した目標の航跡の候補である基準時刻ｔＰにおける目標の状態量ベクトルの推定値の候補である。
【０１１２】
相関部１６１は、算出した時刻ｔにおける目標の状態量ベクトルの推定値と、観測値データが表わす時刻ｔにおける観測値ベクトルとに基づいて、処理装置９１１を用いて、その観測値データの観測誤差の推定値を算出する。相関部１６１は、例えば、次の式の右辺を計算することにより、その観測値データの観測誤差の推定値を算出する。
【０１１３】
Δｚ＝ｚ−ｈ（ｘｅ（ｔ））
ただし、Δｚは、その観測値データの観測誤差の推定値である。ｚは、観測値データが表わす観測値ベクトルである。ｔは、その観測値データの観測時刻である。ｈは、観測関数である。
【０１１４】
相関部１６１は、算出した観測誤差の推定値に基づいて、処理装置９１１を用いて、その観測値データが、その航跡の目標を観測した観測値を表わす観測値データであるか否かを判定する。例えば、相関部１６１は、ゲート行列算出部１５３が算出したゲート行列に基づいて、次の式が表わす条件が満たされる場合に、その観測値データが、その航跡の目標を観測した観測値を表わす観測値データであると判断する。
【０１１５】
ΔｚＴＳ（ｔ）−１Δｚ≦ｄ
ただし、上付きのＴは、転置行列を表わす。上付きの−１は、逆行列を表わす。ｄは、所定の閾値である。
【０１１６】
尤度算出部１６２は、候補記憶部１４２が記憶した目標の軌跡の候補それぞれについて、処理装置９１１を用いて、ゲート行列算出部１５３が算出したゲート行列と、相関部１６１が抽出した観測値データとに基づいて、その目標の軌跡の候補の尤度を算出する。例えば、尤度算出部１６２は、次の式の右辺を計算することにより、その軌跡の候補の尤度を算出する。
【数３４】
ただし、φは、その軌跡の候補の尤度である。Ｔは、相関処理の対象となる観測値データの観測時刻を要素とする集合である。ｌｏｇｅは、ネイピア数を底とする対数（自然対数）関数を表わす。ＰＤは、探知確率パラメータである。探知確率パラメータＰＤは、０より大きく１より小さい実数であり、センサ８１０の観測範囲内に目標が存在するとき、その目標を観測した観測値を観測値取得部１１１が抽出する確率を表わす。βは、不要信号密度パラメータである。不要信号密度パラメータβは、０より大きい実数であり、ノイズなどの影響による不要信号が観測値空間の単位体積あたりに存在する平均個数を表わす。探知確率パラメータＰＤや不要信号密度パラメータβは、あらかじめ設定された値を尤度算出部１６２が記憶装置９１４を用いて記憶しておく。なお、尤度算出部１６２は、追尾結果などに基づいて、探知確率パラメータＰＤや不要信号密度パラメータβを更新する構成であってもよい。
ｄｅｔは、行列式を表わす。πは、円周率である。Ｓ（ｔ）は、ゲート行列算出部１５３が算出した時刻ｔにおけるゲート行列である。Ｇ（ｔ）は、その航跡の候補について相関部１６１が抽出した観測値データのうち、観測時刻が時刻ｔである観測値データが表わす観測値ベクトルを要素とする集合である。ｅｘｐは、ネイピア数を底とする指数関数を表わす。Δｚは、観測値ベクトルｚについて相関部１６１が算出した観測誤差の推定値である。上付きのＴは、転置行列を表わす。上付きの−１は、逆行列を表わす。
【０１１７】
選択部１６３は、尤度算出部１６２が算出した尤度に基づいて、処理装置９１１を用いて、候補記憶部１４２が記憶した航跡の候補のなかから、正しい航跡に近い航跡である可能性の高い候補を抽出する。選択部１６３は、航跡の候補を１つだけ抽出する構成であってもよいし、複数の候補を抽出する構成であってもよい。
例えば、選択部１６３は、処理装置９１１を用いて、候補記憶部１４２が記憶した航跡の候補のなかから、尤度算出部１６２が算出した尤度が大きい順に、所定の数の候補を抽出する。あるいは、例えば、選択部１６３は、処理装置９１１を用いて、尤度算出部１６２が算出した尤度のなかから、一番大きな尤度を抽出し、候補記憶部１４２が記憶した航跡の候補それぞれについて、尤度算出部１６２が算出した尤度を、抽出した最大尤度で割った商を算出する。選択部１６３は、処理装置９１１を用いて、算出した商が所定の閾値より大きい候補を抽出する。
【０１１８】
推定値記憶部１７１は、記憶装置９１４を用いて、選択部１６３が抽出した航跡の候補を表わす状態量ベクトルの推定値を記憶する。
【０１１９】
推定値更新部１７２は、処理装置９１１を用いて、推定値記憶部１７１が記憶した状態量ベクトルの推定値を出発点として、もっと尤度が大きい航跡を表わす状態量ベクトルの推定値を算出する。推定値更新部１７２は、算出した状態量ベクトルの推定値で、推定値記憶部１７１が記憶した状態量ベクトルの推定値を更新する。推定値記憶部１７１は、記憶装置９１４を用いて、更新された状態量ベクトルの推定値を記憶する。
【０１２０】
例えば、推定値更新部１７２は、推定値記憶部１７１が記憶した状態量ベクトルの推定値をわずかに変化させる。推定値更新部１７２が変化させた状態量ベクトルの推定値に基づいて、相関部１６１が相関処理をし、尤度算出部１６２が尤度を算出する。推定値更新部１７２は、尤度算出部１６２が算出した尤度に基づいて、推定値記憶部１７１が記憶した状態量ベクトルの推定値の近傍における尤度の変化傾向を把握する。推定値更新部１７２は、把握した変化傾向に基づいて、状態量ベクトルの推定値をどの方向にどのくらい変化させたら、もっと尤度が大きい航跡を表わす状態量ベクトルの推定値となるかを算出する。
具体的には、例えば次のようにして、もっと尤度が大きい航跡を表わす状態量ベクトルの推定値を算出する。
【０１２１】
推定値更新部１７２は、処理装置９１１を用いて、推定値記憶部１７１が記憶した状態量ベクトルの推定値それぞれについて、状態量ベクトルの各成分を微かに変化させた状態量ベクトルを算出する。推定値更新部１７２は、例えば、状態量ベクトルのある成分に、所定の微小増分を加えた状態量ベクトルと、その成分から微小増分を差し引いた状態量ベクトルとを算出する。例えば、状態量ベクトルが九次元ベクトルである場合、推定値更新部１７２は、各成分について２つの状態量ベクトルを算出するので、全部で１８個の状態量ベクトルを算出する。
相関部１６１（詳細相関部）は、処理装置９１１を用いて、推定値更新部１７２が算出した１８個の状態量ベクトルと、その元となった１つの状態量ベクトルとを合わせた合計１９個の状態量ベクトルそれぞれについて、その状態量ベクトルが表わす航跡を、候補記憶部１４２が記憶した航跡の候補の代わりとして、観測値記憶部１２１が記憶した観測値データのなかから、観測値データを抽出する。尤度算出部１６２（詳細尤度算出部）は、処理装置９１１を用いて、相関部１６１と同様、１９個の状態量ベクトルそれぞれについて、その状態量ベクトルが表わす航跡を、候補記憶部１４２が記憶した航跡の候補の代わりとして、その航跡の尤度を算出する。
推定値更新部１７２は、処理装置９１１を用いて、尤度算出部１６２が算出した尤度に基づいて、状態量ベクトルの各成分について、尤度が最大になると予想される値を算出する。
例えば、推定値更新部１７２は、状態量ベクトルの一つの成分に注目し、注目した成分以外の成分の値は変えずに、その成分の値だけを変えて算出した尤度を比較する。推定値更新部１７２は、その成分のもとの値ｕを使った状態量ベクトルについて算出した尤度φ（ｕ）と、微小増分Δｕを加えた値を使った状態量ベクトルについて算出した尤度φ（ｕ＋Δｕ）と、微小増分Δｕを差し引いた値を使った状態量ベクトルについて算出した尤度φ（ｕ−Δｕ）との間の大小関係を判定する。
φ（ｕ−Δｕ）≧φ（ｕ）≦φ（ｕ＋Δｕ）である場合、推定値更新部１７２は、もとの値ｕを、その成分の更新した値とする。
φ（ｕ−Δｕ）＜φ（ｕ）＞φ（ｕ＋Δｕ）である場合も、推定値更新部１７２は、もとの値ｕを、その成分の更新した値とする。
φ（ｕ−Δｕ）＜φ（ｕ）≦φ（ｕ＋Δｕ）である場合、推定値更新部１７２は、尤度φ（ｕ）から尤度φ（ｕ−Δｕ）を差し引いた差Δφ−＝φ（ｕ）−φ（ｕ−Δｕ）と、尤度φ（ｕ＋Δｕ）から尤度φ（ｕ）を差し引いた差Δφ＋＝φ（ｕ＋Δｕ）−φ（ｕ）とを算出する。推定値更新部１７２は、算出した２つの差の大小関係を比較する。
Δφ＋≧Δφ−である場合、推定値更新部１７２は、０超所定の値未満の乱数ｒを生成し、生成した乱数ｒをもとの値ｕに加えた和ｕ＋ｒを、その成分の更新した値とする。
Δφ＋＜Δφ−である場合、推定値更新部１７２は、差Δφ＋と差Δφ−との平均値Δφ＝（Δφ＋＋Δφ−）／２と、差Δφ−から差Δφ＋を差し引いた差Δ２φ＝Δφ−−Δφ＋とを算出する。推定値更新部１７２は、算出した平均値Δφを、算出した差Δ２φで割った商を微小増分Δｕに乗じた積をもとの値ｕに加えた和ｕ＋Δφ／Δ２φ・Δｕを、その成分の更新した値とする。
φ（ｕ−Δｕ）≧φ（ｕ）＞φ（ｕ＋Δｕ）である場合、推定値更新部１７２は、尤度φ（ｕ）から尤度φ（ｕ＋Δｕ）を差し引いた差Δφ＋＝φ（ｕ）−φ（ｕ＋Δｕ）と、尤度φ（ｕ−Δｕ）から尤度φ（ｕ）を差し引いた差Δφ−＝φ（ｕ−Δｕ）−φ（ｕ）とを算出する。推定値更新部１７２は、算出した２つの差の大小関係を比較する。
Δφ＋≦Δφ−である場合、推定値更新部１７２は、０超所定の値未満の乱数ｒを生成し、生成した乱数ｒをもとの値ｕから差し引いた差ｕ−ｒを、その成分の更新した値とする。
Δφ＋＞Δφ−である場合、推定値更新部１７２は、差Δφ＋と差Δφ−との平均値Δφ＝（Δφ＋＋Δφ−）／２と、差Δφ＋から差Δφ−を差し引いた差Δ２φ＝Δφ＋−Δφ−とを算出する。推定値更新部１７２は、算出した平均値Δφを、算出した差Δ２φで割った商を微小増分Δｕに乗じた積をもとの値ｕから差し引いた差ｕ−Δφ／Δ２φ・Δｕを、その成分の更新した値とする。
推定値更新部１７２は、例えばこのようにして状態量ベクトルのすべての成分について更新した値を算出することにより、更新した状態量ベクトルの推定値を算出する。
推定値記憶部１７１は、記憶装置９１４を用いて、推定値更新部１７２が更新した状態量ベクトルの推定値を記憶する。
【０１２２】
収束判定部１７３は、処理装置９１１を用いて、状態量ベクトルの推定値が収束したか否かを判定する。例えば、収束判定部１７３は、推定値更新部１７２が更新した更新後の状態量ベクトルの推定値と、推定値更新部１７２が更新する前に推定値記憶部１７１が記憶していた更新前の状態量ベクトルの推定値との差ベクトルを算出する。収束判定部１７３は、算出した差ベクトルの絶対値を所定の閾値と比較する。差ベクトルの絶対値が閾値より小さい場合、収束判定部１７３は、状態量ベクトルの推定値が収束したと判定する。
【０１２３】
誤差上限値更新部１７４は、状態量ベクトルの推定値が収束したと収束判定部１７３が判定した場合、処理装置９１１を用いて、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値を更新する。例えば、誤差上限値更新部１７４は、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値に、０超１未満の所定の係数を乗じた積を算出して、更新したモデル化誤差の上限値とする。
誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限値更新部１７４が更新したモデル化誤差の上限値を記憶する。
【０１２４】
終了判定部１７５は、状態量ベクトルの推定値が収束したと収束判定部１７３が判定した場合、処理装置９１１を用いて、状態量ベクトルの推定を終了するか否かを判定する。例えば、終了判定部１７５は、状態量ベクトルの推定値が収束したと収束判定部１７３が判定したとき推定値記憶部１７１が記憶している状態量ベクトルの推定値を記憶する。状態量ベクトルの推定値が収束したと収束判定部１７３が判定したのが一回目である場合、終了判定部１７５は、状態量ベクトルの推定を終了しないと判定する。状態量ベクトルの推定値が収束したと収束判定部１７３が判定したのが二回目もしくはそれ以降である場合、終了判定部１７５は、記憶している前回の状態量ベクトルの推定値と、そのとき推定値記憶部１７１が記憶している状態量ベクトルの推定値との差ベクトルを算出する。終了判定部１７５は、算出した差ベクトルの絶対値を所定の閾値と比較する。差ベクトルの絶対値が閾値より小さい場合、終了判定部１７５は、状態量ベクトルの推定を終了すると判定する。
【０１２５】
推定値出力部１７６は、状態量ベクトルの推定を終了すると終了判定部１７５が判定した場合、出力装置９１３を用いて、推定値記憶部１７１が記憶した状態量ベクトルの推定値を出力する。なお、推定値記憶部１７１が記憶した状態量ベクトルの推定値のなかに、ほぼ同一の推定値が複数ある場合、推定値出力部１７６は、そのなかで最も尤度が大きい推定値だけを出力する構成であってもよい。
【０１２６】
図１２は、この実施の形態における追尾処理Ｓ６００の流れの一例を示すフローチャート図である。
追尾処理Ｓ６００（推定方法、航跡推定方法）は、例えば、観測値取得工程Ｓ６６１と、候補算出工程Ｓ６６２と、誤差上限初期化工程Ｓ６６３と、２つのゲート行列算出工程Ｓ６６４，Ｓ６８３と、候補選択工程Ｓ６６５と、２つの相関工程Ｓ６６６，Ｓ６７３と、２つの尤度算出工程Ｓ６６７，Ｓ６７４と、選択工程Ｓ６６８と、推定値選択工程Ｓ６７１と、参照生成工程Ｓ６７２と、推定値更新工程Ｓ６７５と、収束判定工程Ｓ６７６と、個別終了判定工程Ｓ６７７と、全体終了判定工程Ｓ６８１と、誤差上限値更新工程Ｓ６８２と、推定値出力工程Ｓ６８４とを有する。
追尾装置１００は、観測値取得工程Ｓ６６１から追尾処理Ｓ６００を開始する。
【０１２７】
観測値取得工程Ｓ６６１において、観測値取得部１１１は、入力装置９１２を用いて、センサ８１０が出力した信号を入力する。観測値取得部１１１は、処理装置９１１を用いて、入力した信号に基づいて、観測値を取得する。観測値記憶部１２１は、記憶装置９１４を用いて、観測値取得部１１１が取得した観測値を記憶する。
【０１２８】
候補算出工程Ｓ６６２において、候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰを決定する。候補算出部１４１は、処理装置９１１を用いて、決定した基準時刻ｔＰにおける目標の状態量の推定値の候補を複数生成する。候補記憶部１４２は、記憶装置９１４を用いて、候補算出部１４１が生成した複数の候補を記憶する。
【０１２９】
誤差上限初期化工程Ｓ６６３において、誤差上限初期化部１５１は、処理装置９１１を用いて、モデル化誤差の上限値の初期値を設定する。誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限初期化部１５１が設定した初期値を、モデル化誤差の上限値として記憶する。
【０１３０】
ゲート行列算出工程Ｓ６６４において、ゲート行列算出部１５３は、処理装置９１１を用いて、誤差上限初期化工程Ｓ６６３で誤差上限値記憶部１５２が記憶したモデル化誤差の上限値と、候補算出工程Ｓ６６２で候補算出部１４１が決定した基準時刻ｔＰとに基づいて、複数の観測時刻ｔそれぞれにおけるゲート行列Ｓ（ｔ）を算出する。
【０１３１】
候補選択工程Ｓ６６５において、選択部１６３は、処理装置９１１を用いて、候補記憶部１４２が記憶した複数の候補のなかから、まだ尤度を算出していない候補を一つ選択する。
候補記憶部１４２が記憶したすべての候補について尤度を算出済であり、選択すべき候補がない場合、選択部１６３は、処理装置９１１を用いて、選択工程Ｓ６６８へ処理を進める。
候補記憶部１４２が記憶した候補のなかに、まだ尤度を算出していない候補がある場合、選択部１６３は、処理装置９１１を用いて、まだ尤度を算出していない候補のなかから、候補を一つ選択する。選択部１６３は、処理装置９１１を用いて、相関工程Ｓ６６６へ処理を進める。
【０１３２】
相関工程Ｓ６６６において、相関部１６１は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、候補選択工程Ｓ６６５で選択部１６３が選択した候補とに基づいて、観測値記憶部１２１が記憶した観測値のなかから、その候補が表わす航跡の目標を観測したと考えられる観測値を抽出する。
【０１３３】
尤度算出工程Ｓ６６７において、尤度算出部１６２は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、候補選択工程Ｓ６６５で選択部１６３が選択した候補と、相関工程Ｓ６６６で相関部１６１が抽出した観測値とに基づいて、その候補の尤度を算出する。選択部１６３は、記憶装置９１４を用いて、尤度算出部１６２が算出した尤度を記憶する。選択部１６３は、処理装置９１１を用いて、候補選択工程Ｓ６６５に処理を戻し、次の候補を選択する。
【０１３４】
選択工程Ｓ６６８において、選択部１６３は、処理装置９１１を用いて、尤度算出工程Ｓ６６７で記憶した尤度に基づいて、候補記憶部１４２が記憶した候補のなかから、正しい航跡に近い航跡を表わす候補を抽出する。推定値記憶部１７１は、記憶装置９１４を用いて、選択部１６３が抽出した候補（基準時刻ｔＰにおける状態量の推定値）を記憶する。
【０１３５】
推定値選択工程Ｓ６７１において、推定値更新部１７２は、処理装置９１１を用いて、選択工程Ｓ６６８で推定値記憶部１７１が記憶した推定値のうち、まだ推定が終了していない推定値のなかから、まだ更新をしていない推定値を一つ選択する。
推定値記憶部１７１が記憶した推定値すべてについての更新が終わり、選択すべき推定値がない場合、推定値更新部１７２は、処理装置９１１を用いて、全体終了判定工程Ｓ６８１へ処理を進める。
推定値記憶部１７１が記憶した推定値のなかに、まだ更新をしていない推定値がある場合、推定値更新部１７２は、処理装置９１１を用いて、まだ更新をしていない推定値のなかから、推定値を一つ選択する。推定値更新部１７２は、処理装置９１１を用いて、参照生成工程Ｓ６７２へ処理を進める。
【０１３６】
参照生成工程Ｓ６７２において、推定値更新部１７２は、処理装置９１１を用いて、所定の複数の方向のなかから、推定値を変化させる方向を選択する。
すべての方向についての処理が終わり、選択すべき方向がない場合、推定値更新部１７２は、処理装置９１１を用いて、推定値更新工程Ｓ６７５へ処理を進める。
所定の複数の方向のなかに、まだ処理していない方向がある場合、推定値更新部１７２は、処理装置９１１を用いて、まだ処理していない方向のなかから、方向を一つ選択する。推定値更新部１７２は、処理装置９１１を用いて、選択した方向へ向けて、推定値選択工程Ｓ６７１で選択した推定値をわずかに変化させる。推定値更新部１７２は、処理装置９１１を用いて、相関工程Ｓ６７３へ処理を進める。
【０１３７】
相関工程Ｓ６７３において、相関部１６１（詳細相関部）は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４またはＳ６８３でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、参照生成工程Ｓ６７２で推定値更新部１７２が変化させた推定値とに基づいて、観測値記憶部１２１が記憶した観測値のなかから、その推定値が表わす航跡の目標を観測したと考えられる観測値を抽出する。
【０１３８】
尤度算出工程Ｓ６７４において、尤度算出部１６２（詳細尤度算出部）は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４またはＳ６８３でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、推定値更新部１７２で推定値更新部１７２が変化させた推定値と、相関工程Ｓ６７３で相関部１６１が抽出した観測値とに基づいて、その推定値の尤度を算出する。推定値更新部１７２は、記憶装置９１４を用いて、尤度算出部１６２が算出した尤度を記憶する。推定値更新部１７２は、処理装置９１１を用いて、参照生成工程Ｓ６７２に処理を戻し、次の方向を選択する。
【０１３９】
推定値更新工程Ｓ６７５において、推定値更新部１７２は、処理装置９１１を用いて、尤度算出工程Ｓ６７４で記憶した尤度に基づいて、更新前よりも尤度が大きくなる推定値を算出する。推定値記憶部１７１は、処理装置９１１を用いて、推定値選択工程Ｓ６７１で推定値更新部１７２が選択した推定値を、推定値更新部１７２が算出した推定値で更新し、記憶装置９１４を用いて、更新した推定値を記憶する。
【０１４０】
収束判定工程Ｓ６７６において、収束判定部１７３は、処理装置９１１を用いて、推定値選択工程Ｓ６７１で推定値更新部１７２が選択した推定値が収束したか否かを判定する。
収束したと判定した場合、収束判定部１７３は、処理装置９１１を用いて、個別終了判定工程Ｓ６７７へ処理を進める。
まだ収束していないと判定した場合、推定値更新部１７２は、処理装置９１１を用いて、参照生成工程Ｓ６７２に処理を戻し、更に尤度が大きくなる推定値を算出する。
【０１４１】
個別終了判定工程Ｓ６７７において、終了判定部１７５は、処理装置９１１を用いて、推定値選択工程Ｓ６７１で推定値更新部１７２が選択した推定値について、状態量の推定を終了するか否かを判定する。終了判定部１７５は、記憶装置９１４を用いて、判定した判定結果を記憶する。
推定値更新部１７２は、処理装置９１１を用いて、推定値選択工程Ｓ６７１に処理を戻し、次の推定値を選択する。
【０１４２】
全体終了判定工程Ｓ６８１において、終了判定部１７５は、処理装置９１１を用いて、個別終了判定工程Ｓ６７７で記憶した判定結果に基づいて、推定値記憶部１７１が記憶したすべての推定値について、状態量の推定を終了すると判定したか否かを判定する。
すべての推定値について状態量の推定を終了すると判定し、状態量の推定を終了しないと判定した推定値がない場合、終了判定部１７５は、処理装置９１１を用いて、推定値出力工程Ｓ６８４へ処理を進める。
状態量の推定を終了しないと判定した推定値がまだある場合、終了判定部１７５は、処理装置９１１を用いて、誤差上限値更新工程Ｓ６８２へ処理を進める。
【０１４３】
誤差上限値更新工程Ｓ６８２において、誤差上限値更新部１７４は、処理装置９１１を用いて、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値を更新する。誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限値更新部１７４が更新したモデル化誤差の上限値を記憶する。
【０１４４】
ゲート行列算出工程Ｓ６８３において、ゲート行列算出部１５３は、処理装置９１１を用いて、誤差上限値更新工程Ｓ６８２で誤差上限値記憶部１５２が記憶した更新後のモデル化誤差の上限値に基づいて、観測時刻ｔそれぞれにおけるゲート行列Ｓ（ｔ）を算出する。
推定値更新部１７２は、推定値選択工程Ｓ６７１に処理を戻す。推定値選択工程Ｓ６７１において、推定値更新部１７２は、推定値記憶部１７１が記憶した推定値のうち、個別終了判定工程Ｓ６７７で状態量の推定を終了しないと終了判定部１７５が判定した推定値について、また最初から推定値を一つずつ選択する。
【０１４５】
推定値出力工程Ｓ６８４において、推定値出力部１７６は、出力装置９１３を用いて、推定値記憶部１７１が記憶した推定値を出力する。
【０１４６】
以上、各実施の形態で説明した具体的な構成は一例であり、これに限定されるものではない。例えば、重要でない部分の構成を他の構成で置き換えた構成としてもよし、異なる実施の形態で説明した構成を組み合わせた構成としてもよい。
【０１４７】
推定装置（追尾装置１００）は、データを処理する処理装置（９１１）と、候補算出部（１４１；航跡候補生成部１４０）と、相関部（１６１；概探索部１６０）と、尤度算出部（１６２；概探索部１６０）と、選択部（１６３；概探索部１６０）と、推定値更新部（１７２；詳細探索部１７０）と、推定値出力部（１７６；詳細探索部１７０）とを有する。
上記候補算出部は、目標の複数の状態量（例えば位置・速度・加速度）のうち所定の第一の状態量（例えば加速度）を除く他の状態量（例えば位置・速度）について、上記処理装置を用いて、上記他の状態量の推定値の複数の候補値を算出する。
上記相関部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出する。
上記尤度算出部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記相関部が仮定したモデル化誤差上限値と、上記相関部が抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出する。
上記候補選択部は、上記処理装置を用いて、上記尤度算出部が算出した尤度に基づいて、上記候補算出部が算出した複数の候補値のなかから、上記他の状態量の推定値を選択する。
上記推定値更新部は、上記処理装置を用いて、上記相関部が仮定した第一の状態量の推定値と上記候補選択部が選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新する。
上記推定値出力部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力する。
【０１４８】
第一の状態量が所定の推定値であると仮定して、状態量の推定値の候補値を算出するので、状態量の推定値の組み合わせの数が減り、相関部や尤度算出部の処理を減らすことができる。これにより、目標の航跡の推定に必要な計算量が減るので、処理能力の低い処理装置を用いてもリアルタイムでの処理が可能になるなど、追尾装置１００のコストを下げることができる。
【０１４９】
上記推定装置（１００）は、収束判定部（１７３；詳細探索部１７０）と、誤差上限値更新部（１７４；詳細探索部１７０）と、詳細相関部（相関部１６１；詳細探索部１７０）と、詳細尤度算出部（尤度算出部１６２；詳細探索部１７０）とを有する。
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定する。
上記誤差上限値更新部は、上記推定値更新部が上記複数の状態量の推定量を更新した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくする。
上記詳細相関部は、上記処理装置を用いて、上記複数の状態量が、上記推定値更新部が更新した推定値であり、上記モデル化誤差が、上記誤差上限値更新部が小さくしたモデル化誤差上限値以下であるとの仮定に基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出する。
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出する。
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新する。
上記推定値出力部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力する。
【０１５０】
状態量の推定値が収束するまで更新処理を繰り返すので、航跡の推定精度を高くすることができる。
【０１５１】
上記推定装置（１００）は、収束判定部（１７３；詳細探索部１７０）と、終了判定部（１７５；詳細探索部１７０）と、誤差上限値更新部（１７４；詳細探索部１７０）と、詳細相関部（相関部１６１；詳細探索部１７０）と、詳細尤度算出部（尤度算出部１６２；詳細探索部１７０）とを有する。
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定する。
上記終了判定部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記複数の状態量の推定を終了するか否かを判定する。
上記誤差上限値更新部は、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくする。
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出する。
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出する。
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合、および、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新する。
上記推定値出力部は、上記複数の状態量の推定を終了すると上記終了判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力する。
【０１５２】
状態量の推定値が収束するまで、モデル化誤差上限値を変えずに更新処理を繰り返し、状態量の推定値が収束した場合に、モデル化誤差上限値を小さくして、更に、状態量の推定値が収束するまで更新処理を繰り返すので、状態量の推定精度を高くすることができる。
【０１５３】
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値が上記目標を観測した観測値であるかと判定する範囲を算出するために使用するゲート行列を算出する。
上記終了判定部は、上記処理装置を用いて、上記詳細相関部が算出したゲート行列に基づいて、上記複数の状態量の推定を終了するか否かを判定する。
【０１５４】
ゲート行列に基づいて状態量の推定を終了するか否かを判定するので、精度よく終了判定をすることができる。
【０１５５】
上記終了判定部は、上記複数の状態量の推定を終了するか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記複数の状態量の推定を終了すると判定する。
【０１５６】
判定回数に基づいて状態量の推定を終了するか否かを判定するので、終了判定に必要な計算量を少なくすることができる。
【０１５７】
上記誤差上限値更新部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差上限値を更新する。
ａｉｍａｘ←√［（ａｉｍａｘ）２−Δａｉ２］
ただし、ａｉｍａｘは、上記モデル化誤差上限値を表わすベクトルのｉ番目の成分。Δａｉは、上記推定値更新部が更新した後の第一の状態量の推定値を表わすベクトルのｉ番目の成分と、上記推定値更新部が更新する前の第一の状態量の推定値を表わすベクトルのｉ番目の成分との差。
【０１５８】
これにより、モデル化誤差上限値を最適に更新することができるので、全体としての繰り返し回数が減り、目標の航跡の推定に必要な計算量を少なくすることができる。
【０１５９】
上記誤差上限値更新部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差上限値を更新する。
ａｉｍａｘ←λ・ａｉｍａｘ
ただし、λは、モデル化誤差上限値の逓減率であり、０超１未満の実数。
【０１６０】
これにより、モデル化誤差上限値の更新に必要な計算量を少なくすることができるので、目標の航跡の推定に必要な計算量を少なくすることができる。
【０１６１】
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、ゲート行列を算出し、算出したゲート行列を使用して、上記複数の観測値が上記目標を観測した観測値であると判定する範囲を算出し、上記複数の観測値のうち、算出した範囲内に入る観測値を、上記目標を観測した観測値であると判定する。
Ｓ←Ｒ＋Ｑ
ただし、Ｓは、ゲート行列であり、上記観測値を表わすベクトルの成分の数を次数とする正方行列。Ｒは、観測雑音共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記観測値を表わすベクトルの各成分の観測精度の二乗を対角成分とする対角行列。Ｑは、モデル化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記モデル化誤差上限値を表わすベクトルの各成分から算出される値を対角成分とする対角行列。
【０１６２】
観測誤差とモデル化誤差とを考慮したゲート行列を使って、目標を観測した観測値を判定するので、モデル化誤差が大きい場合でも、目標を観測した観測値を精度よく判定することができる。
【０１６３】
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、ゲート行列を算出し、算出したゲート行列を使用して、上記複数の観測値が上記目標を観測した観測値であると判定する範囲を算出し、上記複数の観測値のうち、算出した範囲内に入る観測値を、上記目標を観測した観測値であると判定する。
Ｓ←Ｒ＋Ｑ＋Ｑ’
ただし、Ｑ’は、量子化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とする対称行列。
【０１６４】
観測誤差とモデル化誤差と量子化誤差とを考慮したゲート行列を使って、目標を観測した観測値を判定するので、量子化誤差が大きい場合でも、目標を観測した観測値を精度よく判定することができる。
【０１６５】
上記推定装置は、複数の時刻における上記目標の位置を、上記目標を観測した観測値として入力し、所定の基準時刻における上記目標の位置及び速度及び加速度を、上記複数の状態量として推定する。
上記相関部は、上記処理装置を用いて、上記目標の加速度を上記第一の状態量として、上記加速度の推定値及びモデル化誤差上限値を仮定する。
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差共分散行列の対角成分を算出する。
ｑｉ（ｔｋ）←（ａｉｍａｘ・Δｔｋ２／２）２
ただし、ｑｉ（ｔｋ）は、時刻ｔｋにおけるモデル化誤差共分散行列Ｑのｉ行ｉ列成分。ａｉｍａｘは、上記加速度のモデル化誤差上限値を表わすベクトルのｉ番目の成分。Δｔｋは、上記基準時刻から上記時刻ｔｋまでの経過時間。
【０１６６】
目標の加速度のモデル化誤差上限値に基づいて目標の位置のモデル化誤差共分散行列を算出し、算出した目標の位置のモデル化誤差共分散行列に基づいて、ゲート行列を算出するので、目標を観測した観測値を精度よく判定することができる。
【０１６７】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定する。
上記相関部及び上記詳細相関部は、上記処理装置を用いて、上記基準時刻における上記目標の複数の状態量の推定値または推定値の候補に基づいて、上記複数の時刻それぞれにおける上記目標の複数の状態量を予測し、上記複数の観測値のうち次の不等式を満たす観測値を、上記目標を観測した観測値であると判定する。
［ｚ−ｈ（ｘｋ）］Ｔ・Ｓｋ−１・［ｚ−ｈ（ｘｋ）］≦ｄ
ただし、ｚは、上記観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。Ｓｋは、上記時刻ｔｋにおけるゲート行列。上付きの−１は、逆行列を表わす。ｄは、ゲートの大きさを表わすパラメータであり、０超の実数。
【０１６８】
観測時刻によって異なるゲート行列に基づいて目標を観測した観測値を判定するので、目標を観測した観測値を精度よく判定することができる。
【０１６９】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定する。
上記尤度算出部及び詳細尤度算出部は、上記処理装置を用いて、次の式の右辺を計算することにより、尤度を算出する。
【数３５】
ただし、φ（ＺＮ｜ｘＰ）は、上記複数の状態量が上記推定値である尤度。ｌｎは、自然対数を表わす。ＰＤは、目標の探知確率を表わすパラメータであり、０超１未満の実数。βは、不要信号密度を表わすパラメータであり、０超の実数。ｄｅｔは、行列式を表わす。πは、円周率。Ｓｋは、時刻ｔｋにおけるゲート行列。ｍｋは、上記複数の観測値のうち、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値の数。ｚｋ，ｊは、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。上付きの−１は、逆行列を表わす。
【０１７０】
上の式を使って算出した尤度に基づいて、目標の航跡の候補を選択し、更新するので、航跡の推定精度を高くすることができる。
【０１７１】
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値を更新したことによる上記尤度の変化に基づいて、上記複数の状態量の推定値が収束したか否かを判定する。
【０１７２】
状態量の推定値の更新による尤度の変化に基づいて収束判定をするので、収束判定を効率よくすることができ、目標の航跡の推定に必要な計算量を減らすことができる。
【０１７３】
上記収束判定部は、上記複数の状態量の推定値が収束したか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値が収束したと判定する。
【０１７４】
収束判定をした回数に基づいて収束判定をするので、収束判定に必要な計算量を減らすことができ、全体として、目標の航跡の推定に必要な計算量を減らすことができる。
【０１７５】
上記推定値更新部は、上記処理装置を用いて、上記尤度が高くなる上記複数の状態量の推定値の差分を算出し、上記複数の状態量それぞれについて、算出した差分を上記状態量の推定値に加えることにより、上記複数の状態量の推定値を更新する。
【０１７６】
上記推定値更新部は、上記処理装置を用いて、最急降下法およびニュートン法および準ニュートン法および共役分配法のいずれかにより、上記尤度が高くなる上記複数の状態量の推定値の差分を算出する。
【０１７７】
上記のような最適化方式を使って、状態量の推定値を更新するので、効率よく状態量の推定値を更新することができる。
【０１７８】
以上説明した推定装置（追尾装置１００）は、データを処理する処理装置を有するコンピュータを上記推定装置として機能させるコンピュータプログラムを、コンピュータが実行することにより実現することができる。
【符号の説明】
【０１７９】
１００ 追尾装置、１１０ 観測データ抽出部、１１１ 観測値取得部、１２０ 観測データ蓄積部、１２１ 観測値記憶部、１３０ 観測データ管理部、１４０ 航跡候補生成部、１４１ 候補算出部、１４２ 候補記憶部、１５０ 相関ゲート設定部、１５１ 誤差上限初期化部、１５２ 誤差上限値記憶部、１５３ ゲート行列算出部、１６０ 概探索部、１６１ 相関部、１６２ 尤度算出部、１６３ 選択部、１７０ 詳細探索部、１７１ 推定値記憶部、１７２ 推定値更新部、１７３ 収束判定部、１７４ 誤差上限値更新部、１７５ 終了判定部、１７６ 推定値出力部、７００ 目標、８００ 追尾システム、８１０ センサ、９１１ 処理装置、９１２ 入力装置、９１３ 出力装置、９１４ 記憶装置。
【技術分野】
【０００１】
この発明は、入力データの時系列から目標航跡を抽出することにより目標の航跡を推定し目標を追尾する推定装置（追尾装置）に関する。
【背景技術】
【０００２】
センサによって取得された、多数の不要信号を含む入力データの時系列から、目標航跡を抽出する方式として、ＭＬ−ＰＤＡ（Ｍａｘｉｍｕｍ Ｌｉｋｅｌｉｈｏｏｄ Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｄａｔａ Ａｓｓｏｃｉａｔｉｏｎ）がある。ＭＬ−ＰＤＡでは、目標の位置や速度などをあらかじめ仮定し、最尤法により目標航跡を抽出する。
ＭＬ−ＰＤＡにおいて、パラメータ探索を効率化する方式として、格子点探索方式による概探索と、概探索の結果を初期値とする準Ｎｅｗｔｏｎ法等の最適化アルゴリズムによる詳細探索とを組み合わせる方式がある。
更に、概探索を効率化する方式として、最初は粗く格子点を生成し、評価関数が予め設定した閾値以上となった格子点を含む領域において再び格子点を生成するということを繰り返す方式がある。
また、航跡の位置ベクトルと観測値ベクトルとの差が小さいほど評価関数値が増大することを利用して、格子点の位置成分を観測値ベクトルに置き換えることにより、速度や加速度など、直接観測できない高次のパラメータについてのみ格子点を生成する方式がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００８−９６１３７号公報
【非特許文献】
【０００４】
【非特許文献１】Ｔ．Ｋｉｒｕｂａｒａｊａｎ他「Ｐａｓｓｉｖｅ ｒａｎｇｉｎｇ ｏｆ ａ ｌｏｗ ｏｂｓｅｒｖａｂｌｅ ｂａｌｌｉｓｔｉｃ ｍｉｓｓｉｌｅ ｉｎ ａ ｇｒａｖｉｔａｔｉｏｎａｌ ｆｉｅｌｄ」ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍｓ，ｖｏｌ．３７，ｎｏ．２，４８１〜４９４ページ、２００１年。
【非特許文献２】Ｗ．Ｒ．Ｂｌａｎｄｉｎｇ他「Ｄｉｒｅｃｔｅｄ ｓｕｂｓｐａｃｅ ｓｅａｒｃｈ ＭＬ−ＰＤＡ ｗｉｔｈ ａｐｐｌｉｃａｔｉｏｎ ｔｏ ａｃｔｉｖｅ ｓｏｎａｒ ｔｒａｃｋｉｎｇ」ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍｓ，ｖｏｌ．４４，ｎｏ．１，２０１〜２１６ページ、２００８年。
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ＭＬ−ＰＤＡを用いて目標を追尾する場合、例えば目標が平面内を等速直線運動しているとすると、推定すべきパラメータ（状態量）は、目標の初期位置ベクトルと速度ベクトルであり、各々２次元の計４次元空間から尤度が最大となるパラメータを探索する。一方、目標が平面内を等加速度運動しているとすると、推定すべきパラメータに加速度ベクトルが加わるため、６次元空間からパラメータを探索する必要がある。目標が空間内を運動しているとすると、等速直線運動の場合で６次元空間、等加速度運動の場合で９次元空間からパラメータを探索する必要がある。
したがって、推定すべきパラメータの数が増えると、追尾に必要な計算量が多くなり、処理に時間がかかる。
【０００６】
格子点探索方式による概探索と、概探索の結果を初期値とする最適化アルゴリズムによる詳細探索とを組み合わせる方式において、概探索の処理時間は、生成する格子点数に依存する。また、詳細探索は、一般に概探索に比べて少ない処理で済む。探索精度を向上させるためには、概探索において格子点の数を十分多く生成する必要がある。格子点を増やすと、計算量が多くなるので、処理に時間がかかる。
【０００７】
粗く格子点を生成し、段階的に格子点生成領域を絞り込む格子点探索方式を用いる場合、概探索における推定精度を見積もることはできるが、探索空間の次元が減るわけではないため、探索の処理に時間がかかる。
また、格子点の位置成分を観測値ベクトルに置き換える格子点探索方式を用いる場合、位置成分の候補数が観測値数に依存するため、観測値数が少ない場合は格子点数を減らすことができるが、高次のパラメータについての格子点数が減るわけではないため、高次のパラメータ数が多い場合は探索の処理に時間がかかる。
【０００８】
この発明は、例えば上記のような課題を解決するためになされたものであり、処理時間を低減しつつ目標航跡を精度良く抽出することを目的とする。
【課題を解決するための手段】
【０００９】
この発明にかかる推定装置は、データを処理する処理装置と、候補算出部と、相関部と、尤度算出部と、選択部と、推定値更新部と、推定値出力部とを有し、
上記候補算出部は、目標の複数の状態量のうち所定の第一の状態量を除く他の状態量について、上記処理装置を用いて、上記他の状態量の推定値の複数の候補値を算出し、
上記相関部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出し、
上記尤度算出部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記相関部が仮定したモデル化誤差上限値と、上記相関部が抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出し、
上記候補選択部は、上記処理装置を用いて、上記尤度算出部が算出した尤度に基づいて、上記候補算出部が算出した複数の候補値のなかから、上記他の状態量の推定値を選択し、
上記推定値更新部は、上記処理装置を用いて、上記相関部が仮定した第一の状態量の推定値と上記候補選択部が選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする。
【発明の効果】
【００１０】
この発明にかかる推定装置によれば、処理時間を低減しつつ目標航跡を精度良く抽出することができる。
【図面の簡単な説明】
【００１１】
【図１】実施の形態１における追尾システム８００の全体構成の一例を示すシステム構成図。
【図２】実施の形態１における追尾装置１００のハードウェア資源の一例を示すハードウェア構成図。
【図３】実施の形態１における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図４】実施の形態１における追尾処理Ｓ６００の流れの一例を示すフローチャート図。
【図５】目標の航跡の一例を示す図。
【図６】実施の形態１における概探索ステップＳ６２０における相関ゲートと、得られる航跡との関係の一例を示す図。
【図７】実施の形態１における詳細探索ステップＳ６３０における更新前の相関ゲート及び航跡と更新後の相関ゲート及び航跡との関係の一例を示す図。
【図８】実施の形態１における詳細探索ステップＳ６３０により最終的に得られる相関ゲートと航跡との関係の一例を示す図。
【図９】実施の形態３における量子化誤差を説明するための図。
【図１０】実施の形態６における追尾処理Ｓ６００の流れの一例を示すフローチャート図。
【図１１】実施の形態７における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図１２】実施の形態７における追尾処理Ｓ６００の流れの一例を示すフローチャート図。
【発明を実施するための形態】
【００１２】
実施の形態１．
実施の形態１について、図１〜図８を用いて説明する。
【００１３】
図１は、この実施の形態における追尾システム８００の全体構成の一例を示すシステム構成図である。
追尾システム８００（推定システム）は、目標７００を観測し、観測した目標７００の航跡を推定して、目標７００を追尾する。追尾システム８００は、センサ８１０と、追尾装置１００とを有する。
センサ８１０（観測装置）は、目標を観測し、観測した観測結果を表わす信号を出力する。センサ８１０は、連続して、もしくは繰り返し、観測を行う。なお、センサ８１０は、追尾装置１００の一部であってもよい。
追尾装置１００（推定装置、航跡推定装置）は、センサ８１０が観測した観測結果に基づいて、目標７００の航跡を推定する。
【００１４】
図２は、この実施の形態における追尾装置１００のハードウェア資源の一例を示すハードウェア構成図である。
追尾装置１００は、例えば、コンピュータである。追尾装置１００は、例えば、処理装置９１１と、入力装置９１２と、出力装置９１３と、記憶装置９１４とを有する。
処理装置９１１は、記憶装置９１４が記憶したプログラムを実行することにより、データを処理し、追尾装置１００全体を制御する。
記憶装置９１４は、処理装置９１１が実行するプログラムや、処理装置９１１が処理するデータなどを記憶する。記憶装置９１４は、例えば、揮発性メモリ、不揮発性メモリ、半導体メモリ、磁気ディスク装置、光ディスク装置などである。
入力装置９１２は、追尾装置１００の外部から信号を入力し、入力した信号を処理装置９１１が処理できるデータに変換する。入力装置９１２が変換したデータは、処理装置９１１が直接処理する構成でもよいし、一時的に記憶装置９１４が記憶する構成でもよい。入力装置９１２は、例えば、アナログ信号をデジタルデータに変換するアナログデジタル変換装置、他の装置が送信した信号を受信して復調する受信装置、操作者の操作を入力するキーボードやマウスなどの操作入力装置などである。
出力装置９１３は、処理装置９１１が処理したデータや記憶装置９１４が記憶したデータなどを変換して、追尾装置１００の外部に出力する。出力装置９１３は、例えば、デジタルデータをアナログ信号に変換するデジタルアナログ変換装置、他の装置に対する信号を送信する送信装置、操作者に対して情報を提示する表示装置・音声出力装置などである。
以下に説明する追尾装置１００の機能ブロックは、記憶装置９１４が記憶したプログラムを処理装置９１１が実行することにより実現される。なお、これは一例であり、追尾装置１００の機能ブロックは、他の方式で実現したものであってもよい。追尾装置１００の機能ブロックは、例えば、アナログ回路やデジタル回路などの電気的構成で実現したものであってもよいし、機械的構成など他の構成により実現したものであってもよい。また、追尾装置１００は、物理的に１つの装置である必要はなく、物理的に複数の装置が、それぞれ追尾装置１００の機能ブロックのうちのいくつかを実現し、全体として追尾装置１００として機能する構成であってもよい。
【００１５】
図３は、この実施の形態における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図である。
追尾装置１００は、例えば、観測データ抽出部１１０と、観測データ蓄積部１２０と、観測データ管理部１３０と、航跡候補生成部１４０と、相関ゲート設定部１５０と、概探索部１６０と、詳細探索部１７０とを有する。
【００１６】
観測データ抽出部１１０は、入力装置９１２を用いて、センサ８１０が出力した信号（入力データ）を入力する。観測データ抽出部１１０は、処理装置９１１を用いて、入力した信号から、目標を観測した観測値（観測データ）を抽出する。なお、センサ８１０が出力する信号は、ノイズなどの影響による不要信号を含むので、観測データ抽出部１１０が抽出する観測値は、必ずしも目標を観測した観測値ではない可能性がある。
観測データ蓄積部１２０は、記憶装置９１４を用いて、観測データ抽出部１１０が抽出した観測値を、センサ８１０が観測した観測時刻（フレーム番号）と対応づけて蓄積して記憶する。
観測データ管理部１３０は、処理装置９１１を用いて、目標の航跡の推定に使用する観測値の観測時刻の範囲を決定する。観測データ管理部１３０は、観測データ蓄積部１２０が記憶した観測値のなかから、観測時刻が決定した範囲内である観測値（観測データ時系列）を抽出する。
航跡候補生成部１４０は、処理装置９１１を用いて、目標の航跡の候補を生成する。航跡候補生成部１４０は、複数の候補（航跡候補群）を生成する。
相関ゲート設定部１５０は、処理装置９１１を用いて、相関ゲート行列を設定・更新する。
概探索部１６０は、処理装置９１１を用いて、観測データ管理部１３０が抽出した観測値と、相関ゲート設定部１５０が設定した相関ゲート行列とに基づいて、航跡候補生成部１４０が生成した候補のなかから、実際の目標の航跡に近い航跡（航跡候補）を抽出する。
詳細探索部１７０は、処理装置９１１を用いて、観測データ管理部１３０が抽出した観測値と、相関ゲート設定部１５０が設定した相関ゲート行列とに基づいて、概探索部１６０が抽出した航跡から出発して、更に実際の目標の航跡に近い航跡を算出する。詳細探索部１７０は、相関ゲート設定部１５０が更新した相関ゲート行列に基づいて、更新した航跡から更に実際の目標の航跡に近い航跡を算出する。詳細探索部１７０は、航跡が収束するまでこれを繰り返す。詳細探索部１７０は、処理装置９１１を用いて、算出した航跡（航跡推定値）を出力する。
【００１７】
図４は、この実施の形態における追尾処理Ｓ６００の流れの一例を示すフローチャート図である。
追尾処理Ｓ６００（推定方法、航跡推定方法）において、追尾装置１００は、センサ８１０が観測した観測結果に基づいて、目標を追尾し、目標の航跡を推定する。追尾処理Ｓ６００は、例えば、データ入力ステップＳ６１１と、観測データ抽出・蓄積ステップＳ６１２と、使用フレーム選択ステップＳ６１３と、相関ゲート設定ステップＳ６１４と、航跡候補設定ステップＳ６１５と、概探索ステップＳ６２０と、詳細探索ステップＳ６３０と、相関ゲート更新ステップＳ６４３と、ゲーティングステップＳ６４４と、尤度計算ステップＳ６４５と、航跡推定値出力ステップＳ６５１とを有する。追尾装置１００は、追尾処理Ｓ６００をデータ入力ステップＳ６１１から開始する。
【００１８】
まず、データ入力ステップＳ６１１において、センサ８１０は、データを取得する。センサ８１０は、例えば、レーダ、赤外線センサ、光学カメラなどである。
次に、観測データ抽出・蓄積ステップＳ６１２において、観測データ抽出部１１０は、入力データから観測データを抽出する。観測データ蓄積部１２０は、観測データを観測された時刻ごとに蓄積する。観測データは、例えば、２次元画像の位置座標である。観測データを抽出する方式としては、例えば、輝度値が予め設定した閾値を超えた画素を抽出する方式、輝度値が上位の画素を一定数個抽出する方式などがある。
【００１９】
次に、使用フレーム選択ステップＳ６１３において、観測データ管理部１３０は、航跡の推定に使用する時刻フレーム（観測時刻）を任意に選択する。選択された時刻フレームをｔ１、ｔ２、…、ｔＮとする。ただし、ｕ＜ｖなる任意のｕとｖについてｔｕ＜ｔｖであるとする。
観測データ管理部１３０は、観測データ蓄積部１２０が蓄積した観測データ群から前記時刻フレームにおける式（１）で表わされる観測データ時系列ＺＮを取り出す。
【数１１】
ただし、Ｚｋは、時刻ｔｋにおける観測データである。時刻ｔｋにおける観測データＺｋは、Ｍｋ個の観測値ベクトルから構成される。また、ｚｋ，ｊは、時刻ｔｋにおける観測値ベクトルである。ｘｋ，ｊ，ｙｋ，ｊは、それぞれｘ座標、ｙ座標を表わす。
【００２０】
次に、相関ゲート設定ステップＳ６１４において、相関ゲート設定部１５０は、式（２）で表わされる相関ゲート行列Ｓｋを設定する。
【数１２】
ただし、Ｒｋは、観測雑音共分散行列である。観測雑音共分散行列Ｒｋは、正定値対称行列であり、例えば、観測値ベクトルの観測精度の２乗値が対角成分に並んだ行列である。
Ｑｋは、モデル化誤差共分散行列である。モデル化誤差共分散行列Ｑｋは、半正定値対称行列であり、例えば、式（３）で表わされる、想定される最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を加速度として目標が移動した場合の目標の変位量の２乗値が対角成分に並んだ行列である。
【数１３】
ただし、ｔＰは、目標の位置、速度、加速度の初期値を定める基準時刻である。基準時刻ｔＰは、例えば、観測開始時刻ｔ１や、最新時刻ｔＮなどである。
また、最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）の値は事前に設定しておく。最大加速度誤差ベクトルａｍａｘの値は、例えば、推定したい目標の種類に応じて設定した値を用いる。
【００２１】
次に、航跡候補設定ステップＳ６１５において、航跡候補生成部１４０は、概探索のための航跡候補群を作成する。ここで、航跡は、基準時刻ｔＰにおける目標の位置ベクトル、速度ベクトル、加速度ベクトルを成分に持つ状態ベクトルｘＰとして、式（４）で記述される。
【数１４】
ただし、（ｘＰ，ｙＰ）は、基準時刻ｔＰにおける目標の位置ベクトルである。（ｘＰドット，ｙＰドット）は、基準時刻ｔＰにおける目標の速度ベクトルである。（ｘＰツードット，ｙＰツードット）は、基準時刻ｔＰにおける目標の加速度ベクトルである。目標の位置ベクトル、速度ベクトル、及び加速度ベクトルは、いずれも、ｘｙ座標で表わす。また、上付きのＴは行列の転置を表わす。
【００２２】
航跡候補生成部１４０は、加速度ベクトルの候補を、ある固定値ａｆｉｘ＝（ａｘｆｉｘ，ａｙｆｉｘ）とした上で、あらかじめ定められた範囲における航跡候補、すなわち状態ベクトル候補を複数個生成する。状態ベクトル候補を生成する範囲は、例えば、位置成分は、センサの探知可能領域の範囲、速度成分は、目標の想定される移動方向に沿った値の範囲である。ここで、航跡候補生成部１４０は、位置成分、速度成分が前記範囲内で等間隔になるように生成する構成であってもよいし、乱数を用いて不等間隔に生成する構成であってもよい。また、航跡候補生成部１４０は、位置成分の候補については、観測値ベクトルを使う構成であってもよい。
また、加速度ベクトル候補の固定値ａｆｉｘは、事前に設定しておく。加速度ベクトル候補の固定値ａｆｉｘは、例えば、０［ｍ／ｓ２］である。あるいは、航跡候補生成部１４０は、前回の推定結果の加速度ベクトル推定値を、加速度ベクトル候補の固定値ａｆｉｘとして用いる構成であってもよい。
【００２３】
次に、概探索ステップＳ６２０において、概探索部１６０は、概探索をする。概探索ステップＳ６２０は、状態遷移ステップＳ６２３と、ゲーティングステップＳ６２４と、尤度計算ステップＳ６２５と、最大尤度比航跡候補抽出ステップＳ６２７とを有する。概探索部１６０は、航跡候補生成部１４０が生成した航跡候補それぞれについて、状態遷移ステップＳ６２３、ゲーティングステップＳ６２４、尤度計算ステップＳ６２５を実行する。
【００２４】
概探索ステップＳ６２０の状態遷移ステップＳ６２３において、概探索部１６０は、航跡候補ごとに、式（５）にしたがって、各時刻ｔｋにおける状態ベクトルｘｋを計算する。
【数１５】
ただし、Ｉ２は２行２列の単位行列である。０２は、２行２列の零行列である。
【００２５】
次に、概探索ステップＳ６２０のゲーティングステップＳ６２４において、概探索部１６０は、航跡候補ごとに、各時刻ｔｋにおける状態ベクトルｘｋの位置成分と、相関ゲート設定部１５０が設定した相関ゲート行列Ｓｋに基づいて、楕円形状の相関ゲートＧｋを、式（６）を使って設定する。
【数１６】
ただし、Ｈは、状態ベクトルから位置成分を取り出す観測行列である。また、ｄは、事前に設定するゲートサイズパラメータである。ゲートサイズパラメータｄの値は、自由度２のカイ２乗分布より定まる正定数である。上付きのＴは、転置を表わす。上付きの−１は、逆行列を表わす。
【００２６】
次に、概探索ステップＳ６２０の尤度計算ステップＳ６２５において、概探索部１６０は、航跡候補ごとに、各時刻ｔｋにおける状態ベクトルｘｋと、相関ゲートＧｋに含まれる観測値ベクトルｚｋ，ｊとに基づいて、式（７）を使って尤度φを計算する。
【数１７】
ただし、ＰＤは、事前に設定する探知確率パラメータである。ＰＤは、０＜ＰＤ＜１なる定数である。また、βは、事前に設定する不要信号密度パラメータである。βは、正定数である。また、ｍｋは時刻ｔｋにおける観測データＺｋを構成するＭｋ個の観測値ベクトルのうち、式（６）の相関ゲートＧｋに含まれる観測値ベクトルの個数である。また、ｚｋ，ｊは、前記観測データＺｋを構成する観測値ベクトルのうち、式（６）の相関ゲートＧｋに含まれるものである。ｌｎは、自然対数（ネイピア数を底とする対数関数）を表わす。ｄｅｔは、行列式を表わす。ｅｘｐは、ネイピア数を底とする指数関数を表わす。
【００２７】
なお、式（６）及び式（７）は、目標の位置観測誤差の分布が正規分布で与えられる場合についての相関ゲート及び尤度の計算式の例である。観測誤差の分布が正規分布以外である場合などにおいては、相関ゲートは楕円形状以外の形状である構成であってもよい。また、尤度の計算式は任意に設定する構成であってもよい。例えば、尤度の計算式は、観測値ベクトルと状態ベクトルの位置成分との距離の逆２乗値を用いる構成であってもよい。また、尤度の計算式は、観測データの振幅情報や輝度情報に基づくものを用いる構成であってもよい。
【００２８】
概探索部１６０は、状態遷移ステップＳ６２３、ゲーティングステップＳ６２４、尤度計算ステップＳ６２５を繰り返し、航跡候補生成部１４０が生成したすべての航跡候補を処理する。
【００２９】
次に、概探索ステップＳ６２０の最大尤度比航跡候補抽出ステップＳ６２７において、概探索部１６０は、航跡候補の中から、尤度φが最大となるものを抽出する。そして、詳細探索部１７０は、最大尤度となる航跡候補（状態ベクトル候補）を、状態ベクトルの初期値として入力する。
【００３０】
次に、詳細探索ステップＳ６３０において、詳細探索部１７０は、詳細探索をする。詳細探索ステップＳ６３０は、探索方向計算ステップＳ６３１と、航跡更新ステップＳ６３２と、状態遷移ステップＳ６３３と、ゲーティングステップＳ６３４と、尤度計算ステップＳ６３５と、探索収束条件分岐ステップＳ６３６とを有する。
【００３１】
詳細探索ステップＳ６３０の探索方向計算ステップＳ６３１において、詳細探索部１７０は、前回の探索における状態ベクトルを起点として、より尤度φが大きくなる方向ベクトルを計算する。探索方向の計算方式は、例えば、最急降下法、Ｎｅｗｔｏｎ法、準Ｎｅｗｔｏｎ法、共役勾配法などがある。
【００３２】
次に、詳細探索ステップＳ６３０の航跡更新ステップＳ６３２において、詳細探索部１７０は、前回の探索における状態ベクトルを、探索方向計算ステップＳ６３１で計算した方向ベクトルを用いて、前回の探索よりも尤度の大きい状態ベクトルに更新する。
【００３３】
次に、詳細探索ステップＳ６３０の状態遷移ステップＳ６３３、ゲーティングステップＳ６３４及び尤度計算ステップＳ６３５において、詳細探索部１７０は、状態遷移、ゲーティング、尤度計算を行う。状態遷移ステップＳ６３３、ゲーティングステップＳ６３４及び尤度計算ステップＳ６３５における詳細探索部１７０の動作は、概探索の対応するステップ（状態遷移ステップＳ６２３、ゲーティングステップＳ６２４及び尤度計算ステップＳ６２５）における概探索部１６０の動作と同一である。
【００３４】
そして、詳細探索ステップＳ６３０の探索収束条件分岐ステップＳ６３６において、詳細探索部１７０は、探索の収束を判定する。探索の収束判定方式には、例えば、尤度勾配ベクトルのノルムが十分小さい所定の閾値より小さくなった場合に収束したと判定する方式、前回の探索の尤度と今回の探索の尤度との差が十分小さい所定の閾値より小さくなった場合に収束したと判定する方式、探索収束条件分岐ステップＳ６３６を通過した回数があらかじめ定めた上限値を超えた場合に収束したと判定する方式などがある。
探索が収束したと判定しなかった場合、詳細探索部１７０は、相関ゲート更新ステップＳ６４３へ処理を進める。
探索が収束したと判定した場合、詳細探索部１７０は、航跡推定値出力ステップＳ６５１へ処理を進める。
【００３５】
相関ゲート更新ステップＳ６４３において、相関ゲート設定部１５０は、相関ゲート行列Ｓｋを更新する。例えば、相関ゲート設定部１５０は、探索によって得られた加速度ベクトル推定値ａｅｓｔ＝（ａｘｅｓｔ，ａｙｅｓｔ）、加速度ベクトル固定値ａｆｉｘ＝（ａｘｆｉｘ，ａｙｆｉｘ）、及び想定される最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を用いて、式（８）のように、相関ゲート行列Ｓｋを更新する。
【数１８】
【００３６】
最大加速度誤差ベクトルａｍａｘは、加速度ベクトル固定値ａｆｉｘと実際の目標加速度ベクトルとの差の最大許容量を表すものである。また、加速度ベクトル固定値ａｆｉｘと加速度ベクトル推定値ａｅｓｔとの差は、加速度ベクトル固定値ａｆｉｘが実際の目標加速度ベクトルに近づくための補正量を表すものである。したがって、前記補正量の分だけ加速度誤差が減少したと考えられる。そこで、前記最大許容量から前記補正量を減算することにより、モデル化誤差共分散行列Ｑｋの大きさを小さくする。そして、更新後の加速度誤差最大許容量（ａハットｘｍａｘ，ａハットｙｍａｘ）を新たな最大加速度誤差ベクトルａハットｍａｘとして設定する。ただし、式（９）の右辺が負値となる場合、相関ゲート設定部１５０は、更新後の加速度誤差最大許容量ａハットｘｍａｘあるいはａハットｙｍａｘを、０［ｍ／ｓ２］とする。
【００３７】
そして、ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５において、詳細探索部１７０は、更新された相関ゲート行列Ｓｋを用いて、ゲーティング及び尤度計算を行う。ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５における詳細探索部１７０の動作は、詳細探索ステップＳ６３０のゲーティングステップＳ６３４及び尤度計算ステップＳ６３５における動作と同様である。
そして、詳細探索部１７０は、再び、詳細探索ステップＳ６３０に処理を戻し、探索方向計算ステップＳ６３１以降の動作を行う。
【００３８】
探索収束条件分岐ステップＳ６３６で探索が収束したと判定した場合、航跡推定値出力ステップＳ６５１において、詳細探索部１７０は、航跡推定値として探索した状態ベクトルを出力する。
【００３９】
図５は、目標の航跡の一例を示す図である。
例えば、目標が一定の加速度で等加速度運動をしているものとする。航跡候補生成部１４０が生成した航跡の候補における加速度は、実際の目標の加速度と異なっている。
【００４０】
図６は、この実施の形態における概探索ステップＳ６２０における相関ゲートと、得られる航跡との関係の一例を示す図である。
この例において、基準時刻ｔＰは、時刻ｔ１である。観測時刻と基準時刻ｔＰとの差が大きくなるにつれて、相関ゲートが大きくなる。これにより、加速度の固定値と実際の目標の加速度との間の差が大きい場合でも、観測値が相関ゲートに入るので、どの航跡の候補が実際の航跡に最も近いかを判定することができる。
【００４１】
図７は、この実施の形態における詳細探索ステップＳ６３０における更新前の相関ゲート及び航跡と更新後の相関ゲート及び航跡との関係の一例を示す図である。
詳細探索部１７０は、基準時刻における目標の位置、速度及び加速度の推定値を更新することにより、推定航跡を実際の目標の航跡に近づける。これにより、相関ゲートを小さくしても、観測値が相関ゲートに入るようになる。
相関ゲート設定部１５０が小さくした相関ゲートに基づいて、詳細探索部１７０が航跡を更新することにより、更に、推定航跡を実際の目標の航跡に近づけることができる。
【００４２】
図８は、この実施の形態における詳細探索ステップＳ６３０により最終的に得られる相関ゲートと航跡との関係の一例を示す図である。
航跡の更新と相関ゲートの更新とを、航跡が収束するまで繰り返すことにより、最終的に、実際の目標の航跡にほぼ一致する航跡を推定することができる。
【００４３】
この実施の形態における追尾装置１００は、入力データの時系列から目標航跡を抽出することによって目標を追尾する。
追尾装置１００は、観測データ抽出部１１０と、観測データ蓄積部１２０と、観測データ管理部１３０と、航跡候補生成部１４０と、相関ゲート設定部１５０と、概探索部１６０と、詳細探索部１７０とを有する。
前記観測データ抽出部１１０は、時刻ごとに入力データから観測データを抽出する。
前記観測データ蓄積部１２０は、前記観測データを時刻ごとに蓄積する。
前記観測データ管理部１３０は、指定された複数の時刻の観測データを前記観測データ蓄積部１２０から取り出す。
前記航跡候補生成部１４０は、目標航跡を記述するパラメータのうちいくつかを固定した上でパラメータの候補を多数生成する。
前記相関ゲート設定部１５０は、前記航跡候補生成部１４０で生成されたパラメータ候補におけるいくつかの固定されたパラメータと目標の真のパラメータとの誤差を最大限考慮した相関ゲートを設定する。
前記概探索部１６０は、前記航跡候補生成部１４０で生成されたパラメータ候補と、前記観測データ管理部１３０で取り出された観測データとから、前記相関ゲート設定部１５０で設定された相関ゲートを用いて前記パラメータ候補の尤度を計算するとともに、前記尤度が最大となるパラメータ候補を抽出する。
前記詳細探索部１７０は、前記概探索部１６０で抽出された尤度が最大となるパラメータ候補を初期値として、前記尤度が大きくなる方向にパラメータを更新しながら、前記相関ゲートをより小さくなるように更新して、前記尤度が最大となるパラメータを探索することにより、目標航跡を記述するパラメータの推定値を出力する。
【００４４】
前記詳細探索部１７０は、前記尤度が大きくなる方向にパラメータを更新するたびに、前記相関ゲートをより小さくなるように更新することを繰り返す。
【００４５】
前記航跡候補生成部１４０は、航跡を記述するパラメータとして目標の位置、速度、加速度を設定するとともに、固定パラメータとして加速度を選択する。
前記相関ゲート設定部１５０は、前記航跡候補生成部１４０で固定された加速度と目標の真の加速度との誤差を最大限考慮した相関ゲートを設定する。
【００４６】
前記相関ゲート設定部１５０は、目標の観測誤差共分散行列と、目標の真の加速度との最大誤差許容量を考慮した共分散行列を用いた式（２）のような相関ゲート行列を用いて相関ゲートを規定する。
【００４７】
前記相関ゲート設定部１５０は、目標の真の加速度との最大誤差許容量を考慮した共分散行列として式（３）を用いる。
【００４８】
前記概探索部１６０及び前記詳細探索部１７０は、前記相関ゲート行列を用いて相関ゲートを式（６）のように設定し、パラメータの尤度を式（７）のように計算する。
【００４９】
前記詳細探索部１７０は、前記相関ゲートを式（８）のように更新する。
【００５０】
この実施の形態における追尾装置１００は、設定した加速度固定値と実際の目標加速度との差をモデル化誤差として表現し、モデル化誤差を考慮した相関ゲートを設定して概探索を行う。これにより、設定した加速度固定値が実際と異なっていても目標観測値を相関ゲート内に捕捉することができる。
さらに、この実施の形態における追尾装置１００は、概探索においては位置ベクトルと速度ベクトルのみを探索し、加速度ベクトルのような高次のパラメータについては探索を行わない。これにより、加速度ベクトルも探索する場合に比べて航跡候補数を減らすことができ、処理時間を削減することができる。
さらに、この実施の形態における追尾装置１００は、詳細探索においては加速度誤差を表すモデル化誤差共分散行列の大きさを、推定結果の加速度を基に計算した補正量を用いて段階的に小さくしながら、加速度ベクトルのような高次のパラメータも含めて探索を行う。これにより、相関ゲートを目標観測値の存在する近傍に絞り込みつつ、実際の目標の位置ベクトル、速度ベクトル、及び加速度ベクトルを精度良く推定することが可能となる。
【００５１】
なお、この実施の形態では、平面内を等加速度運動する目標を追尾する構成について説明したが、追尾装置１００は、他の運動モデルに基づいて目標を追尾する構成であってもよい。例えば、目標の位置、速度、加速度などを二次元ベクトルで表わすのではなく、三次元ベクトルで表わす構成であってもよい。あるいは、目標が、等加速度運動ではなく、例えば等速円運動など他の運動をしていると仮定して、目標を追尾する構成であってもよい。
【００５２】
実施の形態２．
実施の形態２について、説明する。
なお、実施の形態１と共通する部分については、説明を省略する。
【００５３】
実施の形態１における相関ゲート設定部１５０は、式（８）のように加速度ベクトル推定結果を用いて相関ゲート行列Ｓｋを更新する。これに対し、この実施の形態における相関ゲート設定部１５０は、加速度ベクトル推定結果を用いずに、相関ゲート行列Ｓｋを更新する。例えば、相関ゲート設定部１５０は、想定される最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を用いて、式（１０）のように、相関ゲート行列Ｓｋを更新する。
【数１９】
ただし、γは、事前に設定する減衰係数パラメータである。γは、０＜γ＜１なる定数である。また、ｎは、詳細探索の反復回数（探索収束条件分岐ステップＳ６３６を通過した回数）である。
【００５４】
この実施の形態における追尾装置１００において、前記詳細探索部１７０は、前記相関ゲートを式（１０）のように更新する。
【００５５】
この実施の形態における追尾装置１００は、式（１０）による相関ゲート行列の更新をして、加速度ベクトルの推定結果に関わらず、一定率でモデル化誤差共分散行列Ｑｋの大きさを減少させる。これにより、詳細探索における相関ゲートの絞り込みを容易にすることができる。
【００５６】
実施の形態３．
実施の形態３について、図９を用いて、説明する。
なお、実施の形態１及び実施の形態２と共通する部分については、説明を省略する。
【００５７】
実施の形態１における概探索部１６０は、式（２）のような相関ゲート行列を設定する。これに対し、この実施の形態における概探索部１６０は、格子点探索方式によって概探索を行う場合、探索空間を量子化することと同等であることから、状態ベクトル候補には観測誤差の他に量子化誤差が含まれると考え、式（１１）のような相関ゲート行列を設定する。
【数２０】
ただし、Ｑ’ｋは量子化誤差共分散行列である。量子化誤差共分散行列Ｑ’ｋは、一定の半正定値対称行列である。
【００５８】
図９は、この実施の形態における量子化誤差を説明するための図である。
例えば、航跡候補生成部１４０が生成する航跡の候補において、所定の間隔を有する複数の値を、位置や速度の推定値の候補とする場合、尤度が最も高い航跡の候補を選択しても、実際の目標の航跡の位置や速度との間にずれが生じる。これが量子化誤差である。
したがって、位置や速度の推定値の候補の間の間隔から、量子化誤差の絶対値の最大値を求めることができる。
概探索部１６０は、このようにして算出した量子化誤差の絶対値の最大値に基づいて、量子化誤差協分散行列Ｑ’ｋを算出する。
【００５９】
詳細探索で探索を進めるごとに、量子化誤差の影響は小さくなる。そこで、相関ゲート設定部１５０は、例えば、式（１０）のモデル化誤差共分散行列の更新式と同様に、量子化誤差共分散行列に詳細探索の反復回数に応じて一定の減衰係数を乗ずる。相関ゲート設定部１５０は、例えば、次の式により、相関ゲート行列Ｓｋを更新する。
【数２１】
ただし、ζは、事前に設定する減衰係数パラメータである。ζは、０＜ζ＜１なる定数である。また、ｎは、詳細探索の反復回数（探索収束条件分岐ステップＳ６３６を通過した回数）である。
【００６０】
あるいは、相関ゲート設定部１５０は、例えば、次の式により、相関ゲート行列Ｓｋを更新する。
【数２２】
【００６１】
この実施の形態における追尾装置１００において、前記相関ゲート設定部１５０は、目標の観測誤差共分散行列と、目標の真の加速度との最大誤差許容量を考慮した共分散行列に加えて、さらに概探索におけるパラメータ候補の量子化誤差を考慮した誤差共分散行列を用いた式（１１）のような相関ゲート行列を用いて相関ゲートを規定する。
【００６２】
この実施の形態における追尾装置１００は、概探索において量子化誤差の影響を考慮することにより、目標航跡を精度良く抽出することができる。
【００６３】
実施の形態４．
実施の形態４について、説明する。
なお、実施の形態１〜実施の形態３と共通する部分については、説明を省略する。
【００６４】
実施の形態１における相関ゲート設定部１５０は、最大加速度誤差ベクトルａｍａｘ＝（ａｘｍａｘ，ａｙｍａｘ）を事前に設定する。これに対し、この実施の形態における相関ゲート設定部１５０は、相関ゲートの大きさに上限値を事前に設定することにより、最大加速度誤差ベクトルａｍａｘの値を計算する。例えば、式（６）のように相関ゲートＧｋを設定する場合、相関ゲートＧｋの大きさＶｋは、式（１２）で計算することができる。
【数２３】
ただしｄｅｔは、行列式を表わす。相関ゲート設定部１５０は、このＶｋが事前に設定した上限値を超えないような最大加速度誤差ベクトルａｍａｘの値を求める。
【００６５】
この実施の形態における追尾装置１００において、前記相関ゲート設定部１５０は、前記相関ゲートの大きさに上限値を設定することにより、目標の真の加速度との最大誤差許容量を計算する。
【００６６】
これにより、最大加速度誤差ベクトルの値を相関ゲートの大きさから直感的に設定することが可能となる。
【００６７】
実施の形態５．
実施の形態５について、説明する。
なお、実施の形態１〜実施の形態４と共通する部分については、説明を省略する。
【００６８】
実施の形態１における観測データ管理部１３０は、使用フレーム選択ステップＳ６１３において、航跡の推定に使用する時刻フレームを任意に選択する。これに対し、この実施の形態における観測データ管理部１３０は、使用する時刻フレームの長さに上限値を設定した上で、上限値を超えないように時刻フレームを選択する。
【００６９】
あるいは、観測データ蓄積部１２０は、あらかじめ相関ゲートの大きさの上限を定めておき、相関ゲートの大きさが、あらかじめ定めた上限を超えないよう、時刻フレームの長さを設定する構成であってもよい。
実施の形態４で述べたように、相関ゲートの大きさは、式（１２）を使って計算することができる。実施の形態４では、最大加速度誤差ベクトルを変えることにより相関ゲートの大きさを変える構成を説明したが、相関ゲートの大きさは、基準時刻からの経過時間によっても変化する。例えば、観測データ蓄積部１２０は、最大加速度誤差ベクトルをあらかじめ定めた値に設定したうえで、相関ゲートの大きさが上限を超えないよう、基準時刻からの経過時間の絶対値の最大値を算出する。
【００７０】
あるいは、観測データ蓄積部１２０は、例えば、相関ゲートに含まれる観測値ベクトルの数に上限値を設定し、相関ゲートに含まれる観測値ベクトルの数が上限値を超えないよう、時刻フレームの長さを設定する構成であってもよい。
例えば、観測データ蓄積部１２０は、基準時刻ｔＰから最も離れた観測時刻ｔｘ（例えば、ｔＰ＝ｔ１とするとｔｘ＝ｔＮである。）において、推定結果に用いた相関ゲートに含まれる観測値ベクトルの数ｍｘが事前に設定した閾値Ｍを超えたときの時刻フレーム数Ｎを上限値として設定する。
【００７１】
この実施の形態における追尾装置１００において、前記観測データ管理部１３０は、前記相関ゲートの大きさを基に、使用する時刻の長さに上限値を指定する。
【００７２】
前記観測データ管理部１３０は、前記詳細探索部１７０が出力した結果における最も相関ゲートの大きい時刻について、相関ゲートに含まれる観測データの数が事前に設定した閾値を超えたときに使用していた時刻の長さを上限値とする。
【００７３】
これにより、使用する時刻フレームが長くなることで、基準時刻から最も離れた時刻における相関ゲートが大きくなりすぎて、目標以外の観測値ベクトルが推定結果に与える影響が大きくなることを緩和することができる。
【００７４】
実施の形態６．
実施の形態６について、図１０を用いて説明する。
なお、実施の形態１〜実施の形態５と共通する部分については、説明を省略する。
【００７５】
図１０は、この実施の形態における追尾処理Ｓ６００の流れの一例を示すフローチャート図である。
この実施の形態における追尾処理Ｓ６００は、実施の形態１における追尾処理Ｓ６００のステップに加えて、相関ゲート収束条件分岐ステップＳ６４１を有する。相関ゲート更新ステップＳ６４３は、相関ゲート収束条件分岐ステップＳ６４１の次段にある。
【００７６】
ある相関ゲート行列Ｓを用いたときの状態ベクトルｘの尤度をφ（ｘ；Ｓ）と記述する。詳細探索ステップＳ６３０の航跡更新ステップＳ６３２における更新前の状態ベクトルをｘａ、更新後の状態ベクトルをｘｂ、相関ゲート更新ステップＳ６４３における更新前の相関ゲート行列をＳａ、更新後の相関ゲート行列をＳｂとする。
航跡更新ステップＳ６３２において詳細探索部１７０は尤度が大きくなるように状態ベクトルを更新するので、φ（ｘａ；Ｓａ）≦φ（ｘｂ；Ｓａ）が成り立つ。
しかし、相関ゲート更新ステップＳ６４３において相関ゲート設定部１５０は尤度を考慮せずに相関ゲート行列を更新するので、φ（ｘｂ；Ｓａ）＞φ（ｘｂ；Ｓｂ）となる可能性がある。このため、φ（ｘａ；Ｓａ）≦φ（ｘｂ；Ｓｂ）となるとは限らない。
したがって、航跡を更新するたびに相関ゲートを更新すると、尤度が大きくならず、正しい航跡を推定できない可能性がある。詳細探索ステップＳ６３０の探索方向計算ステップＳ６３１における方向ベクトルが計算できなくなり、真の目標航跡に向けた状態ベクトルの更新が行われない可能性がある。
【００７７】
これを回避するため、詳細探索ステップＳ６３０の探索収束条件分岐ステップＳ６３６において、探索が収束していないと判定した場合、相関ゲート行列を更新せず、一定の相関ゲート行列を保ったまま、詳細探索部１７０は、探索方向計算ステップＳ６３１に処理を戻す。
探索が収束した場合、詳細探索部１７０は、相関ゲート収束条件分岐ステップＳ６４１に処理を進める。
【００７８】
相関ゲート収束条件分岐ステップＳ６４１において、相関ゲート設定部１５０は、相関ゲートが収束したか否かを判定する。相関ゲートの収束判定方式には、例えば、前回の相関ゲート行列の行列式と今回の相関ゲート行列の行列式との差が十分小さい所定の閾値よりも小さくなった場合に収束したと判定する方式、相関ゲート収束条件分岐ステップＳ６４１を通過した回数があらかじめ定めた上限値を超えた場合に収束したと判定する方式などがある。
相関ゲートが収束したと判定した場合、相関ゲート設定部１５０は、航跡推定値出力ステップＳ６５１へ処理を進める。
相関ゲートが収束したと判定しなかった場合、相関ゲート設定部１５０は、相関ゲート更新ステップＳ６４３へ処理を進める。
【００７９】
相関ゲート更新ステップＳ６４３において、相関ゲート設定部１５０は、例えば実施の形態１と同様、式（８）及び式（９）により、相関ゲート行列を更新する。なお、相関ゲート設定部１５０は、実施の形態２や実施の形態３で説明した方式など、他の方式で相関ゲート行列を更新する構成でもよい。
【００８０】
ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５において、詳細探索部１７０は、更新された相関ゲート行列を用いて、ゲーティング及び尤度計算を行う。ゲーティングステップＳ６４４及び尤度計算ステップＳ６４５における詳細探索部１７０の動作は、詳細探索ステップＳ６３０のゲーティングステップＳ６３４及び尤度計算ステップＳ６３５における動作と同様である。
そして、詳細探索部１７０は、再び、詳細探索ステップＳ６３０に処理を戻し、探索方向計算ステップＳ６３１以降の動作を行う。
【００８１】
相関ゲート収束条件分岐ステップＳ６４１で相関ゲートが収束したと判定した場合、航跡推定値出力ステップＳ６５１において、詳細探索部１７０は、航跡推定値として探索した状態ベクトルを出力する。
【００８２】
この実施の形態における追尾装置１００において、前記詳細探索部１７０は、前記尤度が大きくなる方向にパラメータを更新することを繰り返し、尤度が十分大きくなった段階で前記相関ゲートをより小さくなるように更新して、再び尤度が大きくなる方向にパラメータを更新することを繰り返す。
【００８３】
この実施の形態における追尾装置１００は、相関ゲートの更新により尤度が減少する可能性を回避することができ、詳細探索における相関ゲートの絞り込みを容易にすることができる。
【００８４】
実施の形態７．
実施の形態７について、図１１〜図１２を用いて説明する。
なお、実施の形態１〜実施の形態６と共通する部分については、同一の符号を付し、説明を省略する。
【００８５】
図１１は、この実施の形態における追尾装置１００の機能ブロックの構成の一例を示すブロック構成図である。
追尾装置１００（推定装置、航跡推定装置）は、観測値取得部１１１と、観測値記憶部１２１と、候補算出部１４１と、候補記憶部１４２と、誤差上限初期化部１５１と、誤差上限値記憶部１５２と、ゲート行列算出部１５３と、相関部１６１と、尤度算出部１６２と、選択部１６３と、推定値記憶部１７１と、推定値更新部１７２と、収束判定部１７３と、誤差上限値更新部１７４と、終了判定部１７５と、推定値出力部１７６とを有する。
【００８６】
観測値取得部１１１は、入力装置９１２を用いて、センサ８１０が出力した信号を入力する。観測値取得部１１１は、処理装置９１１を用いて、入力した信号に基づいて、センサ８１０が目標を観測した観測値を取得する。観測値取得部１１１は、処理装置９１１を用いて、取得した観測値を表わす観測値データを生成する。
【００８７】
例えば、センサ８１０は、レーダである。センサ８１０は、電波を放射し、放射した電波が目標に当たって反射した反射波を受信する。センサ８１０は、受信した反射波の強さを表わす信号を出力する。センサ８１０は、電波を放射する方向を一次元的に走査する。これにより、センサ８１０が出力した信号から、目標が存在する方向を判別できる。また、電波を放射してから反射波を受信するまでの経過時間に基づいて、センサ８１０から目標までの距離を判別できる。
例えば、観測値取得部１１１は、処理装置９１１を用いて、センサ８１０が出力した信号が表わす反射波の強さが所定の閾値より大きくなったとき、センサ８１０が目標を観測したと判定する。観測値取得部１１１は、処理装置９１１を用いて、反射波の強さが閾値より大きくなったタイミングなどに基づいて、センサ８１０が観測した目標が存在する方向や、センサ８１０から目標までの距離などを算出する。観測値取得部１１１は、処理装置９１１を用いて、算出した目標の方向と、目標までの距離とに基づいて、センサ８１０の位置を原点とした直交座標系における目標の位置（例えばＸ座標及びＹ座標）を算出する。観測値取得部１１１は、処理装置９１１を用いて、算出した目標の位置を観測値とする観測値データを生成する。
【００８８】
あるいは、例えば、センサ８１０は、光学カメラである。センサ８１０は、目標が放射する可視光線や赤外線などの光を受光する。センサ８１０は、受光した光の強さを画素の輝度とする２次元画像を生成する。センサ８１０は、生成した画像を表わす信号を出力する。
例えば、観測値取得部１１１は、処理装置９１１を用いて、センサ８１０が出力した信号が表わす画像において、輝度が高い順に所定の数の画素を抽出し、抽出した画素に目標が観測されたと判定する。なお、輝度が高い画素が連続している場合、観測値取得部１１１は、１つの目標とみなし、そのなかで一番輝度が高い画素だけを抽出する構成であってもよい。観測値取得部１１１は、処理装置９１１を用いて、抽出した画素の画像内における位置（例えばＸ座標及びＹ座標）を算出する。観測値取得部１１１は、処理装置９１１を用いて、算出した画素の位置を観測値とする観測値データを生成する。
【００８９】
なお、センサ８１０は、目標の位置を二次元的に観測する構成に限らず、目標の位置を三次元的に観測する構成であってもよいし、ドップラー効果などに基づいて目標の速度を観測する構成であってもよい。
【００９０】
センサ８１０が出力する信号には、ノイズなどの影響による不要信号が含まれている。このため、観測値取得部１１１が抽出する観測値は、必ずしも目標を観測したものではない可能性がある。逆に、目標が存在しても、観測された信号が弱い場合は、ノイズなどに紛れて、観測値取得部１１１が観測値として抽出しない可能性がある。
【００９１】
観測値記憶部１２１は、記憶装置９１４を用いて、観測値取得部１１１が生成した観測値データを蓄積して記憶する。センサ８１０は、連続して、もしくは繰り返し、観測を行うので、時間の経過に伴い、観測値記憶部１２１が記憶する観測値データは増えていく。観測値記憶部１２１は、センサ８１０が目標を観測した時刻がわかるように、観測値データを記憶する。例えば、観測値記憶部１２１は、処理装置９１１を用いて、観測値データが表わす観測値の観測時刻やフレーム番号を表わすデータを生成して観測値データに付加し、記憶装置９１４を用いて、生成したデータ付加した観測値データを記憶する。あるいは、観測値記憶部１２１は、処理装置９１１を用いて、観測値データが表わす観測値の観測時刻やフレーム番号ごとに観測値データを分類し、記憶装置９１４を用いて、分類した観測値データを記憶する。なお、観測値記憶部１２１は、観測時刻から所定時間以上経過した観測値データを破棄し、観測時刻からの経過時間が所定時間内である観測値データのみを保持する構成であってもよい。
【００９２】
追尾装置１００は、所定の運動モデルに基づいて目標を追尾する。例えば、目標の状態量として、時刻ｔにおける三次元空間内における位置を表わす位置ベクトルｐ（ｔ）、速度を表わす速度ベクトルｐドット（ｔ）、加速度を表わす加速度ベクトルｐツードット（ｔ）の三つの三次元ベクトルを考える場合、目標の状態量ベクトルｘ（ｔ）は、この三つの三次元ベクトルの成分を成分とする九次元ベクトルで表わすことができる。
【数２４】
ただし、ｐｘ（ｔ）は、時刻ｔにおける目標の位置の三次元直交座標系におけるＸ座標である。ｐｙ（ｔ）は、時刻ｔにおける目標の位置の三次元直交座標系におけるＹ座標である。ｐｚ（ｔ）は、時刻ｔにおける目標の位置の三次元直交座標系におけるＺ座標である。
ｖｘ（ｔ）は、時刻ｔにおける目標の速度の三次元直交座標系におけるＸ方向成分である。ｖｙ（ｔ）は、時刻ｔにおける目標の速度の三次元直交座標系におけるＹ方向成分である。ｖｚ（ｔ）は、時刻ｔにおける目標の速度の三次元直交座標系におけるＺ方向成分である。
ａｘ（ｔ）は、時刻ｔにおける目標の加速度の三次元直交座標系におけるＸ方向成分である。ａｙ（ｔ）は、時刻ｔにおける目標の加速度の三次元直交座標系におけるＹ方向成分である。ａｚ（ｔ）は、時刻ｔにおける目標の加速度の三次元直交座標系におけるＺ方向成分である。
【００９３】
目標が等加速度運動をしているという運動モデルを用いる場合、目標の航跡は、ある基準時刻ｔＰにおける状態量ベクトルｘ（ｔＰ）で表わすことができる。このとき、基準時刻ｔＰから時間Δｔが経過した時刻ｔ＝ｔＰ＋Δｔにおける目標の状態量ベクトルｘ（ｔＰ＋Δｔ）は、次の式で求めることができる。
【数２５】
ただし、Ｆ（Δｔ）は、時間Δｔの経過に伴う状態量ベクトルの変化を表わす遷移行列である。遷移行列Ｆ（Δｔ）は、例えば、次の式で表わされる。
【数２６】
【００９４】
例えば、センサ８１０が光学カメラである場合、時刻ｔにおける観測値を表わす観測値ベクトルｚ（ｔ）と、その時刻ｔにおける目標の状態量ベクトルｘ（ｔ）との間には、例えば、次のような関係が成り立つ。
【数２７】
ただし、ｐＸ及びｐＹは、目標が観測されたと観測値取得部１１１が判定した画素の二次元座標である。画素の座標は、センサ８１０が撮影した画像の中心を原点とする。また、状態量ベクトルにおける三次元座標系は、センサ８１０の撮影方向をＺ軸とし、センサ８１０が撮影した画像上の横方向をＸ軸、縦方向をＹ軸とする。また、ｖは、観測誤差を表わす二次元ベクトル（観測誤差ベクトル）である。ｈは、観測関数を表わす。観測関数ｈは、例えば、次の式で表わされる。
【数２８】
【００９５】
候補算出部１４１は、処理装置９１１を用いて、目標の航跡の候補を算出する。候補算出部１４１は、処理装置９１１を用いて、複数の候補を算出する。例えば、候補算出部１４１は、処理装置９１１を用いて、目標の航跡を表わす状態量ベクトルの九つの成分それぞれについて、１つ以上の候補を算出し、それを組み合わせることにより、複数の航跡の候補を算出する。
候補記憶部１４２は、記憶装置９１４を用いて、候補算出部１４１が算出した目標の航跡の候補を表わす航跡候補データを記憶する。
【００９６】
例えば、候補算出部１４１は、次のようにして、複数の航跡の候補を算出する。
【００９７】
候補算出部１４１は、観測値記憶部１２１が記憶した観測値データに基づいて、処理装置９１１を用いて、基準時刻ｔＰを決定する。例えば、候補算出部１４１は、観測値記憶部１２１が記憶した観測値データのなかから、観測時刻が最も新しい観測値データを抽出し、抽出した観測値データの観測時刻を基準時刻ｔＰとする。
【００９８】
候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰにおける目標の加速度ベクトルの推定値の候補として、一つの加速度ベクトルを設定する。目標の加速度ベクトルの推定値の候補を一つの加速度ベクトルに固定することにより、実際の目標の加速度ベクトルとの差がモデル化誤差となる。モデル化誤差の期待値は、設定した加速度ベクトルが、想定される実際の目標の加速度ベクトルの確率分布に基づく平均（期待値）に一致する場合に最小になる。想定される実際の目標の加速度ベクトルの確率分布について事前の情報がない場合、候補算出部１４１は、例えば、目標の加速度ベクトルの推定値の候補として、ゼロベクトルを設定する。あるいは、前回推定した航跡がある場合など、想定される実際の目標の加速度ベクトルの確率分布について事前の情報がある場合、候補算出部１４１は、目標の加速度ベクトルの推定値の候補として、例えば、事前の情報に基づいて加速度ベクトルの平均値（期待値）を算出する。例えば、候補算出部１４１は、前回推定した航跡を表わす状態量ベクトルから加速度ベクトルを取り出して、今回の目標の加速度ベクトルの推定値の候補とする。
【００９９】
候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰにおける目標の速度ベクトルの推定値の候補として、複数の速度ベクトルを設定する。例えば、候補算出部１４１は、想定される実際の目標の速度ベクトルの分布範囲のなかから、所定の数の速度ベクトルを、なるべく均等に分布するよう選択して、目標の速度ベクトルの推定値の候補とする。想定される実際の目標の速度ベクトルの分布範囲について事前の情報がない場合、候補算出部１４１は、目標の速度ベクトルの推定値の候補として、例えば、ゼロベクトルを中心とし、絶対値が所定の想定最大速度を超えない球状の範囲内から、所定の数の速度ベクトルを選択する。あるいは、前回推定した航跡がある場合など、想定される実際の目標の速度ベクトルの分布範囲について事前の情報がある場合、候補算出部１４１は、目標の速度ベクトルの推定値の候補として、例えば、事前の情報に基づいて目標の速度ベクトルの分布範囲を求め、その範囲内から、所定の数の速度ベクトルを選択する。
【０１００】
候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補として、複数の位置ベクトルを設定する。例えば、候補算出部１４１は、処理装置９１１を用いて、観測値記憶部１２１が記憶した観測値データのなかから、観測時刻と基準時刻ｔＰとの差が所定の閾値以内である観測値データを抽出する。閾値は、目標が存在しても観測されない欠測の可能性を考慮して、その時間の間に少なくとも１回は、目標が観測される時間に設定しておく。例えば、欠測の可能性が１０％ある場合、センサ８１０が少なくとも３回は観測を行う時間を閾値とすれば、その間に目標が１回も観測されない可能性は、０．１％以下である。
候補算出部１４１は、処理装置９１１を用いて、抽出した観測値データに基づいて、観測時刻ｔにおける目標の位置ベクトルの推定値の候補を算出する。例えば、候補算出部１４１は、処理装置９１１を用いて、次の式の右辺を計算することにより、観測時刻ｔにおける目標の位置ベクトルの推定値の候補を算出する。
【数２９】
ただし、ｐｅ（ｔ）は、候補算出部１４１が抽出した観測値データの観測時刻ｔにおける目標の位置ベクトルの推定値の候補である。ｐＸ及びｐＹは、候補算出部１４１が抽出した観測値データが表わす座標である。ｐｚｅは、目標の位置のＺ座標（センサ８１０から目標までの距離）の推定値の候補である。例えば、センサ８１０の観測可能範囲に基づいて、想定される目標の位置のＺ座標の範囲をあらかじめ定めておき、候補算出部１４１は、想定される範囲内から、複数のＺ座標の推定値の候補を選択する。候補算出部１４１は、選択したＺ座標の推定値の候補に基づいて、観測時刻ｔにおける目標の位置のＸ座標及びＹ座標の推定値の候補を算出する。候補算出部１４１は、目標の位置ベクトルの推定値の候補を、抽出した観測値データの数と選択したＺ座標の推定値の候補の数との積に等しい数だけ算出する。
【０１０１】
候補算出部１４１は、処理装置９１１を用いて、算出した目標の速度ベクトルの推定値の候補に基づいて、目標の位置ベクトルの推定値の候補の時刻合わせを行い、時刻ｔにおける目標の位置ベクトルの推定値の候補を、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補へ変換する。例えば、候補算出部１４１は、処理装置９１１を用いて、次の式の右辺を計算することにより、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補を算出する。
【数３０】
ただし、ｐｅ（ｔＰ）は、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補である。ｐｅ（ｔ）は、候補算出部１４１が抽出した観測値データの観測時刻ｔにおける目標の位置ベクトルの推定値の候補である。ｐドットｅ（ｔＰ）は、候補算出部１４１が算出した基準時刻ｔＰにおける目標の速度ベクトルの推定値の候補である。Δｔは、観測時刻ｔから基準時刻ｔＰを差し引いた差である。
【０１０２】
候補算出部１４１は、処理装置９１１を用いて、算出した基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補と、算出した基準時刻ｔＰにおける目標の速度ベクトルの候補と、算出した基準時刻ｔＰにおける目標の加速度ベクトルの候補とを組み合わせて、目標の航跡を表わす状態量ベクトルの推定値の候補を生成する。
【数３１】
ただし、ｘｅ（ｔＰ）は、基準時刻ｔＰにおける目標の状態量ベクトルの推定値の候補である。ｐツードットｅ（ｔＰ）は、候補算出部１４１が算出した基準時刻ｔＰにおける目標の加速度ベクトルの推定値の候補である。
候補算出部１４１は、状態量ベクトルの推定値の候補を、算出した目標の位置ベクトルの推定値の候補の数と、算出した目標の速度ベクトルの推定値の候補の数との積に等しい数だけ算出する。
【０１０３】
候補算出部１４１は、処理装置９１１を用いて、算出した目標の状態量ベクトルの推定値の候補のなかに重複するものがあれば統合する。例えば、候補算出部１４１は、処理装置９１１を用いて、算出した状態量ベクトルの推定値の候補の間のユークリッド距離を算出し、算出したユークリッド距離が所定の閾値以内である場合に、重複と判断する。候補算出部１４１は、処理装置９１１を用いて、重複と判定した２つの位置ベクトルの推定値の候補を平均して、１つに統合する。３つ以上の候補が重複している可能性がある場合、候補算出部１４１は、例えば、距離が最も近い２つの候補を統合し、統合後の新しい候補と、他の候補との間の距離を算出して、重複しているか否かを判定する。
【０１０４】
なお、目標が観測された観測時刻ｔと基準時刻ｔＰとの間の経過時間Δｔが短い場合、経過時間Δｔの間における目標の移動を考慮せず、観測時刻ｔにおける目標の位置ベクトルの推定値の候補を、そのまま、基準時刻ｔＰにおける目標の位置ベクトルの推定値の候補とする構成であってもよい。
【０１０５】
誤差上限値記憶部１５２は、記憶装置９１４を用いて、候補算出部１４１が算出した目標の状態量ベクトルの推定値の候補において候補を複数算出しなかった状態量について、その状態量の推定値の候補を固定したことによるモデル化誤差の上限値を記憶する。例えば、候補算出部１４１が加速度ベクトルの推定値の候補を固定した場合、誤差上限値記憶部１５２は、実際の加速度ベクトルと、候補算出部１４１が固定した加速度ベクトルの推定値の候補との間の差をモデル化誤差とし、その上限値を記憶する。誤差上限値記憶部１５２が記憶するモデル化誤差の上限値は、誤差上限初期化部１５１や誤差上限値更新部１７４が算出する。例えば、誤差上限値記憶部１５２は、次の式で表わされる最大加速度誤差ベクトルを、モデル化誤差上限値ベクトルとして記憶する。
【数３２】
ただし、Δｐツードットｍａｘは、最大加速度誤差ベクトルである。Δａｘｍａｘは、最大加速度誤差ベクトルのＸ方向成分である。Δａｙｍａｘは、最大加速度誤差ベクトルのＹ方向成分である。Δａｚｍａｘは、最大加速度誤差ベクトルのＺ方向成分である。
【０１０６】
誤差上限初期化部１５１は、処理装置９１１を用いて、誤差上限値記憶部１５２が記憶するモデル化誤差の上限値を初期化する。例えば、想定される目標の加速度ベクトルの確率分布について事前の情報がなく、候補算出部１４１が目標の加速度ベクトルの推定値の候補としてゼロベクトルを設定した場合、誤差上限初期化部１５１は、目標の想定最大加速度を、モデル化誤差の上限値とする。あるいは、例えば、想定される目標の加速度ベクトルの確率分布について事前の情報があり、候補算出部１４１がその期待値を目標の加速度ベクトルの推定値の候補として設定した場合、誤差上限初期化部１５１は、想定される目標の加速度ベクトルの確率分布に基づいて、モデル化誤差の上限値を算出する。誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限初期化部１５１が算出したモデル化誤差の上限値を、モデル化誤差の上限値の初期値として記憶する。
【０１０７】
ゲート行列算出部１５３は、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値に基づいて、ゲート行列を算出する。ゲート行列は、観測時刻ごとに異なるので、ゲート行列算出部１５３は、相関処理の対象となる観測値データの観測時刻ごとに、ゲート行列を算出する。例えば、ゲート行列算出部１５３は、処理装置９１１を用いて、観測値記憶部１２１が記憶した観測値データのなかから、相関処理の対象となる観測値データを抽出する。ゲート行列算出部１５３は、抽出した観測値データの観測時刻ｔに基づいて、ゲート行列を算出する。
例えば、ゲート行列算出部１５３は、処理装置９１１を用いて、次の式の右辺を計算することにより、モデル化誤差共分散行列を算出する。
【数３３】
ただし、Ｑ（ｔＰ＋Δｔ）は、基準時刻ｔＰから時間Δｔが経過した時刻におけるモデル化誤差共分散行列である。ｄｉａｇは、ｎ次元ベクトルのｉ番目の成分をｉ行ｉ列成分とするｎ次の対角行列を表わす。
ゲート行列算出部１５３は、算出したモデル化誤差共分散行列に基づいて、処理装置９１１を用いて、次の式の右辺を計算することにより、時刻ｔにおけるゲート行列を算出する。
【０１０８】
Ｓ（ｔ）＝Ｒ（ｔ）＋Ｑ（ｔ）
ただし、Ｓ（ｔ）は、ゲート行列算出部１５３が抽出した観測値データの観測時刻ｔにおけるゲート行列である。Ｒ（ｔ）は、その観測時刻ｔにおける観測値データの観測誤差共分散行列である。Ｑ（ｔ）は、その観測時刻ｔについてゲート行列算出部１５３が算出したモデル化誤差共分散行列である。Ｓ（ｔ）、Ｒ（ｔ）、Ｑ（ｔ）は、いずれも、観測値ベクトルｚ（ｔ）と同じ次数の正方行列である。
【０１０９】
相関部１６１は、候補記憶部１４２が記憶した目標の航跡の候補それぞれについて、処理装置９１１を用いて、ゲート行列算出部１５３が算出したゲート行列に基づいて、観測値記憶部１２１が記憶した観測値データのなかから、その航跡の目標を観測した観測値であると考えられる観測値を表わす観測値データを抽出する。
例えば、相関部１６１は、次のようにして、観測値データを抽出する。
【０１１０】
相関部１６１は、候補記憶部１４２が記憶した目標の航跡の候補それぞれについて、処理装置９１１を用いて、観測値記憶部１２１が記憶した観測値データのうち相関処理の対象となる観測値データの観測時刻ｔに基づいて、時刻ｔにおける目標の状態量ベクトルの推定値を算出する。相関部１６１は、例えば、次の式の右辺を計算することにより、時刻ｔにおける目標の状態量ベクトルの推定値を算出する。
【０１１１】
ｘｅ（ｔ）＝Ｆ（Δｔ）ｘｅ（ｔＰ）
ただし、ｘｅ（ｔ）は、基準時刻ｔＰから時間Δｔが経過した時刻ｔ＝ｔＰ＋Δｔにおける目標の状態量ベクトルの推定値である。ｘｅ（ｔＰ）は、候補記憶部１４２が記憶した目標の航跡の候補である基準時刻ｔＰにおける目標の状態量ベクトルの推定値の候補である。
【０１１２】
相関部１６１は、算出した時刻ｔにおける目標の状態量ベクトルの推定値と、観測値データが表わす時刻ｔにおける観測値ベクトルとに基づいて、処理装置９１１を用いて、その観測値データの観測誤差の推定値を算出する。相関部１６１は、例えば、次の式の右辺を計算することにより、その観測値データの観測誤差の推定値を算出する。
【０１１３】
Δｚ＝ｚ−ｈ（ｘｅ（ｔ））
ただし、Δｚは、その観測値データの観測誤差の推定値である。ｚは、観測値データが表わす観測値ベクトルである。ｔは、その観測値データの観測時刻である。ｈは、観測関数である。
【０１１４】
相関部１６１は、算出した観測誤差の推定値に基づいて、処理装置９１１を用いて、その観測値データが、その航跡の目標を観測した観測値を表わす観測値データであるか否かを判定する。例えば、相関部１６１は、ゲート行列算出部１５３が算出したゲート行列に基づいて、次の式が表わす条件が満たされる場合に、その観測値データが、その航跡の目標を観測した観測値を表わす観測値データであると判断する。
【０１１５】
ΔｚＴＳ（ｔ）−１Δｚ≦ｄ
ただし、上付きのＴは、転置行列を表わす。上付きの−１は、逆行列を表わす。ｄは、所定の閾値である。
【０１１６】
尤度算出部１６２は、候補記憶部１４２が記憶した目標の軌跡の候補それぞれについて、処理装置９１１を用いて、ゲート行列算出部１５３が算出したゲート行列と、相関部１６１が抽出した観測値データとに基づいて、その目標の軌跡の候補の尤度を算出する。例えば、尤度算出部１６２は、次の式の右辺を計算することにより、その軌跡の候補の尤度を算出する。
【数３４】
ただし、φは、その軌跡の候補の尤度である。Ｔは、相関処理の対象となる観測値データの観測時刻を要素とする集合である。ｌｏｇｅは、ネイピア数を底とする対数（自然対数）関数を表わす。ＰＤは、探知確率パラメータである。探知確率パラメータＰＤは、０より大きく１より小さい実数であり、センサ８１０の観測範囲内に目標が存在するとき、その目標を観測した観測値を観測値取得部１１１が抽出する確率を表わす。βは、不要信号密度パラメータである。不要信号密度パラメータβは、０より大きい実数であり、ノイズなどの影響による不要信号が観測値空間の単位体積あたりに存在する平均個数を表わす。探知確率パラメータＰＤや不要信号密度パラメータβは、あらかじめ設定された値を尤度算出部１６２が記憶装置９１４を用いて記憶しておく。なお、尤度算出部１６２は、追尾結果などに基づいて、探知確率パラメータＰＤや不要信号密度パラメータβを更新する構成であってもよい。
ｄｅｔは、行列式を表わす。πは、円周率である。Ｓ（ｔ）は、ゲート行列算出部１５３が算出した時刻ｔにおけるゲート行列である。Ｇ（ｔ）は、その航跡の候補について相関部１６１が抽出した観測値データのうち、観測時刻が時刻ｔである観測値データが表わす観測値ベクトルを要素とする集合である。ｅｘｐは、ネイピア数を底とする指数関数を表わす。Δｚは、観測値ベクトルｚについて相関部１６１が算出した観測誤差の推定値である。上付きのＴは、転置行列を表わす。上付きの−１は、逆行列を表わす。
【０１１７】
選択部１６３は、尤度算出部１６２が算出した尤度に基づいて、処理装置９１１を用いて、候補記憶部１４２が記憶した航跡の候補のなかから、正しい航跡に近い航跡である可能性の高い候補を抽出する。選択部１６３は、航跡の候補を１つだけ抽出する構成であってもよいし、複数の候補を抽出する構成であってもよい。
例えば、選択部１６３は、処理装置９１１を用いて、候補記憶部１４２が記憶した航跡の候補のなかから、尤度算出部１６２が算出した尤度が大きい順に、所定の数の候補を抽出する。あるいは、例えば、選択部１６３は、処理装置９１１を用いて、尤度算出部１６２が算出した尤度のなかから、一番大きな尤度を抽出し、候補記憶部１４２が記憶した航跡の候補それぞれについて、尤度算出部１６２が算出した尤度を、抽出した最大尤度で割った商を算出する。選択部１６３は、処理装置９１１を用いて、算出した商が所定の閾値より大きい候補を抽出する。
【０１１８】
推定値記憶部１７１は、記憶装置９１４を用いて、選択部１６３が抽出した航跡の候補を表わす状態量ベクトルの推定値を記憶する。
【０１１９】
推定値更新部１７２は、処理装置９１１を用いて、推定値記憶部１７１が記憶した状態量ベクトルの推定値を出発点として、もっと尤度が大きい航跡を表わす状態量ベクトルの推定値を算出する。推定値更新部１７２は、算出した状態量ベクトルの推定値で、推定値記憶部１７１が記憶した状態量ベクトルの推定値を更新する。推定値記憶部１７１は、記憶装置９１４を用いて、更新された状態量ベクトルの推定値を記憶する。
【０１２０】
例えば、推定値更新部１７２は、推定値記憶部１７１が記憶した状態量ベクトルの推定値をわずかに変化させる。推定値更新部１７２が変化させた状態量ベクトルの推定値に基づいて、相関部１６１が相関処理をし、尤度算出部１６２が尤度を算出する。推定値更新部１７２は、尤度算出部１６２が算出した尤度に基づいて、推定値記憶部１７１が記憶した状態量ベクトルの推定値の近傍における尤度の変化傾向を把握する。推定値更新部１７２は、把握した変化傾向に基づいて、状態量ベクトルの推定値をどの方向にどのくらい変化させたら、もっと尤度が大きい航跡を表わす状態量ベクトルの推定値となるかを算出する。
具体的には、例えば次のようにして、もっと尤度が大きい航跡を表わす状態量ベクトルの推定値を算出する。
【０１２１】
推定値更新部１７２は、処理装置９１１を用いて、推定値記憶部１７１が記憶した状態量ベクトルの推定値それぞれについて、状態量ベクトルの各成分を微かに変化させた状態量ベクトルを算出する。推定値更新部１７２は、例えば、状態量ベクトルのある成分に、所定の微小増分を加えた状態量ベクトルと、その成分から微小増分を差し引いた状態量ベクトルとを算出する。例えば、状態量ベクトルが九次元ベクトルである場合、推定値更新部１７２は、各成分について２つの状態量ベクトルを算出するので、全部で１８個の状態量ベクトルを算出する。
相関部１６１（詳細相関部）は、処理装置９１１を用いて、推定値更新部１７２が算出した１８個の状態量ベクトルと、その元となった１つの状態量ベクトルとを合わせた合計１９個の状態量ベクトルそれぞれについて、その状態量ベクトルが表わす航跡を、候補記憶部１４２が記憶した航跡の候補の代わりとして、観測値記憶部１２１が記憶した観測値データのなかから、観測値データを抽出する。尤度算出部１６２（詳細尤度算出部）は、処理装置９１１を用いて、相関部１６１と同様、１９個の状態量ベクトルそれぞれについて、その状態量ベクトルが表わす航跡を、候補記憶部１４２が記憶した航跡の候補の代わりとして、その航跡の尤度を算出する。
推定値更新部１７２は、処理装置９１１を用いて、尤度算出部１６２が算出した尤度に基づいて、状態量ベクトルの各成分について、尤度が最大になると予想される値を算出する。
例えば、推定値更新部１７２は、状態量ベクトルの一つの成分に注目し、注目した成分以外の成分の値は変えずに、その成分の値だけを変えて算出した尤度を比較する。推定値更新部１７２は、その成分のもとの値ｕを使った状態量ベクトルについて算出した尤度φ（ｕ）と、微小増分Δｕを加えた値を使った状態量ベクトルについて算出した尤度φ（ｕ＋Δｕ）と、微小増分Δｕを差し引いた値を使った状態量ベクトルについて算出した尤度φ（ｕ−Δｕ）との間の大小関係を判定する。
φ（ｕ−Δｕ）≧φ（ｕ）≦φ（ｕ＋Δｕ）である場合、推定値更新部１７２は、もとの値ｕを、その成分の更新した値とする。
φ（ｕ−Δｕ）＜φ（ｕ）＞φ（ｕ＋Δｕ）である場合も、推定値更新部１７２は、もとの値ｕを、その成分の更新した値とする。
φ（ｕ−Δｕ）＜φ（ｕ）≦φ（ｕ＋Δｕ）である場合、推定値更新部１７２は、尤度φ（ｕ）から尤度φ（ｕ−Δｕ）を差し引いた差Δφ−＝φ（ｕ）−φ（ｕ−Δｕ）と、尤度φ（ｕ＋Δｕ）から尤度φ（ｕ）を差し引いた差Δφ＋＝φ（ｕ＋Δｕ）−φ（ｕ）とを算出する。推定値更新部１７２は、算出した２つの差の大小関係を比較する。
Δφ＋≧Δφ−である場合、推定値更新部１７２は、０超所定の値未満の乱数ｒを生成し、生成した乱数ｒをもとの値ｕに加えた和ｕ＋ｒを、その成分の更新した値とする。
Δφ＋＜Δφ−である場合、推定値更新部１７２は、差Δφ＋と差Δφ−との平均値Δφ＝（Δφ＋＋Δφ−）／２と、差Δφ−から差Δφ＋を差し引いた差Δ２φ＝Δφ−−Δφ＋とを算出する。推定値更新部１７２は、算出した平均値Δφを、算出した差Δ２φで割った商を微小増分Δｕに乗じた積をもとの値ｕに加えた和ｕ＋Δφ／Δ２φ・Δｕを、その成分の更新した値とする。
φ（ｕ−Δｕ）≧φ（ｕ）＞φ（ｕ＋Δｕ）である場合、推定値更新部１７２は、尤度φ（ｕ）から尤度φ（ｕ＋Δｕ）を差し引いた差Δφ＋＝φ（ｕ）−φ（ｕ＋Δｕ）と、尤度φ（ｕ−Δｕ）から尤度φ（ｕ）を差し引いた差Δφ−＝φ（ｕ−Δｕ）−φ（ｕ）とを算出する。推定値更新部１７２は、算出した２つの差の大小関係を比較する。
Δφ＋≦Δφ−である場合、推定値更新部１７２は、０超所定の値未満の乱数ｒを生成し、生成した乱数ｒをもとの値ｕから差し引いた差ｕ−ｒを、その成分の更新した値とする。
Δφ＋＞Δφ−である場合、推定値更新部１７２は、差Δφ＋と差Δφ−との平均値Δφ＝（Δφ＋＋Δφ−）／２と、差Δφ＋から差Δφ−を差し引いた差Δ２φ＝Δφ＋−Δφ−とを算出する。推定値更新部１７２は、算出した平均値Δφを、算出した差Δ２φで割った商を微小増分Δｕに乗じた積をもとの値ｕから差し引いた差ｕ−Δφ／Δ２φ・Δｕを、その成分の更新した値とする。
推定値更新部１７２は、例えばこのようにして状態量ベクトルのすべての成分について更新した値を算出することにより、更新した状態量ベクトルの推定値を算出する。
推定値記憶部１７１は、記憶装置９１４を用いて、推定値更新部１７２が更新した状態量ベクトルの推定値を記憶する。
【０１２２】
収束判定部１７３は、処理装置９１１を用いて、状態量ベクトルの推定値が収束したか否かを判定する。例えば、収束判定部１７３は、推定値更新部１７２が更新した更新後の状態量ベクトルの推定値と、推定値更新部１７２が更新する前に推定値記憶部１７１が記憶していた更新前の状態量ベクトルの推定値との差ベクトルを算出する。収束判定部１７３は、算出した差ベクトルの絶対値を所定の閾値と比較する。差ベクトルの絶対値が閾値より小さい場合、収束判定部１７３は、状態量ベクトルの推定値が収束したと判定する。
【０１２３】
誤差上限値更新部１７４は、状態量ベクトルの推定値が収束したと収束判定部１７３が判定した場合、処理装置９１１を用いて、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値を更新する。例えば、誤差上限値更新部１７４は、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値に、０超１未満の所定の係数を乗じた積を算出して、更新したモデル化誤差の上限値とする。
誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限値更新部１７４が更新したモデル化誤差の上限値を記憶する。
【０１２４】
終了判定部１７５は、状態量ベクトルの推定値が収束したと収束判定部１７３が判定した場合、処理装置９１１を用いて、状態量ベクトルの推定を終了するか否かを判定する。例えば、終了判定部１７５は、状態量ベクトルの推定値が収束したと収束判定部１７３が判定したとき推定値記憶部１７１が記憶している状態量ベクトルの推定値を記憶する。状態量ベクトルの推定値が収束したと収束判定部１７３が判定したのが一回目である場合、終了判定部１７５は、状態量ベクトルの推定を終了しないと判定する。状態量ベクトルの推定値が収束したと収束判定部１７３が判定したのが二回目もしくはそれ以降である場合、終了判定部１７５は、記憶している前回の状態量ベクトルの推定値と、そのとき推定値記憶部１７１が記憶している状態量ベクトルの推定値との差ベクトルを算出する。終了判定部１７５は、算出した差ベクトルの絶対値を所定の閾値と比較する。差ベクトルの絶対値が閾値より小さい場合、終了判定部１７５は、状態量ベクトルの推定を終了すると判定する。
【０１２５】
推定値出力部１７６は、状態量ベクトルの推定を終了すると終了判定部１７５が判定した場合、出力装置９１３を用いて、推定値記憶部１７１が記憶した状態量ベクトルの推定値を出力する。なお、推定値記憶部１７１が記憶した状態量ベクトルの推定値のなかに、ほぼ同一の推定値が複数ある場合、推定値出力部１７６は、そのなかで最も尤度が大きい推定値だけを出力する構成であってもよい。
【０１２６】
図１２は、この実施の形態における追尾処理Ｓ６００の流れの一例を示すフローチャート図である。
追尾処理Ｓ６００（推定方法、航跡推定方法）は、例えば、観測値取得工程Ｓ６６１と、候補算出工程Ｓ６６２と、誤差上限初期化工程Ｓ６６３と、２つのゲート行列算出工程Ｓ６６４，Ｓ６８３と、候補選択工程Ｓ６６５と、２つの相関工程Ｓ６６６，Ｓ６７３と、２つの尤度算出工程Ｓ６６７，Ｓ６７４と、選択工程Ｓ６６８と、推定値選択工程Ｓ６７１と、参照生成工程Ｓ６７２と、推定値更新工程Ｓ６７５と、収束判定工程Ｓ６７６と、個別終了判定工程Ｓ６７７と、全体終了判定工程Ｓ６８１と、誤差上限値更新工程Ｓ６８２と、推定値出力工程Ｓ６８４とを有する。
追尾装置１００は、観測値取得工程Ｓ６６１から追尾処理Ｓ６００を開始する。
【０１２７】
観測値取得工程Ｓ６６１において、観測値取得部１１１は、入力装置９１２を用いて、センサ８１０が出力した信号を入力する。観測値取得部１１１は、処理装置９１１を用いて、入力した信号に基づいて、観測値を取得する。観測値記憶部１２１は、記憶装置９１４を用いて、観測値取得部１１１が取得した観測値を記憶する。
【０１２８】
候補算出工程Ｓ６６２において、候補算出部１４１は、処理装置９１１を用いて、基準時刻ｔＰを決定する。候補算出部１４１は、処理装置９１１を用いて、決定した基準時刻ｔＰにおける目標の状態量の推定値の候補を複数生成する。候補記憶部１４２は、記憶装置９１４を用いて、候補算出部１４１が生成した複数の候補を記憶する。
【０１２９】
誤差上限初期化工程Ｓ６６３において、誤差上限初期化部１５１は、処理装置９１１を用いて、モデル化誤差の上限値の初期値を設定する。誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限初期化部１５１が設定した初期値を、モデル化誤差の上限値として記憶する。
【０１３０】
ゲート行列算出工程Ｓ６６４において、ゲート行列算出部１５３は、処理装置９１１を用いて、誤差上限初期化工程Ｓ６６３で誤差上限値記憶部１５２が記憶したモデル化誤差の上限値と、候補算出工程Ｓ６６２で候補算出部１４１が決定した基準時刻ｔＰとに基づいて、複数の観測時刻ｔそれぞれにおけるゲート行列Ｓ（ｔ）を算出する。
【０１３１】
候補選択工程Ｓ６６５において、選択部１６３は、処理装置９１１を用いて、候補記憶部１４２が記憶した複数の候補のなかから、まだ尤度を算出していない候補を一つ選択する。
候補記憶部１４２が記憶したすべての候補について尤度を算出済であり、選択すべき候補がない場合、選択部１６３は、処理装置９１１を用いて、選択工程Ｓ６６８へ処理を進める。
候補記憶部１４２が記憶した候補のなかに、まだ尤度を算出していない候補がある場合、選択部１６３は、処理装置９１１を用いて、まだ尤度を算出していない候補のなかから、候補を一つ選択する。選択部１６３は、処理装置９１１を用いて、相関工程Ｓ６６６へ処理を進める。
【０１３２】
相関工程Ｓ６６６において、相関部１６１は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、候補選択工程Ｓ６６５で選択部１６３が選択した候補とに基づいて、観測値記憶部１２１が記憶した観測値のなかから、その候補が表わす航跡の目標を観測したと考えられる観測値を抽出する。
【０１３３】
尤度算出工程Ｓ６６７において、尤度算出部１６２は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、候補選択工程Ｓ６６５で選択部１６３が選択した候補と、相関工程Ｓ６６６で相関部１６１が抽出した観測値とに基づいて、その候補の尤度を算出する。選択部１６３は、記憶装置９１４を用いて、尤度算出部１６２が算出した尤度を記憶する。選択部１６３は、処理装置９１１を用いて、候補選択工程Ｓ６６５に処理を戻し、次の候補を選択する。
【０１３４】
選択工程Ｓ６６８において、選択部１６３は、処理装置９１１を用いて、尤度算出工程Ｓ６６７で記憶した尤度に基づいて、候補記憶部１４２が記憶した候補のなかから、正しい航跡に近い航跡を表わす候補を抽出する。推定値記憶部１７１は、記憶装置９１４を用いて、選択部１６３が抽出した候補（基準時刻ｔＰにおける状態量の推定値）を記憶する。
【０１３５】
推定値選択工程Ｓ６７１において、推定値更新部１７２は、処理装置９１１を用いて、選択工程Ｓ６６８で推定値記憶部１７１が記憶した推定値のうち、まだ推定が終了していない推定値のなかから、まだ更新をしていない推定値を一つ選択する。
推定値記憶部１７１が記憶した推定値すべてについての更新が終わり、選択すべき推定値がない場合、推定値更新部１７２は、処理装置９１１を用いて、全体終了判定工程Ｓ６８１へ処理を進める。
推定値記憶部１７１が記憶した推定値のなかに、まだ更新をしていない推定値がある場合、推定値更新部１７２は、処理装置９１１を用いて、まだ更新をしていない推定値のなかから、推定値を一つ選択する。推定値更新部１７２は、処理装置９１１を用いて、参照生成工程Ｓ６７２へ処理を進める。
【０１３６】
参照生成工程Ｓ６７２において、推定値更新部１７２は、処理装置９１１を用いて、所定の複数の方向のなかから、推定値を変化させる方向を選択する。
すべての方向についての処理が終わり、選択すべき方向がない場合、推定値更新部１７２は、処理装置９１１を用いて、推定値更新工程Ｓ６７５へ処理を進める。
所定の複数の方向のなかに、まだ処理していない方向がある場合、推定値更新部１７２は、処理装置９１１を用いて、まだ処理していない方向のなかから、方向を一つ選択する。推定値更新部１７２は、処理装置９１１を用いて、選択した方向へ向けて、推定値選択工程Ｓ６７１で選択した推定値をわずかに変化させる。推定値更新部１７２は、処理装置９１１を用いて、相関工程Ｓ６７３へ処理を進める。
【０１３７】
相関工程Ｓ６７３において、相関部１６１（詳細相関部）は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４またはＳ６８３でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、参照生成工程Ｓ６７２で推定値更新部１７２が変化させた推定値とに基づいて、観測値記憶部１２１が記憶した観測値のなかから、その推定値が表わす航跡の目標を観測したと考えられる観測値を抽出する。
【０１３８】
尤度算出工程Ｓ６７４において、尤度算出部１６２（詳細尤度算出部）は、処理装置９１１を用いて、ゲート行列算出工程Ｓ６６４またはＳ６８３でゲート行列算出部１５３が算出したゲート行列Ｓ（ｔ）と、推定値更新部１７２で推定値更新部１７２が変化させた推定値と、相関工程Ｓ６７３で相関部１６１が抽出した観測値とに基づいて、その推定値の尤度を算出する。推定値更新部１７２は、記憶装置９１４を用いて、尤度算出部１６２が算出した尤度を記憶する。推定値更新部１７２は、処理装置９１１を用いて、参照生成工程Ｓ６７２に処理を戻し、次の方向を選択する。
【０１３９】
推定値更新工程Ｓ６７５において、推定値更新部１７２は、処理装置９１１を用いて、尤度算出工程Ｓ６７４で記憶した尤度に基づいて、更新前よりも尤度が大きくなる推定値を算出する。推定値記憶部１７１は、処理装置９１１を用いて、推定値選択工程Ｓ６７１で推定値更新部１７２が選択した推定値を、推定値更新部１７２が算出した推定値で更新し、記憶装置９１４を用いて、更新した推定値を記憶する。
【０１４０】
収束判定工程Ｓ６７６において、収束判定部１７３は、処理装置９１１を用いて、推定値選択工程Ｓ６７１で推定値更新部１７２が選択した推定値が収束したか否かを判定する。
収束したと判定した場合、収束判定部１７３は、処理装置９１１を用いて、個別終了判定工程Ｓ６７７へ処理を進める。
まだ収束していないと判定した場合、推定値更新部１７２は、処理装置９１１を用いて、参照生成工程Ｓ６７２に処理を戻し、更に尤度が大きくなる推定値を算出する。
【０１４１】
個別終了判定工程Ｓ６７７において、終了判定部１７５は、処理装置９１１を用いて、推定値選択工程Ｓ６７１で推定値更新部１７２が選択した推定値について、状態量の推定を終了するか否かを判定する。終了判定部１７５は、記憶装置９１４を用いて、判定した判定結果を記憶する。
推定値更新部１７２は、処理装置９１１を用いて、推定値選択工程Ｓ６７１に処理を戻し、次の推定値を選択する。
【０１４２】
全体終了判定工程Ｓ６８１において、終了判定部１７５は、処理装置９１１を用いて、個別終了判定工程Ｓ６７７で記憶した判定結果に基づいて、推定値記憶部１７１が記憶したすべての推定値について、状態量の推定を終了すると判定したか否かを判定する。
すべての推定値について状態量の推定を終了すると判定し、状態量の推定を終了しないと判定した推定値がない場合、終了判定部１７５は、処理装置９１１を用いて、推定値出力工程Ｓ６８４へ処理を進める。
状態量の推定を終了しないと判定した推定値がまだある場合、終了判定部１７５は、処理装置９１１を用いて、誤差上限値更新工程Ｓ６８２へ処理を進める。
【０１４３】
誤差上限値更新工程Ｓ６８２において、誤差上限値更新部１７４は、処理装置９１１を用いて、誤差上限値記憶部１５２が記憶したモデル化誤差の上限値を更新する。誤差上限値記憶部１５２は、記憶装置９１４を用いて、誤差上限値更新部１７４が更新したモデル化誤差の上限値を記憶する。
【０１４４】
ゲート行列算出工程Ｓ６８３において、ゲート行列算出部１５３は、処理装置９１１を用いて、誤差上限値更新工程Ｓ６８２で誤差上限値記憶部１５２が記憶した更新後のモデル化誤差の上限値に基づいて、観測時刻ｔそれぞれにおけるゲート行列Ｓ（ｔ）を算出する。
推定値更新部１７２は、推定値選択工程Ｓ６７１に処理を戻す。推定値選択工程Ｓ６７１において、推定値更新部１７２は、推定値記憶部１７１が記憶した推定値のうち、個別終了判定工程Ｓ６７７で状態量の推定を終了しないと終了判定部１７５が判定した推定値について、また最初から推定値を一つずつ選択する。
【０１４５】
推定値出力工程Ｓ６８４において、推定値出力部１７６は、出力装置９１３を用いて、推定値記憶部１７１が記憶した推定値を出力する。
【０１４６】
以上、各実施の形態で説明した具体的な構成は一例であり、これに限定されるものではない。例えば、重要でない部分の構成を他の構成で置き換えた構成としてもよし、異なる実施の形態で説明した構成を組み合わせた構成としてもよい。
【０１４７】
推定装置（追尾装置１００）は、データを処理する処理装置（９１１）と、候補算出部（１４１；航跡候補生成部１４０）と、相関部（１６１；概探索部１６０）と、尤度算出部（１６２；概探索部１６０）と、選択部（１６３；概探索部１６０）と、推定値更新部（１７２；詳細探索部１７０）と、推定値出力部（１７６；詳細探索部１７０）とを有する。
上記候補算出部は、目標の複数の状態量（例えば位置・速度・加速度）のうち所定の第一の状態量（例えば加速度）を除く他の状態量（例えば位置・速度）について、上記処理装置を用いて、上記他の状態量の推定値の複数の候補値を算出する。
上記相関部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出する。
上記尤度算出部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記相関部が仮定したモデル化誤差上限値と、上記相関部が抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出する。
上記候補選択部は、上記処理装置を用いて、上記尤度算出部が算出した尤度に基づいて、上記候補算出部が算出した複数の候補値のなかから、上記他の状態量の推定値を選択する。
上記推定値更新部は、上記処理装置を用いて、上記相関部が仮定した第一の状態量の推定値と上記候補選択部が選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新する。
上記推定値出力部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力する。
【０１４８】
第一の状態量が所定の推定値であると仮定して、状態量の推定値の候補値を算出するので、状態量の推定値の組み合わせの数が減り、相関部や尤度算出部の処理を減らすことができる。これにより、目標の航跡の推定に必要な計算量が減るので、処理能力の低い処理装置を用いてもリアルタイムでの処理が可能になるなど、追尾装置１００のコストを下げることができる。
【０１４９】
上記推定装置（１００）は、収束判定部（１７３；詳細探索部１７０）と、誤差上限値更新部（１７４；詳細探索部１７０）と、詳細相関部（相関部１６１；詳細探索部１７０）と、詳細尤度算出部（尤度算出部１６２；詳細探索部１７０）とを有する。
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定する。
上記誤差上限値更新部は、上記推定値更新部が上記複数の状態量の推定量を更新した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくする。
上記詳細相関部は、上記処理装置を用いて、上記複数の状態量が、上記推定値更新部が更新した推定値であり、上記モデル化誤差が、上記誤差上限値更新部が小さくしたモデル化誤差上限値以下であるとの仮定に基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出する。
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出する。
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新する。
上記推定値出力部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力する。
【０１５０】
状態量の推定値が収束するまで更新処理を繰り返すので、航跡の推定精度を高くすることができる。
【０１５１】
上記推定装置（１００）は、収束判定部（１７３；詳細探索部１７０）と、終了判定部（１７５；詳細探索部１７０）と、誤差上限値更新部（１７４；詳細探索部１７０）と、詳細相関部（相関部１６１；詳細探索部１７０）と、詳細尤度算出部（尤度算出部１６２；詳細探索部１７０）とを有する。
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定する。
上記終了判定部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記複数の状態量の推定を終了するか否かを判定する。
上記誤差上限値更新部は、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくする。
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出する。
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出する。
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合、および、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新する。
上記推定値出力部は、上記複数の状態量の推定を終了すると上記終了判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力する。
【０１５２】
状態量の推定値が収束するまで、モデル化誤差上限値を変えずに更新処理を繰り返し、状態量の推定値が収束した場合に、モデル化誤差上限値を小さくして、更に、状態量の推定値が収束するまで更新処理を繰り返すので、状態量の推定精度を高くすることができる。
【０１５３】
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値が上記目標を観測した観測値であるかと判定する範囲を算出するために使用するゲート行列を算出する。
上記終了判定部は、上記処理装置を用いて、上記詳細相関部が算出したゲート行列に基づいて、上記複数の状態量の推定を終了するか否かを判定する。
【０１５４】
ゲート行列に基づいて状態量の推定を終了するか否かを判定するので、精度よく終了判定をすることができる。
【０１５５】
上記終了判定部は、上記複数の状態量の推定を終了するか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記複数の状態量の推定を終了すると判定する。
【０１５６】
判定回数に基づいて状態量の推定を終了するか否かを判定するので、終了判定に必要な計算量を少なくすることができる。
【０１５７】
上記誤差上限値更新部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差上限値を更新する。
ａｉｍａｘ←√［（ａｉｍａｘ）２−Δａｉ２］
ただし、ａｉｍａｘは、上記モデル化誤差上限値を表わすベクトルのｉ番目の成分。Δａｉは、上記推定値更新部が更新した後の第一の状態量の推定値を表わすベクトルのｉ番目の成分と、上記推定値更新部が更新する前の第一の状態量の推定値を表わすベクトルのｉ番目の成分との差。
【０１５８】
これにより、モデル化誤差上限値を最適に更新することができるので、全体としての繰り返し回数が減り、目標の航跡の推定に必要な計算量を少なくすることができる。
【０１５９】
上記誤差上限値更新部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差上限値を更新する。
ａｉｍａｘ←λ・ａｉｍａｘ
ただし、λは、モデル化誤差上限値の逓減率であり、０超１未満の実数。
【０１６０】
これにより、モデル化誤差上限値の更新に必要な計算量を少なくすることができるので、目標の航跡の推定に必要な計算量を少なくすることができる。
【０１６１】
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、ゲート行列を算出し、算出したゲート行列を使用して、上記複数の観測値が上記目標を観測した観測値であると判定する範囲を算出し、上記複数の観測値のうち、算出した範囲内に入る観測値を、上記目標を観測した観測値であると判定する。
Ｓ←Ｒ＋Ｑ
ただし、Ｓは、ゲート行列であり、上記観測値を表わすベクトルの成分の数を次数とする正方行列。Ｒは、観測雑音共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記観測値を表わすベクトルの各成分の観測精度の二乗を対角成分とする対角行列。Ｑは、モデル化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記モデル化誤差上限値を表わすベクトルの各成分から算出される値を対角成分とする対角行列。
【０１６２】
観測誤差とモデル化誤差とを考慮したゲート行列を使って、目標を観測した観測値を判定するので、モデル化誤差が大きい場合でも、目標を観測した観測値を精度よく判定することができる。
【０１６３】
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、ゲート行列を算出し、算出したゲート行列を使用して、上記複数の観測値が上記目標を観測した観測値であると判定する範囲を算出し、上記複数の観測値のうち、算出した範囲内に入る観測値を、上記目標を観測した観測値であると判定する。
Ｓ←Ｒ＋Ｑ＋Ｑ’
ただし、Ｑ’は、量子化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とする対称行列。
【０１６４】
観測誤差とモデル化誤差と量子化誤差とを考慮したゲート行列を使って、目標を観測した観測値を判定するので、量子化誤差が大きい場合でも、目標を観測した観測値を精度よく判定することができる。
【０１６５】
上記推定装置は、複数の時刻における上記目標の位置を、上記目標を観測した観測値として入力し、所定の基準時刻における上記目標の位置及び速度及び加速度を、上記複数の状態量として推定する。
上記相関部は、上記処理装置を用いて、上記目標の加速度を上記第一の状態量として、上記加速度の推定値及びモデル化誤差上限値を仮定する。
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差共分散行列の対角成分を算出する。
ｑｉ（ｔｋ）←（ａｉｍａｘ・Δｔｋ２／２）２
ただし、ｑｉ（ｔｋ）は、時刻ｔｋにおけるモデル化誤差共分散行列Ｑのｉ行ｉ列成分。ａｉｍａｘは、上記加速度のモデル化誤差上限値を表わすベクトルのｉ番目の成分。Δｔｋは、上記基準時刻から上記時刻ｔｋまでの経過時間。
【０１６６】
目標の加速度のモデル化誤差上限値に基づいて目標の位置のモデル化誤差共分散行列を算出し、算出した目標の位置のモデル化誤差共分散行列に基づいて、ゲート行列を算出するので、目標を観測した観測値を精度よく判定することができる。
【０１６７】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定する。
上記相関部及び上記詳細相関部は、上記処理装置を用いて、上記基準時刻における上記目標の複数の状態量の推定値または推定値の候補に基づいて、上記複数の時刻それぞれにおける上記目標の複数の状態量を予測し、上記複数の観測値のうち次の不等式を満たす観測値を、上記目標を観測した観測値であると判定する。
［ｚ−ｈ（ｘｋ）］Ｔ・Ｓｋ−１・［ｚ−ｈ（ｘｋ）］≦ｄ
ただし、ｚは、上記観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。Ｓｋは、上記時刻ｔｋにおけるゲート行列。上付きの−１は、逆行列を表わす。ｄは、ゲートの大きさを表わすパラメータであり、０超の実数。
【０１６８】
観測時刻によって異なるゲート行列に基づいて目標を観測した観測値を判定するので、目標を観測した観測値を精度よく判定することができる。
【０１６９】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定する。
上記尤度算出部及び詳細尤度算出部は、上記処理装置を用いて、次の式の右辺を計算することにより、尤度を算出する。
【数３５】
ただし、φ（ＺＮ｜ｘＰ）は、上記複数の状態量が上記推定値である尤度。ｌｎは、自然対数を表わす。ＰＤは、目標の探知確率を表わすパラメータであり、０超１未満の実数。βは、不要信号密度を表わすパラメータであり、０超の実数。ｄｅｔは、行列式を表わす。πは、円周率。Ｓｋは、時刻ｔｋにおけるゲート行列。ｍｋは、上記複数の観測値のうち、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値の数。ｚｋ，ｊは、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。上付きの−１は、逆行列を表わす。
【０１７０】
上の式を使って算出した尤度に基づいて、目標の航跡の候補を選択し、更新するので、航跡の推定精度を高くすることができる。
【０１７１】
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値を更新したことによる上記尤度の変化に基づいて、上記複数の状態量の推定値が収束したか否かを判定する。
【０１７２】
状態量の推定値の更新による尤度の変化に基づいて収束判定をするので、収束判定を効率よくすることができ、目標の航跡の推定に必要な計算量を減らすことができる。
【０１７３】
上記収束判定部は、上記複数の状態量の推定値が収束したか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値が収束したと判定する。
【０１７４】
収束判定をした回数に基づいて収束判定をするので、収束判定に必要な計算量を減らすことができ、全体として、目標の航跡の推定に必要な計算量を減らすことができる。
【０１７５】
上記推定値更新部は、上記処理装置を用いて、上記尤度が高くなる上記複数の状態量の推定値の差分を算出し、上記複数の状態量それぞれについて、算出した差分を上記状態量の推定値に加えることにより、上記複数の状態量の推定値を更新する。
【０１７６】
上記推定値更新部は、上記処理装置を用いて、最急降下法およびニュートン法および準ニュートン法および共役分配法のいずれかにより、上記尤度が高くなる上記複数の状態量の推定値の差分を算出する。
【０１７７】
上記のような最適化方式を使って、状態量の推定値を更新するので、効率よく状態量の推定値を更新することができる。
【０１７８】
以上説明した推定装置（追尾装置１００）は、データを処理する処理装置を有するコンピュータを上記推定装置として機能させるコンピュータプログラムを、コンピュータが実行することにより実現することができる。
【符号の説明】
【０１７９】
１００ 追尾装置、１１０ 観測データ抽出部、１１１ 観測値取得部、１２０ 観測データ蓄積部、１２１ 観測値記憶部、１３０ 観測データ管理部、１４０ 航跡候補生成部、１４１ 候補算出部、１４２ 候補記憶部、１５０ 相関ゲート設定部、１５１ 誤差上限初期化部、１５２ 誤差上限値記憶部、１５３ ゲート行列算出部、１６０ 概探索部、１６１ 相関部、１６２ 尤度算出部、１６３ 選択部、１７０ 詳細探索部、１７１ 推定値記憶部、１７２ 推定値更新部、１７３ 収束判定部、１７４ 誤差上限値更新部、１７５ 終了判定部、１７６ 推定値出力部、７００ 目標、８００ 追尾システム、８１０ センサ、９１１ 処理装置、９１２ 入力装置、９１３ 出力装置、９１４ 記憶装置。
【特許請求の範囲】
【請求項１】
データを処理する処理装置と、候補算出部と、相関部と、尤度算出部と、選択部と、推定値更新部と、推定値出力部とを有し、
上記候補算出部は、目標の複数の状態量のうち所定の第一の状態量を除く他の状態量について、上記処理装置を用いて、上記他の状態量の推定値の複数の候補値を算出し、
上記相関部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出し、
上記尤度算出部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記相関部が仮定したモデル化誤差上限値と、上記相関部が抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出し、
上記候補選択部は、上記処理装置を用いて、上記尤度算出部が算出した尤度に基づいて、上記候補算出部が算出した複数の候補値のなかから、上記他の状態量の推定値を選択し、
上記推定値更新部は、上記処理装置を用いて、上記相関部が仮定した第一の状態量の推定値と上記候補選択部が選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする推定装置。
【請求項２】
上記推定装置は、更に、収束判定部と、誤差上限値更新部と、詳細相関部と、詳細尤度算出部とを有し、
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定し、
上記誤差上限値更新部は、上記推定値更新部が上記複数の状態量の推定量を更新した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくし、
上記詳細相関部は、上記処理装置を用いて、上記複数の状態量が、上記推定値更新部が更新した推定値であり、上記モデル化誤差が、上記誤差上限値更新部が小さくしたモデル化誤差上限値以下であるとの仮定に基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出し、
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出し、
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする請求項１に記載の推定装置。
【請求項３】
上記推定装置は、更に、収束判定部と、終了判定部と、誤差上限値更新部と、詳細相関部と、詳細尤度算出部とを有し、
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定し、
上記終了判定部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記複数の状態量の推定を終了するか否かを判定し、
上記誤差上限値更新部は、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくし、
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出し、
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出し、
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合、および、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記複数の状態量の推定を終了すると上記終了判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする請求項１に記載の推定装置。
【請求項４】
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値が上記目標を観測した観測値であるかと判定する範囲を算出するために使用するゲート行列を算出し、
上記終了判定部は、上記処理装置を用いて、上記詳細相関部が算出したゲート行列に基づいて、上記複数の状態量の推定を終了するか否かを判定することを特徴とする請求項３に記載の推定装置。
【請求項５】
上記終了判定部は、上記複数の状態量の推定を終了するか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記複数の状態量の推定を終了すると判定することを特徴とする請求項３に記載の推定装置。
【請求項６】
上記誤差上限値更新部は、上記処理装置を用いて、次のいずれかの式の右辺を計算することにより、上記モデル化誤差上限値を更新することを特徴とする請求項２乃至請求項５のいずれかに記載の推定装置。
ａｉｍａｘ←√［（ａｉｍａｘ）２−Δａｉ２］
ただし、ａｉｍａｘは、上記モデル化誤差上限値を表わすベクトルのｉ番目の成分。Δａｉは、上記推定値更新部が更新した後の第一の状態量の推定値を表わすベクトルのｉ番目の成分と、上記推定値更新部が更新する前の第一の状態量の推定値を表わすベクトルのｉ番目の成分との差。
ａｉｍａｘ←λ・ａｉｍａｘ
ただし、λは、モデル化誤差上限値の逓減率であり、０超１未満の実数。
【請求項７】
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次のいずれかの式の右辺を計算することにより、ゲート行列を算出し、算出したゲート行列を使用して、上記複数の観測値が上記目標を観測した観測値であると判定する範囲を算出し、上記複数の観測値のうち、算出した範囲内に入る観測値を、上記目標を観測した観測値であると判定することを特徴とする請求項２乃至請求項６のいずれかに記載の推定装置。
Ｓ←Ｒ＋Ｑ
ただし、Ｓは、ゲート行列であり、上記観測値を表わすベクトルの成分の数を次数とする正方行列。Ｒは、観測雑音共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記観測値を表わすベクトルの各成分の観測精度の二乗を対角成分とする対角行列。Ｑは、モデル化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記モデル化誤差上限値を表わすベクトルの各成分から算出される値を対角成分とする対角行列。
Ｓ←Ｒ＋Ｑ＋Ｑ’
ただし、Ｑ’は、量子化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とする対称行列。
【請求項８】
上記推定装置は、複数の時刻における上記目標の位置を、上記目標を観測した観測値として入力し、所定の基準時刻における上記目標の位置及び速度及び加速度を、上記複数の状態量として推定し、
上記相関部は、上記処理装置を用いて、上記目標の加速度を上記第一の状態量として、上記加速度の推定値及びモデル化誤差上限値を仮定し、
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差共分散行列の対角成分を算出することを特徴とする請求項７に記載の推定装置。
ｑｉ（ｔｋ）←（ａｉｍａｘ・Δｔｋ２／２）２
ただし、ｑｉ（ｔｋ）は、時刻ｔｋにおけるモデル化誤差共分散行列Ｑのｉ行ｉ列成分。ａｉｍａｘは、上記加速度のモデル化誤差上限値を表わすベクトルのｉ番目の成分。Δｔｋは、上記基準時刻から上記時刻ｔｋまでの経過時間。
【請求項９】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定し、
上記相関部及び上記詳細相関部は、上記処理装置を用いて、上記基準時刻における上記目標の複数の状態量の推定値または推定値の候補に基づいて、上記複数の時刻それぞれにおける上記目標の複数の状態量を予測し、上記複数の観測値のうち次の不等式を満たす観測値を、上記目標を観測した観測値であると判定することを特徴とする請求項７または請求項８に記載の推定装置。
［ｚ−ｈ（ｘｋ）］Ｔ・Ｓｋ−１・［ｚ−ｈ（ｘｋ）］≦ｄ
ただし、ｚは、上記観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。Ｓｋは、上記時刻ｔｋにおけるゲート行列。上付きの−１は、逆行列を表わす。ｄは、ゲートの大きさを表わすパラメータであり、０超の実数。
【請求項１０】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定し、
上記尤度算出部及び詳細尤度算出部は、上記処理装置を用いて、次の式の右辺を計算することにより、尤度を算出することを特徴とする請求項７乃至請求項９のいずれかに記載の推定装置。
【数１】
ただし、φ（ＺＮ｜ｘＰ）は、上記複数の状態量が上記推定値である尤度。ｌｎは、自然対数を表わす。ＰＤは、目標の探知確率を表わすパラメータであり、０超１未満の実数。βは、不要信号密度を表わすパラメータであり、０超の実数。ｄｅｔは、行列式を表わす。πは、円周率。Ｓｋは、時刻ｔｋにおけるゲート行列。ｍｋは、上記複数の観測値のうち、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値の数。ｚｋ，ｊは、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。上付きの−１は、逆行列を表わす。
【請求項１１】
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値を更新したことによる上記尤度の変化に基づいて、上記複数の状態量の推定値が収束したか否かを判定することを特徴とする請求項２乃至請求項１０のいずれかに記載の推定装置。
【請求項１２】
上記収束判定部は、上記複数の状態量の推定値が収束したか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値が収束したと判定することを特徴とする請求項２乃至請求項１０のいずれかに記載の推定装置。
【請求項１３】
上記推定値更新部は、上記処理装置を用いて、更新前よりも上記尤度が高くなる上記複数の状態量の推定値の差分を算出し、上記複数の状態量それぞれについて、算出した差分を上記状態量の推定値に加えることにより、上記複数の状態量の推定値を更新することを特徴とする請求項１乃至請求項１２のいずれかに記載の推定装置。
【請求項１４】
上記推定値更新部は、上記処理装置を用いて、最急降下法およびニュートン法および準ニュートン法および共役分配法のいずれかにより、更新前よりも上記尤度が高くなる上記複数の状態量の推定値の差分を算出することを特徴とする請求項１３に記載の推定装置。
【請求項１５】
データを処理する処理装置を有するコンピュータが実行することにより、上記コンピュータが請求項１乃至請求項１４のいずれかに記載の推定装置として機能することを特徴とするコンピュータプログラム。
【請求項１６】
データを処理する処理装置を有する推定装置が、目標の複数の状態量を推定する推定方法において、
上記複数の状態量のうち所定の第一の状態量を除く他の状態量について、上記処理装置が、上記他の状態量の推定値の複数の候補値を算出し、
算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置が、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出し、
算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置が、仮定したモデル化誤差上限値と、抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出し、
上記処理装置が、算出した尤度に基づいて、算出した複数の候補値のなかから、上記他の状態量の推定値を選択し、
上記処理装置が、仮定した第一の状態量の推定値と選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新し、
上記処理装置が、更新した上記複数の状態量の推定値を出力することを特徴とする推定方法。
【請求項１】
データを処理する処理装置と、候補算出部と、相関部と、尤度算出部と、選択部と、推定値更新部と、推定値出力部とを有し、
上記候補算出部は、目標の複数の状態量のうち所定の第一の状態量を除く他の状態量について、上記処理装置を用いて、上記他の状態量の推定値の複数の候補値を算出し、
上記相関部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出し、
上記尤度算出部は、上記候補算出部が算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置を用いて、上記相関部が仮定したモデル化誤差上限値と、上記相関部が抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出し、
上記候補選択部は、上記処理装置を用いて、上記尤度算出部が算出した尤度に基づいて、上記候補算出部が算出した複数の候補値のなかから、上記他の状態量の推定値を選択し、
上記推定値更新部は、上記処理装置を用いて、上記相関部が仮定した第一の状態量の推定値と上記候補選択部が選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする推定装置。
【請求項２】
上記推定装置は、更に、収束判定部と、誤差上限値更新部と、詳細相関部と、詳細尤度算出部とを有し、
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定し、
上記誤差上限値更新部は、上記推定値更新部が上記複数の状態量の推定量を更新した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくし、
上記詳細相関部は、上記処理装置を用いて、上記複数の状態量が、上記推定値更新部が更新した推定値であり、上記モデル化誤差が、上記誤差上限値更新部が小さくしたモデル化誤差上限値以下であるとの仮定に基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出し、
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出し、
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする請求項１に記載の推定装置。
【請求項３】
上記推定装置は、更に、収束判定部と、終了判定部と、誤差上限値更新部と、詳細相関部と、詳細尤度算出部とを有し、
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値が収束したか否かを判定し、
上記終了判定部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束したと上記収束判定部が判定した場合に、上記処理装置を用いて、上記複数の状態量の推定を終了するか否かを判定し、
上記誤差上限値更新部は、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、上記モデル化誤差上限値を小さくし、
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値のなかから上記目標を観測した観測値を抽出し、
上記詳細尤度算出部は、上記推定値更新部が更新した上記複数の状態量の推定値について、上記処理装置を用いて、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値と、上記詳細相関部が抽出した観測値とに基づいて、上記複数の状態量が上記推定値である尤度を算出し、
上記推定値更新部は、上記推定値更新部が更新した上記複数の状態量の推定値が収束していないと上記収束判定部が判定した場合、および、上記複数の状態量の推定を終了しないと上記終了判定部が判定した場合に、上記処理装置を用いて、更新した上記複数の状態量の推定値を出発点として、上記詳細尤度算出部が算出する尤度が更に高くなるよう、上記複数の状態量の推定値を更新し、
上記推定値出力部は、上記複数の状態量の推定を終了すると上記終了判定部が判定した場合に、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値を出力することを特徴とする請求項１に記載の推定装置。
【請求項４】
上記詳細相関部は、上記処理装置を用いて、上記推定値更新部が更新した上記複数の状態量の推定値と、上記相関部が仮定し若しくは上記誤差上限値更新部が小さくしたモデル化誤差上限値とに基づいて、上記複数の観測値が上記目標を観測した観測値であるかと判定する範囲を算出するために使用するゲート行列を算出し、
上記終了判定部は、上記処理装置を用いて、上記詳細相関部が算出したゲート行列に基づいて、上記複数の状態量の推定を終了するか否かを判定することを特徴とする請求項３に記載の推定装置。
【請求項５】
上記終了判定部は、上記複数の状態量の推定を終了するか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記複数の状態量の推定を終了すると判定することを特徴とする請求項３に記載の推定装置。
【請求項６】
上記誤差上限値更新部は、上記処理装置を用いて、次のいずれかの式の右辺を計算することにより、上記モデル化誤差上限値を更新することを特徴とする請求項２乃至請求項５のいずれかに記載の推定装置。
ａｉｍａｘ←√［（ａｉｍａｘ）２−Δａｉ２］
ただし、ａｉｍａｘは、上記モデル化誤差上限値を表わすベクトルのｉ番目の成分。Δａｉは、上記推定値更新部が更新した後の第一の状態量の推定値を表わすベクトルのｉ番目の成分と、上記推定値更新部が更新する前の第一の状態量の推定値を表わすベクトルのｉ番目の成分との差。
ａｉｍａｘ←λ・ａｉｍａｘ
ただし、λは、モデル化誤差上限値の逓減率であり、０超１未満の実数。
【請求項７】
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次のいずれかの式の右辺を計算することにより、ゲート行列を算出し、算出したゲート行列を使用して、上記複数の観測値が上記目標を観測した観測値であると判定する範囲を算出し、上記複数の観測値のうち、算出した範囲内に入る観測値を、上記目標を観測した観測値であると判定することを特徴とする請求項２乃至請求項６のいずれかに記載の推定装置。
Ｓ←Ｒ＋Ｑ
ただし、Ｓは、ゲート行列であり、上記観測値を表わすベクトルの成分の数を次数とする正方行列。Ｒは、観測雑音共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記観測値を表わすベクトルの各成分の観測精度の二乗を対角成分とする対角行列。Ｑは、モデル化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とし、上記モデル化誤差上限値を表わすベクトルの各成分から算出される値を対角成分とする対角行列。
Ｓ←Ｒ＋Ｑ＋Ｑ’
ただし、Ｑ’は、量子化誤差共分散行列であり、上記観測値を表わすベクトルの成分の数を次数とする対称行列。
【請求項８】
上記推定装置は、複数の時刻における上記目標の位置を、上記目標を観測した観測値として入力し、所定の基準時刻における上記目標の位置及び速度及び加速度を、上記複数の状態量として推定し、
上記相関部は、上記処理装置を用いて、上記目標の加速度を上記第一の状態量として、上記加速度の推定値及びモデル化誤差上限値を仮定し、
上記相関部及び上記詳細相関部は、上記処理装置を用いて、次の式の右辺を計算することにより、上記モデル化誤差共分散行列の対角成分を算出することを特徴とする請求項７に記載の推定装置。
ｑｉ（ｔｋ）←（ａｉｍａｘ・Δｔｋ２／２）２
ただし、ｑｉ（ｔｋ）は、時刻ｔｋにおけるモデル化誤差共分散行列Ｑのｉ行ｉ列成分。ａｉｍａｘは、上記加速度のモデル化誤差上限値を表わすベクトルのｉ番目の成分。Δｔｋは、上記基準時刻から上記時刻ｔｋまでの経過時間。
【請求項９】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定し、
上記相関部及び上記詳細相関部は、上記処理装置を用いて、上記基準時刻における上記目標の複数の状態量の推定値または推定値の候補に基づいて、上記複数の時刻それぞれにおける上記目標の複数の状態量を予測し、上記複数の観測値のうち次の不等式を満たす観測値を、上記目標を観測した観測値であると判定することを特徴とする請求項７または請求項８に記載の推定装置。
［ｚ−ｈ（ｘｋ）］Ｔ・Ｓｋ−１・［ｚ−ｈ（ｘｋ）］≦ｄ
ただし、ｚは、上記観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。Ｓｋは、上記時刻ｔｋにおけるゲート行列。上付きの−１は、逆行列を表わす。ｄは、ゲートの大きさを表わすパラメータであり、０超の実数。
【請求項１０】
上記推定装置は、複数の時刻における観測値を入力し、所定の基準時刻における複数の状態量を推定し、
上記尤度算出部及び詳細尤度算出部は、上記処理装置を用いて、次の式の右辺を計算することにより、尤度を算出することを特徴とする請求項７乃至請求項９のいずれかに記載の推定装置。
【数１】
ただし、φ（ＺＮ｜ｘＰ）は、上記複数の状態量が上記推定値である尤度。ｌｎは、自然対数を表わす。ＰＤは、目標の探知確率を表わすパラメータであり、０超１未満の実数。βは、不要信号密度を表わすパラメータであり、０超の実数。ｄｅｔは、行列式を表わす。πは、円周率。Ｓｋは、時刻ｔｋにおけるゲート行列。ｍｋは、上記複数の観測値のうち、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値の数。ｚｋ，ｊは、上記時刻ｔｋにおいて上記目標を観測した観測値であると、上記相関部または上記詳細相関部が判定した観測値を表わす縦ベクトル。ｈは、観測関数を表わす。ｘｋは、時刻ｔｋにおける上記目標の複数の状態量の予測値を表わす縦ベクトル。上付きのＴは、行列の転置を表わす。上付きの−１は、逆行列を表わす。
【請求項１１】
上記収束判定部は、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値を更新したことによる上記尤度の変化に基づいて、上記複数の状態量の推定値が収束したか否かを判定することを特徴とする請求項２乃至請求項１０のいずれかに記載の推定装置。
【請求項１２】
上記収束判定部は、上記複数の状態量の推定値が収束したか否かを判定した回数が所定の回数を超えた場合に、上記処理装置を用いて、上記推定値更新部が上記複数の状態量の推定値が収束したと判定することを特徴とする請求項２乃至請求項１０のいずれかに記載の推定装置。
【請求項１３】
上記推定値更新部は、上記処理装置を用いて、更新前よりも上記尤度が高くなる上記複数の状態量の推定値の差分を算出し、上記複数の状態量それぞれについて、算出した差分を上記状態量の推定値に加えることにより、上記複数の状態量の推定値を更新することを特徴とする請求項１乃至請求項１２のいずれかに記載の推定装置。
【請求項１４】
上記推定値更新部は、上記処理装置を用いて、最急降下法およびニュートン法および準ニュートン法および共役分配法のいずれかにより、更新前よりも上記尤度が高くなる上記複数の状態量の推定値の差分を算出することを特徴とする請求項１３に記載の推定装置。
【請求項１５】
データを処理する処理装置を有するコンピュータが実行することにより、上記コンピュータが請求項１乃至請求項１４のいずれかに記載の推定装置として機能することを特徴とするコンピュータプログラム。
【請求項１６】
データを処理する処理装置を有する推定装置が、目標の複数の状態量を推定する推定方法において、
上記複数の状態量のうち所定の第一の状態量を除く他の状態量について、上記処理装置が、上記他の状態量の推定値の複数の候補値を算出し、
算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置が、上記第一の状態量が所定の推定値であり、上記第一の状態量が上記推定値であると仮定することによって生じるモデル化誤差が所定のモデル化誤差上限値以下であり、上記他の状態量が上記候補値であるとの仮定に基づいて、上記目標を観測した観測値を含む複数の観測値のなかから、上記目標を観測した観測値を抽出し、
算出した上記他の状態量の推定値の複数の候補値それぞれについて、上記処理装置が、仮定したモデル化誤差上限値と、抽出した観測値とに基づいて、上記他の状態量が上記候補値である尤度を算出し、
上記処理装置が、算出した尤度に基づいて、算出した複数の候補値のなかから、上記他の状態量の推定値を選択し、
上記処理装置が、仮定した第一の状態量の推定値と選択した他の状態量の推定値とを出発点として、上記目標の複数の状態量が推定値である尤度が高くなるよう、上記複数の状態量の推定値を更新し、
上記処理装置が、更新した上記複数の状態量の推定値を出力することを特徴とする推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１２−１２７８９０（Ｐ２０１２−１２７８９０Ａ）
【公開日】平成２４年７月５日（２０１２．７．５）
【国際特許分類】
【出願番号】特願２０１０−２８１２７２（Ｐ２０１０−２８１２７２）
【出願日】平成２２年１２月１７日（２０１０．１２．１７）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年７月５日（２０１２．７．５）
【国際特許分類】
【出願日】平成２２年１２月１７日（２０１０．１２．１７）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]