予測装置及び予測システム及びコンピュータプログラム及び予測方法
【課題】観測対象の将来の位置を高い精度で予測する。
【解決手段】観測装置８１０（位置観測装置）は、移動体（観測対象）の位置を観測する。ジャイロセンサ８２０（角速度観測装置）は、移動体の角速度を観測する。バイアス補正部１４１（角速度誤差推定装置）は、観測装置８１０が観測した位置に基づいて、ジャイロセンサ８２０が観測した角速度の誤差を推定する。バイアス補正部１４１（角速度補正装置）は、推定した誤差に基づいて、ジャイロセンサ８２０が観測した角速度を補正する。運動状態外挿部１８０（予測位置算出装置）は、バイアス補正部１４１が補正した角速度に基づいて、移動体の将来の位置を予測する。
【解決手段】観測装置８１０（位置観測装置）は、移動体（観測対象）の位置を観測する。ジャイロセンサ８２０（角速度観測装置）は、移動体の角速度を観測する。バイアス補正部１４１（角速度誤差推定装置）は、観測装置８１０が観測した位置に基づいて、ジャイロセンサ８２０が観測した角速度の誤差を推定する。バイアス補正部１４１（角速度補正装置）は、推定した誤差に基づいて、ジャイロセンサ８２０が観測した角速度を補正する。運動状態外挿部１８０（予測位置算出装置）は、バイアス補正部１４１が補正した角速度に基づいて、移動体の将来の位置を予測する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、移動する観測対象の将来の位置を予測する予測装置に関する。
【背景技術】
【０００２】
観測対象の将来の位置を予測するには、観測対象の現在の位置や速度などを精度よく推定する必要がある。
観測対象の位置を観測する方式には、レーダや全地球測位システム（ＧＰＳ）受信機などの位置観測装置を用いる方式がある。いずれの方式も観測誤差があるので、カルマンフィルタなどの追尾フィルタを用いて、観測対象の現在の位置や速度などを推定する技術がある。
カルマンフィルタなどの追尾フィルタは、観測対象が等速直線運動など所定の運動モデルにしたがって運動していることを前提とする場合がある。運動モデルが観測対象の実際の運動に合致していない場合は推定精度が低くなるので、複数の運動モデルを切り替えて観測対象の現在の位置や速度などを推定する技術がある。
また、観測対象の角速度を観測する方式には、ジャイロセンサなどの角速度観測装置を用いる方式がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平１１−１２０５００号公報
【特許文献２】特開２００５−２７４３００号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
観測対象の運動状態が変化した場合、追尾フィルタは一般に応答が遅いので、運動モデルをすぐに切り替えることはできない。角速度観測装置は一般に応答が速いが、観測誤差があるので、そのままでは、運動モデルの選択に用いることはできない。
この発明は、例えば、上記のような課題を解決するためになされたものであり、観測対象の将来の位置を高い精度で予測することを目的とする。
【課題を解決するための手段】
【０００５】
この発明にかかる予測装置は、位置観測装置が観測した観測対象の位置と、角速度観測装置が観測した上記観測対象の角速度とに基づいて、上記観測対象の将来の位置を予測する予測装置において、
上記予測装置は、角速度誤差推定装置と、角速度補正装置と、予測位置算出装置とを有し、
上記角速度誤差推定装置は、上記位置観測装置が観測した位置に基づいて、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度補正装置は、上記角速度誤差推定装置が推定した誤差に基づいて、上記角速度観測装置が観測した角速度を補正し、
上記予測位置算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記観測対象の将来の位置を予測することを特徴とする。
【発明の効果】
【０００６】
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【図面の簡単な説明】
【０００７】
【図１】実施の形態１における追尾システム８００の構成の一例を示すブロック構成図。
【図２】実施の形態１におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図。
【図３】実施の形態１における観測装置８１０が観測位置を観測する時刻と、ジャイロセンサ８２０がジャイロ角速度を観測する時刻との間の関係の一例を示す図。
【図４】実施の形態１における観測角速度位置予測部１４３及び推定角速度位置予測部１４４が予測する予測位置６３１，６３２の関係の一例を示す図。
【図５】実施の形態１における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の一例を示す図。
【図６】実施の形態１における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の別の例を示す図。
【図７】実施の形態１におけるバイアス補正処理Ｓ７１０の流れの一例を示すフローチャート図。
【図８】実施の形態１における旋回判定部１４２の構成の一例を示す詳細ブロック図。
【図９】実施の形態１における旋回判定部１４２の動作を説明するための図。
【図１０】実施の形態１におけるモデル選択部１３２の構成の一例を示す詳細ブロック図。
【図１１】実施の形態１における推移確率更新部２１２の動作の例を説明するための図。
【図１２】実施の形態１における推移確率更新部２１２が更新する推移確率の例を示す図。
【図１３】実施の形態１における運動状態外挿部１８０の動作を説明するための図。
【図１４】実施の形態２における追尾システム８００の構成の一例を示すブロック構成図。
【図１５】実施の形態３における追尾システム８００の構成の一例を示すブロック構成図。
【図１６】実施の形態３におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図。
【図１７】実施の形態３における観測角速度位置予測部１４３及び直線位置予測部１４８が予測する予測位置６３１，６３３の関係の一例を示す図。
【図１８】実施の形態４における予測システム８０１の構成の一例を示すシステム構成図。
【発明を実施するための形態】
【０００８】
実施の形態１．
実施の形態１について、図１〜図１３を用いて説明する。
【０００９】
図１は、この実施の形態における追尾システム８００の構成の一例を示すブロック構成図である。
【００１０】
追尾システム８００（予測システム）は、観測装置８１０（位置観測装置）を有する。観測装置８１０は、航空機、船舶、車両などの移動体（観測対象）の位置を観測する。観測装置８１０が観測した移動体の位置を「観測位置」と呼ぶ。観測装置８１０は、例えば、レーダ、ＧＰＳ受信機などのセンサである。観測装置８１０は、例えばレーダのように、移動体を外部から観測する装置であってもよいし、例えばＧＰＳ受信機のように、移動体に搭載された装置であってもよい。
【００１１】
追尾システム８００は、ジャイロセンサ８２０（角速度観測装置）を有する。ジャイロセンサ８２０は、観測対象である移動体に搭載されている。ジャイロセンサ８２０は、搭載された移動体の角速度を観測する。ジャイロセンサ８２０が観測した角速度を「ジャイロ角速度」（ＪＹＲＯ角速度）と呼ぶ。
【００１２】
追尾システム８００は、目標追尾装置１００（追尾部１３０、運動状態判定部１４０、バイアス補正値メモリ１５０、運動性能記憶部１６０、運動状態外挿部１８０）を有する。目標追尾装置１００は、観測装置８１０及びジャイロセンサ８２０の観測結果に基づいて、移動体の将来の位置を高精度に推定する。
【００１３】
目標追尾装置１００は、バイアス補正値メモリ１５０（角速度誤差記憶装置）を有する。バイアス補正値メモリ１５０は、後述するバイアス補正部１４１が算出したバイアス誤差を記憶する。
目標追尾装置１００は、運動性能記憶部１６０を有する。運動性能記憶部１６０は、移動体の最高速度、最大加速度、最大角速度など移動体の運動性能を記憶している。運動性能記憶部１６０は、例えば、移動体の種別と、その種別に属する移動体の運動性能との組を、データベース形式（運動性能データベース）で記憶している。
【００１４】
目標追尾装置１００（予測装置）は、追尾部１３０を有する。
追尾部１３０は、複数の追尾フィルタ１３１（追尾装置）を有する。なお、符号にアルファベット小文字を添えて「追尾フィルタ１３１ａ」「追尾フィルタ１３１ｂ」などと記載することにより、複数の追尾フィルタ１３１を区別する場合がある。
追尾フィルタ１３１は、所定の運動モデルに基づいて、追尾処理をする。すなわち、追尾フィルタ１３１は、移動体が所定の運動モデルにしたがって運動しているものと仮定して、観測装置８１０が観測した観測位置に基づいて、移動体の位置や速度などを推定する。以下、追尾フィルタ１３１が推定した移動体の位置や速度などの推定値を「追尾位置」「追尾速度」などと呼ぶ。追尾フィルタ１３１は、例えばカルマンフィルタ、アルファ−ベータ（α−β）フィルタ、アルファ−ベータ−ガンマ（α−β−γ）フィルタやその他のフィルタなどである。
各追尾フィルタ１３１ａ〜１３１ｃは、それぞれ異なる運動モデルを持っている。各追尾フィルタ１３１ａ〜１３１ｃは、それぞれが持つ運動モデルにしたがって移動体が運動しているものと仮定して、移動体の位置や速度などを推定する。例えば、追尾フィルタ１３１ａは、運動モデルとして旋回運動モデルを持っている。すなわち、追尾フィルタ１３１ａは、移動体が旋回運動をしているものと仮定して、移動体の位置・速度・角速度などを推定する。また、例えば、追尾フィルタ１３１ｂは、運動モデルとして等速直線運動モデルを持っている。すなわち、追尾フィルタ１３１ｂ（直線位置推定装置）は、移動体が等速直線運動をしているものと仮定して、追尾位置・速度などを推定する。また、例えば、追尾フィルタ１３１ｃは、運動モデルとして等加速度運動モデルを持っている。すなわち、追尾フィルタ１３１ｃは、移動体が等加速度運動をしているものと仮定して、移動体の位置・速度・加速度などを推定する。
なお、旋回運動モデルを持つ追尾フィルタ１３１は、移動体が所定の角速度で旋回運動をしていると仮定する構成（所定角速度位置推定装置）であってもよい。その場合、複数の追尾フィルタ１３１が旋回運動モデルを持ち、それぞれが異なる角速度を仮定する構成であってもよい。
等加速度運動モデルを持つ追尾フィルタ１３１も同様に、移動体が所定の加速度で等加速度運動をしていると仮定する構成であってもよい。その場合、複数の追尾フィルタ１３１が等加速度運動モデルを持ち、それぞれが異なる加速度を仮定する構成であってもよい。
【００１５】
追尾部１３０は、モデル選択部１３２（予測位置算出装置）を有する。モデル選択部１３２は、後述する旋回判定部１４２の判定結果などに基づいて、移動体の実際の運動に最もよく合致している運動モデルを選択する。モデル選択部１３２は，選択した運動モデルに基づいて、移動体の位置・速度・角速度・加速度などの推定値を算出する。以下、モデル選択部１３２が算出した移動体の位置・角速度などの推定値を「推定位置」「推定角速度」などと呼ぶ。
例えば、モデル選択部１３２は、複数の追尾フィルタ１３１がそれぞれ持っている運動モデルが、移動体の実際の運動に合致している確率を算出し、算出した確率に基づいて、各追尾フィルタ１３１ａ〜１３１ｃが推定した追尾位置などに重み付けをして合成し、推定位置を算出する。
あるいは、モデル選択部１３２は、複数の追尾フィルタ１３１のなかから、移動体の実際の運動に運動モデルが合致している確率が最も高い追尾フィルタ１３１を選択し、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとして採用する。
【００１６】
また、追尾フィルタ１３１は、推定した追尾位置や追尾速度などがどの程度の誤差を含むかを推定する。例えば、追尾フィルタ１３１は、追尾位置や追尾速度などの誤差分散や誤差共分散を算出する。
モデル選択部１３２は、追尾フィルタ１３１が算出した誤差分散や誤差共分散に基づいて、推定位置や推定速度などの誤差分散や誤差共分散を算出する。例えば、モデル選択部１３２は、複数の追尾フィルタ１３１が算出した誤差分散や誤差共分散を合成して、推定位置や推定速度などの誤差分散や誤差共分散を算出する。あるいは、モデル選択部１３２は、選択した追尾フィルタ１３１が算出した誤差分散や誤差共分散を、推定位置や推定速度などの誤差分散や誤差共分散として採用する。
【００１７】
目標追尾装置１００は、運動状態判定部１４０を有する。運動状態判定部１４０は、ジャイロセンサ８２０が観測したジャイロ角速度に基づいて、移動体の運動状態を判定する。
運動状態判定部１４０は、バイアス補正部１４１（角速度誤差推定装置および角速度補正装置）を有する。バイアス補正部１４１は、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差を推定する。バイアス補正部１４１は、推定したバイアス誤差に基づいて、ジャイロ角速度を補正し、補正したジャイロ角速度を出力する。バイアス補正部１４１は、例えば、追尾フィルタ１３１が推定した追尾角速度（ＴＲＫ角速度）、モデル選択部１３２が算出した推定角速度などに基づいて、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差を推定する。
運動状態判定部１４０は、旋回判定部１４２（旋回判定装置）を有する。旋回判定部１４２は、バイアス補正部１４１が補正したジャイロ角速度や、運動性能記憶部１６０が記憶した移動体の運動性能などにに基づいて、移動体が旋回しているか否かを判定する。
【００１８】
目標追尾装置１００は、運動状態外挿部１８０（予測位置算出装置）を有する。運動状態外挿部１８０は、移動体の将来の位置を予測する。運動状態外挿部１８０が予測した移動体の位置を「予測位置」と呼ぶ。運動状態外挿部１８０は、モデル選択部１３２が算出した推定位置・推定速度などや、旋回判定部１４２の判定結果などに基づいて、予測位置を算出する。
【００１９】
図２は、この実施の形態におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図である。
バイアス補正部１４１は、旋回運動モデルを持つ追尾フィルタ１３１ａが推定した追尾角速度を入力する。バイアス補正部１４１は、モデル選択部１３２が算出した推定位置・推定速度・誤差分散・誤差共分散を入力する。バイアス補正部１４１は、ジャイロセンサ８２０が観測したジャイロ角速度を入力する。なお、ジャイロセンサ８２０が観測したジャイロ角速度をバイアス補正部１４１が直接入力する代わりに、ジャイロセンサ８２０が観測したジャイロ角速度を平滑処理する平滑処理部（追尾フィルタ）を設け、平滑処理部が平滑処理したジャイロ角速度を、バイアス補正部１４１が入力する構成としてもよい。
【００２０】
バイアス補正部１４１は、観測角速度位置予測部１４３（観測角速度位置予測装置）を有する。観測角速度位置予測部１４３は、モデル選択部１３２が算出した推定位置や推定速度などに基づいて、移動体の将来の位置を予測する。ただし、観測角速度位置予測部１４３は、移動体の角速度として、モデル選択部１３２が算出した推定角速度ではなく、バイアス補正部１４１が入力したジャイロ角速度を用いる。観測角速度位置予測部１４３は、ジャイロ角速度に基づいて、移動体の将来の位置を予測する。
【００２１】
図３は、この実施の形態における観測装置８１０が観測位置を観測する時刻と、ジャイロセンサ８２０がジャイロ角速度を観測する時刻との間の関係の一例を示す図である。
一般に、ジャイロセンサ８２０の観測間隔は、観測装置８１０の観測間隔より短い。ある時点において、ジャイロセンサ８２０が最も新しいジャイロ角速度を観測した時刻をｔ１とし、観測装置８１０が最も新しい観測位置を観測した時刻をｔ２とする。一般に、時刻ｔ１と時刻ｔ２とは一致せず、時刻ｔ１のほうが時刻ｔ２よりも後の時刻である。
追尾フィルタ１３１は、観測装置８１０が観測した観測位置に基づいて、追尾位置などを推定するので、追尾フィルタ１３１が推定した追尾位置などは、時刻ｔ２におけるものである。モデル選択部１３２が算出した推定位置なども、同様に、時刻ｔ２におけるものである。
モデル選択部１３２が算出した推定位置などと、ジャイロセンサ８２０が観測したジャイロ角速度とを組み合わせて、移動体の将来の位置を予測するためには、推定位置などの時刻と、ジャイロ角速度の時刻とが一致している必要がある。
【００２２】
このため、観測角速度位置予測部１４３は、例えば、時刻ｔ２における推定位置などに基づいて外挿処理を行い、時刻ｔ１における推定位置などを算出する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ１における推定位置などを算出する。
【数１１】
ただし、山型付きのｘ（文中において「ｘ＾」と表記する。）は、推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。推定値ベクトルｘ＾は、時刻ｔの関数である。推定値ベクトルｘ＾の要素は、所定の座標系で表した時刻ｔにおける推定位置の座標などである。推定値ベクトルｘ＾の次数ｋは、推定値の数である。例えば、３次元座標で表された位置および速度を推定値とする場合、次数ｋは６である。
Ｆは、状態遷移行列である。状態遷移行列Ｆは、ｋ次の正方行列である。状態遷移行列Ｆは、時間Δｔの関数である。状態遷移行列Ｆは、所定の運動モデルにおいて、ある時刻における移動体の位置などを表わすベクトルを、時間Δｔが経過した後における移動体の位置などを表わすベクトルへ写す写像を表わす。
すなわち、この式は、時刻ｔ２における推定値ベクトルｘ＾（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を作用させて、時刻ｔ１における推定値ベクトルｘ＾（ｔ１）を算出することを意味している。
なお、状態遷移行列Ｆの運動モデルは、モデル選択部１３２が選択した追尾フィルタ１３１の運動モデルであってもよいし、等速直線運動モデルや旋回運動モデルなどあらかじめ定めた所定の運動モデルであってもよい。通常、時刻ｔ１と時刻ｔ２との差は十分小さいので、運動モデルの差による違いはほとんどない。
【００２３】
観測角速度位置予測部１４３は、算出した時刻ｔ１における推定位置などに基づいて、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ１から所定の時間ΔＴが経過した時刻ｔ３（すなわち、ｔ３＝ｔ１＋ΔＴ）における移動体の位置などを予測する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ３における移動体の位置などを算出する。
【数１２】
ただし、ｘ＾Ｊは、時刻ｔ１におけるジャイロ角速度で推定角速度を置き換えた推定値ベクトルである。ＦＪは、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。
すなわち、この式は、時刻ｔ１における推定値ベクトルｘ＾Ｊ（ｔ１）に、移動体が時刻ｔ１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪを作用させて、時刻ｔ３における推定値ベクトルｘ＾Ｊ（ｔ３）を算出することを意味している。
【００２４】
また、観測角速度位置予測部１４３は、モデル選択部１３２が算出した誤差分散や誤差共分散に基づいて、予測した移動体の将来の位置の誤差分散や誤差共分散を推定する。
【００２５】
推定位置などと同様、誤差分散などの時刻とジャイロ角速度の時刻とを一致させるため、観測角速度位置予測部１４３は、例えば、時刻ｔ２における誤差分散などに基づいて、時刻ｔ１における誤差分散などを算出する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ１における誤差分散などを算出する。
【数１３】
ただし、Ｐは、推定値ベクトルｘ＾の誤差の分散共分散行列である。分散共分散行列Ｐは、ｋ次の正方行列である。分散共分散行列Ｐは、時刻ｔの関数である。分散共分散行列Ｐの要素は、時刻ｔにおける推定値ベクトルの各要素の誤差の間の分散または共分散である。
上付きのＴは、行列の転置を表わす。
Ｑは、システム雑音の分散共分散行列である。分散共分散行列Ｑは、ｋ次の正方行列である。分散共分散行列Ｑは、時間Δｔの関数である。分散共分散行列Ｑの要素は、時間Δｔが経過する間に発生するシステム雑音の分散または共分散である。
すなわち、この式は、時刻ｔ２における推定誤差の分散共分散行列Ｐ（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）の転置行列を左から作用させ、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を右から作用させたものに、時間（ｔ１−ｔ２）の経過により発生したシステム雑音の分散共分散行列Ｑ（ｔ１−ｔ２）を加えて、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）を算出することを意味している。
【００２６】
観測角速度位置予測部１４３は、算出した時刻ｔ１における誤差分散などに基づいて、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ３における誤差分散などを算出する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ３における誤差分散などを算出する。
【数１４】
ただし、ＰＪは、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
この式は、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）に、移動体が時刻ｔ１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）の転置行列を左から作用させ、移動体が時刻ｔ１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）を右から作用させたものに、時間ΔＴの経過により発生したシステム雑音の分散共分散行列Ｑ（ΔＴ）を加えて、時刻ｔ３についての予測誤差の分散共分散行列ＰＪ（ｔ３）を算出することを意味している。
【００２７】
観測角速度位置予測部１４３は、例えば上記のようにして、時刻ｔ３における移動体の位置や誤差分散などを予測する。
なお、観測角速度位置予測部１４３は、バイアス補正部１４１が入力したジャイロ角速度ではなく、後述する角速度補正部１４７が補正したジャイロ角速度に基づいて、移動体の位置や誤差分散などを予測する構成であってもよい。
【００２８】
バイアス補正部１４１は、推定角速度位置予測部１４４（推定角速度位置予測装置）を有する。推定角速度位置予測部１４４は、モデル選択部１３２が算出した推定位置や推定速度などに基づいて、移動体の将来の位置を予測する。ただし、推定角速度位置予測部１４４は、移動体の角速度として、モデル選択部１３２が算出した推定角速度ではなく、追尾フィルタ１３１ａが推定した追尾角速度を用いる。推定角速度位置予測部１４４は、追尾角速度に基づいて、移動体の将来の位置を予測する。
また、推定角速度位置予測部１４４は、モデル選択部１３２が算出した誤差分散や誤差共分散に基づいて、予測した移動体の将来の位置の誤差分散や誤差共分散を推定する。
すなわち、観測角速度位置予測部１４３が推定角速度としてジャイロ角速度を用いるのに対し、推定角速度位置予測部１４４は、推定角速度として追尾角速度を用いて、観測角速度位置予測部１４３と同様の処理をする。なお、追尾フィルタ１３１ａが推定した追尾角速度の時刻と、モデル選択部１３２が算出した推定位置などの時刻とは、同じであるから、時刻を一致させるための計算は不要である。
【００２９】
推定角速度位置予測部１４４は、例えば、次の式の右辺を計算することにより、時刻ｔ３における推定位置や誤差分散などを算出する。
【数１５】
ただし、ｘ＾Ｔは、時刻ｔ２における追尾角速度で推定角速度を置き換えた推定値ベクトルである。ＦＴは、時刻ｔ２における追尾角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。ＰＴは、時刻ｔ２における追尾角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
【００３０】
推定角速度位置予測部１４４は、例えば上記のようにして、移動体の位置や誤差分散などを予測する。
なお、推定角速度位置予測部１４４は、追尾フィルタ１３１ａが推定した追尾角速度ではなく、モデル選択部１３２が算出した推定角速度に基づいて（すなわち、推定角速度を追尾角速度で置き換えることなくそのまま用いて）、移動体の位置や誤差分散などを予測する構成であってもよい。
【００３１】
バイアス補正部１４１は、条件判定部１４５（条件判定装置）を有する。条件判定部１４５は、ジャイロセンサ８２０が観測したジャイロ角速度の誤差を推定する条件（所定の条件）が満たされているか否かを判定する。
【００３２】
図４は、この実施の形態における観測角速度位置予測部１４３及び推定角速度位置予測部１４４が予測する予測位置６３１，６３２の関係の一例を示す図である。
観測位置６１１ａ〜６１１ｄは、時刻ｔ１までの時点で、観測装置８１０が観測した移動体６００の観測位置を表わす。このうち、観測位置６１１ｄは、時刻ｔ２に観測した最も新しい観測位置である。
追尾部１３０は、観測位置６１１ａ〜６１１ｄに基づいて、時刻ｔ２における移動体６００の推定位置６２１を算出する。
観測角速度位置予測部１４３は、追尾部１３０が推定した推定位置６２１などに基づいて、時刻ｔ１における移動体６００の推定位置６２２などを算出する。
観測角速度位置予測部１４３は、算出した推定位置６２２などと、ジャイロ角速度とに基づいて、時刻ｔ３における移動体６００の予測位置６３１を算出する。
推定角速度位置予測部１４４は、追尾部１３０が推定した推定位置６２１などと、追尾フィルタ１３１ａが推定した追尾角速度とに基づいて、時刻ｔ３における移動体６００の予測位置６３２を算出する。
【００３３】
観測装置８１０が観測した観測位置やジャイロセンサ８２０が観測したジャイロ角速度に誤差がなければ、ジャイロ角速度と追尾角速度とは一致する。したがって、観測角速度位置予測部１４３が予測した移動体６００の予測位置６３１と推定角速度位置予測部１４４が予測した移動体６００の予測位置６３２とは一致するはずである。
しかし、実際には、誤差の影響により、予測位置６３１と予測位置６３２とは一致しない。
【００３４】
ジャイロセンサ８２０が観測したジャイロ角速度には、ランダムな誤差と、時刻にかかわらずほぼ一定なバイアス誤差とが含まれる。また、モデル選択部１３２が推定した推定位置などにも、ランダムな誤差が含まれる。
このうち、ランダムな誤差の影響は、観測角速度位置予測部１４３や推定角速度位置予測部１４４が予測した誤差分散などによりある程度予想できる。
したがって、誤差分散などにより予想される範囲を超えて、予測した予測位置６３１と予測位置６３２とが異なる場合には、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差の影響によるものと考えられる。
【００３５】
条件判定部１４５は、例えば、観測角速度位置予測部１４３が予測した予測位置６３１と推定角速度位置予測部１４４が予測した予測位置６３２との差が、予想される範囲内であるか否かを判定し、予想される範囲を超えている場合に、ジャイロ角速度の誤差を推定する条件が満たされていると判定する。
この判定には、統計学上の手法を用いることができる。条件判定部１４５は、例えばカイ二乗検定により、予測位置６３１と予測位置６３２との差が、予想される範囲内であるか否かを判定する。例えば、条件判定部１４５は、次の式の右辺を計算することにより、検定値を算出する。
【数１６】
ただし、εは、検定値である。
条件判定部１４５は、算出した検定値εを、所定の閾値εｔｈと比較する。閾値εｔｈは、所定の有意水準に基づいて、例えばカイ二乗分布表から求めたものである。検定値εが閾値εｔｈ以下である場合、条件判定部１４５は、予測位置６３１と予測位置６３２との差が、予想される範囲内であると判定する。検定値εが閾値εｔｈより大きい場合、条件判定部１４５は、予測位置６３１と予測位置６３２との差が、予想される範囲を超えていると判定する。
【００３６】
図５は、この実施の形態における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の一例を示す図である。
横軸は、時刻ｔを表わす。縦軸は、角速度ωを表わす。
例えば、移動体６００が、直進運動をしたのち、ある時刻を境に、旋回運動に移ったとする。
移動体６００の真の角速度６０６は、直進運動中は０であり、旋回運動中はある値で一定であるものとする。
ジャイロセンサ８２０にバイアス誤差がない場合であっても、それ以外の誤差要因により、ジャイロセンサ８２０が観測するジャイロ角速度６１６は、真の角速度６０６と完全には一致しない。しかし、ジャイロ角速度６１６は、真の角速度６０６の変化に遅れることなく、追随して変化する。
これに対し、追尾フィルタ１３１ａが推定する追尾角速度６２６は、算出間隔が、ジャイロ角速度６１６の観測間隔よりも長く、ランダムな誤差の影響が大きい。また、追尾角速度６２６は、真の角速度６０６の変化に対して、すぐには追随せず、ゆっくりと変化する。したがって、真の角速度６０６が変化した直後は、過渡応答により、真の角速度６０６と追尾角速度６２６との差が大きくなる。
【００３７】
このように、旋回時には、追尾角速度の過渡応答による差が生じるので、予測位置６３１と予測位置６３２との差が、予想される範囲を超えていると条件判定部１４５が判定する場合がある。このため、条件判定部１４５は、バイアス補正区間を設定する。バイアス補正区間とは、時系列的に並べられた複数の判定結果のうち、最新の判定結果を含む連続したＮ回分の判定結果（Ｎは所定の正の整数。）のことである。条件判定部１４５は、バイアス補正区間のなかで、予測位置６３１と予測位置６３２との差が予想される範囲を超えていると判定した回数を数える。条件判定部１４５は、この回数がＭ回以上（Ｍは、Ｎ以下の所定の正の整数。）である場合に、一定のバイアスが生じているものとみなし、バイアス誤差を推定する条件が満たされていると判定する。
【００３８】
バイアス補正部１４１は、角速度誤差推定部１４６（角速度誤差推定装置）を有する。角速度誤差推定部１４６は、バイアス誤差を推定する条件が満たされていると条件判定部１４５が判定した場合に、ジャイロセンサ８２０が観測したジャイロ角速度の誤差を推定する。
【００３９】
角速度誤差推定部１４６は、例えば、カルマンフィルタを用いて、ジャイロセンサ８２０のバイアス誤差を推定する。角速度誤差推定部１４６は、例えば、次の式で定義される観測モデルを用いる。
【数１７】
ただし、Δωｋは、時刻ｋにおけるバイアス誤差の真の値を表わす。ｚｋは、時刻ｋにおけるバイアス誤差の観測値を表わす。ｖｋは、時刻ｋにおけるバイアス誤差の観測誤差を表わす。ωＪ，ｋは、時刻ｋにおいてジャイロセンサ８２０が観測したジャイロ角速度を表わす。ωＴ，ｋは、時刻ｋについて追尾フィルタ１３１ａが推定した追尾角速度を表わす。ｖＪ，ｋは、時刻ｋにおいてジャイロセンサ８２０が観測したジャイロ角速度の観測誤差を表わす。ｖＴ，ｋは、時刻ｋについて追尾フィルタ１３１ａが推定した追尾角速度の推定誤差を表わす。
【００４０】
また、角速度誤差推定部１４６は、例えば、次の式で定義される状態方程式を用いる。
【数１８】
ただし、ｗｋは、時刻ｋ−１から時刻ｋまでの間に発生するシステム雑音を表わす実数である。
【００４１】
図６は、この実施の形態における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の別の例を示す図である。
この例において、ジャイロセンサ８２０が観測するジャイロ角速度６１６は、バイアス誤差Δωを含む。バイアス誤差Δωは、時刻にかかわらず、基本的に不変である。
【００４２】
角速度誤差推定部１４６は、例えば上記のようにして、バイアス誤差Δωを推定する。
バイアス補正値メモリ１５０は、角速度誤差推定部１４６が推定したバイアス誤差Δωを記憶する。
【００４３】
角速度補正部１４７は、バイアス補正値メモリ１５０が記憶したバイアス誤差Δωを用いて、バイアス補正部１４１が入力したジャイロ角速度ωＪを補正する。例えば、角速度補正部１４７は、ジャイロ角速度ωＪからバイアス誤差Δωを差し引いた差（ωＪ−Δω）を計算することにより、補正したジャイロ角速度を得る。
角速度誤差推定部１４６がバイアス誤差Δωを推定した場合、角速度誤差推定部１４６が推定したバイアス誤差Δωをバイアス補正値メモリ１５０が記憶する。角速度補正部１４７は、そのバイアス誤差Δωを用いて、ジャイロ角速度ωＪの誤差を補正する。
角速度誤差推定部１４６がバイアス誤差Δωを推定しなかった場合は、角速度誤差推定部１４６が以前に推定したバイアス誤差Δωをバイアス補正値メモリ１５０が記憶している。角速度補正部１４７は、角速度誤差推定部１４６が以前に推定したバイアス誤差Δωを用いて、ジャイロ角速度ωＪの誤差を補正する。
角速度補正部１４７は、補正したジャイロ角速度を出力する。
【００４４】
図７は、この実施の形態におけるバイアス補正処理Ｓ７１０の流れの一例を示すフローチャート図である。
バイアス補正処理Ｓ７１０において、バイアス補正部１４１は、ジャイロ角速度のバイアス誤差を補正する。
【００４５】
バイアス補正処理Ｓ７１０は、予測位置算出工程Ｓ７１１を有する。予測位置算出工程Ｓ７１１は、バイアス補正処理Ｓ７１０の最初に実行される。
予測位置算出工程Ｓ７１１において、観測角速度位置予測部１４３は、ジャイロ角速度ωＪに基づいて、予測位置や予測誤差分散などを算出する。また、推定角速度位置予測部１４４は、追尾角速度ωＴに基づいて、予測位置や予測誤差分散などを算出する。
【００４６】
バイアス補正処理Ｓ７１０は、条件判定工程Ｓ７１２を有する。条件判定工程Ｓ７１２は、予測位置算出工程Ｓ７１１のあとに実行される。
条件判定工程Ｓ７１２において、条件判定部１４５は、予測位置算出工程Ｓ７１１で観測角速度位置予測部１４３及び推定角速度位置予測部１４４が算出した予測位置や予測誤差分散などに基づいて、検定値εを算出する。条件判定部１４５は、算出した検定値εを閾値εｔｈと比較して、判定式を満たすか否かを判定する。条件判定部１４５は、判定結果を記憶する。条件判定部１４５は、記憶した判定結果のなかから、今回の判定結果を含む過去Ｎ回分の判定結果を取得して、判定式を満たさない（すなわち、検定値εが閾値εｔｈより大きい）と判定した回数を数える。条件判定部１４５は、数えた回数を整数Ｍと比較する。条件判定部１４５は、比較の結果に基づいて、条件分岐する。
【００４７】
バイアス補正処理Ｓ７１０は、バイアス誤差推定処理Ｓ７１３を有する。バイアス誤差推定処理Ｓ７１３は、条件判定工程Ｓ７１２において、判定式を満たさなかった回数がＭ回以上であると条件判定部１４５が判定した場合に実行される。
バイアス誤差推定処理Ｓ７１３において、角速度誤差推定部１４６は、ジャイロ角速度ωＪのバイアス誤差Δωを推定する。
【００４８】
バイアス補正処理Ｓ７１０は、バイアス誤差記憶工程Ｓ７１４を有する。バイアス誤差記憶工程Ｓ７１４は、バイアス誤差推定処理Ｓ７１３のあとに実行される。
バイアス誤差記憶工程Ｓ７１４において、バイアス補正値メモリ１５０は、バイアス誤差推定処理Ｓ７１３で角速度誤差推定部１４６が推定したバイアス誤差Δωを記憶する。
【００４９】
バイアス補正処理Ｓ７１０は、バイアス誤差読出工程Ｓ７１５を有する。バイアス誤差読出工程Ｓ７１５は、条件判定工程Ｓ７１２において、判定式を満たさなかった回数がＭ回未満であると条件判定部１４５が判定した場合に実行される。
バイアス誤差読出工程Ｓ７１５において、角速度補正部１４７は、バイアス補正値メモリ１５０が記憶したバイアス誤差Δωを読み出す。
【００５０】
バイアス補正処理Ｓ７１０は、ジャイロ角速度補正工程Ｓ７１６を有する。ジャイロ角速度補正工程Ｓ７１６は、バイアス誤差記憶工程Ｓ７１４のあと及びバイアス誤差読出工程Ｓ７１５のあとに実行される。
ジャイロ角速度補正工程Ｓ７１６において、角速度補正部１４７は、バイアス誤差推定処理Ｓ７１３で角速度誤差推定部１４６が推定したバイアス誤差Δωあるいは角速度誤差推定部１４６で読み出したバイアス誤差Δωに基づいて、ジャイロ角速度ωＪを補正する。
【００５１】
バイアス補正処理Ｓ７１０は、ジャイロ角速度補正工程Ｓ７１６までで終わりとなる。
【００５２】
図８は、この実施の形態における旋回判定部１４２の構成の一例を示す詳細ブロック図である。
【００５３】
旋回判定部１４２は、最大角速度取得部１９１を有する。最大角速度取得部１９１は、運動性能記憶部１６０が記憶した移動体の運動性能のなかから、移動体の性能限界値である最大の角速度ωｍａｘを取得する。
【００５４】
旋回判定部１４２は、異常値判定部１９２を有する。異常値判定部１９２は、最大角速度取得部１９１が取得した角速度ωｍａｘに基づいて、バイアス補正部１４１が出力した（補正済の）ジャイロ角速度ωＪが異常値であるか否かを判定する。例えば、異常値判定部１９２は、ジャイロ角速度ωＪの絶対値が角速度ωｍａｘより大きい場合に、ジャイロ角速度ωＪが異常値であると判定する。異常値判定部１９２は、異常値であると判定したジャイロ角速度ωＪを除去し、異常値でないと判定したジャイロ角速度ωＪだけを出力する。
【００５５】
旋回判定部１４２は、閾値比較部１９３を有する。閾値比較部１９３は、異常値判定部１９２が出力したジャイロ角速度ωＪを、所定の閾値ωｍｉｎと比較して、どちらが大きいか判定する。
なお、閾値ωｍｉｎは、あらかじめ定めた定数である構成でもよいし、ジャイロ角速度の誤差の標準偏差を算出し、算出した標準偏差（もしくはその定数倍）を閾値ωｍｉｎとする構成でもよい。
【００５６】
旋回判定部１４２は、回数判定部１９５を有する。回数判定部１９５は、ジャイロ角速度ωＪが閾値ωｍｉｎ以上であると閾値比較部１９３が連続して判定した回数を算出する。
例えば、回数判定部１９５は、連続判定回数を記憶しておく。ジャイロ角速度ωＪが閾値ωｍｉｎ以上であると閾値比較部１９３が判定した場合、回数判定部１９５は、記憶した連続判定回数を１増加させる。ジャイロ角速度ωＪが閾値ωｍｉｎ未満であると閾値比較部１９３が判定した場合、回数判定部１９５は、記憶した連続判定回数を０に初期化する。
回数判定部１９５は、算出した連続判定回数を所定の整数Ｍ’と比較して、どちらが大きいか判定する。連続判定回数が整数Ｍ’以上である場合、旋回判定部１４２は移動体が旋回していると判定する。
【００５７】
旋回判定部１４２は、判定結果出力部１９６を有する。連続判定回数が整数Ｍ’以上であると回数判定部１９５が判定した場合、判定結果出力部１９６は、移動体が旋回しているという判定結果を出力する。なお、判定結果出力部１９６は、異常値判定部１９２が出力したジャイロ角速度を移動体の角速度として出力することにより、移動体が旋回していると判定した判定結果を表わす構成としてもよい。移動体が旋回していないと判定した場合は、ジャイロ角速度を出力しないことにより、その判定結果を表わす構成でもよいし、移動体の角速度として０を出力することにより、移動体が旋回していないと判定した判定結果を表わす構成でもよい。
【００５８】
図９は、この実施の形態における旋回判定部１４２の動作を説明するための図である。
ジャイロ角速度ωＪが角速度ωｍａｘより大きい場合は、異常値判定部１９２が除去し、その時刻におけるジャイロ角速度は、欠測扱いとなる。閾値比較部１９３は、異常値判定部１９２が出力したジャイロ角速度を閾値ωｍｉｎと比較する。閾値比較部１９３の比較結果に基づいて、回数判定部１９５が連続判定回数を算出し、判定結果出力部１９６が出力する判定結果が定まる。なお、異常値判定部１９２が除去した場合も含めてジャイロ角速度が欠測の場合は、閾値比較部１９３が比較そのものを行わないので、回数判定部１９５による連続判定回数の算出には影響しない。
【００５９】
図１０は、この実施の形態におけるモデル選択部１３２の構成の一例を示す詳細ブロック図である。
【００６０】
モデル選択部１３２は、モデル確率記憶部２１３を有する。モデル確率記憶部２１３は、追尾フィルタ１３１ａ〜１３１ｃそれぞれについて、その追尾フィルタ１３１が持っている運動モデルが、移動体の実際の運動に合致している確率を記憶している。モデル確率記憶部２１３が記憶している確率を「モデル確率」と呼ぶ。
【００６１】
モデル選択部１３２は、重付平均算出部２１５を有する。重付平均算出部２１５は、モデル確率記憶部２１３が記憶したモデル確率に基づいて、各追尾フィルタ１３１ａ〜１３１ｃが推定した追尾位置などを合成して、推定位置などを算出する。重付平均算出部２１５は、例えば、次の式の右辺を計算することにより、推定位置などを算出する。
【数１９】
ただし、ｘ＾は、重付平均算出部２１５が算出する推定位置などを表わす推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。Ｘ＾は、追尾値行列である。追尾値行列Ｘ＾は、ｋ行ｊ列の行列である。追尾値行列Ｘ＾のｉ列目（ｉは１以上ｊ以下の整数。）は、ｉ番目の追尾フィルタ１３１の追尾値ベクトルｘ＾ｉである。追尾値行列Ｘ＾の列数ｊは、運動モデルの数である。追尾値ベクトルｘ＾ｉは、ｋ次の列ベクトルである。追尾値ベクトルの各要素は、その追尾フィルタ１３１が推定した推定位置の座標などである。ｐは、モデル確率ベクトルである。モデル確率ベクトルは、ｊ次の列ベクトルである。モデル確率ベクトルのｉ行目の要素は、ｉ番目の追尾フィルタ１３１が持っている運動モデルのモデル確率である。
この式は、各運動モデルの追尾値を、モデル確率により重み付けして合成して、推定値を算出することを意味する。
【００６２】
モデル選択部１３２は、推移確率記憶部２１１を有する。推移確率記憶部２１１は、２つの追尾フィルタ１３１のペアそれぞれについて、移動体の実際の運動が、第一の追尾フィルタ１３１が持っている運動モデルに合致している状態から、第二の追尾フィルタ１３１が持っている運動モデルに合致している状態へ推移する確率（モデル間の推移確率パラメータ）を記憶している。推移確率記憶部２１１が記憶している確率を「推移確率」と呼ぶ。
【００６３】
モデル選択部１３２は、モデル確率更新部２１４（重み算出装置）を有する。モデル確率更新部２１４は、推移確率記憶部２１１が記憶した推移確率に基づいて、モデル確率記憶部２１３が記憶したモデル確率を更新する。モデル確率更新部２１４は、例えば、次の式の右辺を計算することにより、モデル確率を更新する。
【数２０】
ただし、ＰＴ，ｋは、時刻ｋにおける推移確率行列である。推移確率行列Ｐは、ｊ次の正方行列である。推移確率行列ＰＴ，ｋのａ行ｂ列の要素（ａ及びｂは、１以上ｊ以下の整数。）は、ｂ番目の追尾フィルタ１３１からａ番目の追尾フィルタ１３１への推移確率である。なお、ａとｂとが等しい場合は、その追尾フィルタ１３１の運動モデルが維持される確率を表わす。推移確率行列ＰＴ，ｋの次数ｊは、運動モデルの数である。
【００６４】
モデル選択部１３２は、推移確率更新部２１２を有する。推移確率更新部２１２は、運動状態判定部１４０が判定した運動状態に基づいて、推移確率記憶部２１１が記憶した推移確率を更新する。
【００６５】
図１１は、この実施の形態における推移確率更新部２１２の動作の例を説明するための図である。
推移確率更新部２１２は、旋回判定部１４２が出力したジャイロ角速度を入力する。推移確率更新部２１２は、入力したジャイロ角速度を、所定の累積区間において積分する。
【００６６】
図１２は、この実施の形態における推移確率更新部２１２が更新する推移確率の例を示す図である。
例えば、運動モデルが２つあり、運動モデルＡは等速直線運動モデル、運動モデルＢは旋回運動モデルであるとする。推移確率更新部２１２は、例えば、ジャイロ角速度を積分した値を用いて、運動モデルＡから運動モデルＢへの推移確率ｐＡ→Ｂを段階的に変化させる。時間方向の累積値を用いることにより、ジャイロ角速度のデータが欠落した場合に、推移確率が急激に変動するのを防ぐ。推移確率更新部２１２は、例えば、次の式の右辺を計算することにより、推移確率ｐＡ→Ｂを算出する。
【数２１】
ただし、ｐｍｉｎは、推移確率の下限値である。ｐｍａｘは、推移確率の上限値である。下限値ｐｍｉｎ及び上限値ｐｍａｘは、あらかじめ定めた０以上１以下の実数である。
この式によれば、累積区間におけるジャイロ角速度ωＪの合計が、移動体の性能限界値である角速度の最大値ωｍａｘ以上の場合、推移確率更新部２１２は、推移確率ｐＡ→Ｂを上限値ｐｍａｘとする。また、累積区間におけるジャイロ角速度ωＪの合計が０の場合、推移確率更新部２１２は、推移確率ｐＡ→Ｂを下限値ｐｍｉｎとする。累積区間におけるジャイロ角速度ωＪの合計が０と最大値ωｍａｘとの間である場合、推移確率更新部２１２は、推移確率ｐＡ→Ｂを、下限値ｐｍｉｎと上限値ｐｍａｘとの間の値とする。
【００６７】
なお、モデル選択部１３２は、追尾フィルタ１３１が推定した追尾位置などを重み付け平均して、推定位置などを算出するのではなく、追尾フィルタ１３１のなかから１つを選択して、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとする構成であってもよい。
例えば、重付平均算出部２１５に代えて、フィルタ選択部を設ける。フィルタ選択部は、モデル確率記憶部２１３が記憶したモデル確率に基づいて、モデル確率が最も高い運動モデルを持つ追尾フィルタ１３１を選択する。フィルタ選択部は、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとして出力する。
モデル選択部１３２は、更に、簡略化した構成として、推移確率パラメータを用いずにモデルを切り替える構成であってもよい。例えば、モデル選択部１３２は、累積区間の間に旋回判定部１４２が判定した判定結果に基づいて、移動体が旋回していると判定した回数の割合を算出する。モデル選択部１３２は、算出した割合が所定の割合を超える場合、旋回運動モデルを持つ追尾フィルタ１３１を選択し、算出した割合が所定の割合未満の場合、等速直線運動モデルを持つ追尾フィルタ１３１を選択する。モデル選択部１３２は、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとして出力する。
【００６８】
運動状態外挿部１８０は、モデル選択部１３２が算出した推定位置などと、旋回判定部１４２が出力したジャイロ角速度ωＪとに基づいて、移動体の将来の位置を予測する。運動状態外挿部１８０は、例えば、次の式の右辺を計算することにより、時刻ｔｐにおける移動体の位置などの予測値を算出する。
【数２２】
ただし、Ｆ０は、移動体が等速直線運動をしているという運動モデルにおける状態遷移行列である。ＦＪは、移動体が角速度ωＪで旋回運動をしているという運動モデルにおける状態遷移行列である。ｔ２は、モデル選択部１３２が算出した最新の推定位置などの時刻である。
この式は、追尾部１３０が算出した最新の推定位置などに基づいて、移動体が旋回していないと旋回判定部１４２が判定した場合は、等速直線運動モデルを用いて移動体の運動状態を外挿し、移動体が旋回していると旋回判定部１４２が判定した場合は、旋回モデルを用いて移動体の運動状態を外挿することにより、移動体の位置を予測することを意味する。
【００６９】
図１３は、この実施の形態における運動状態外挿部１８０の動作を説明するための図である。
【００７０】
旋回判定部１４２は、ジャイロセンサ８２０がジャイロ角速度を観測するたびに、移動体が旋回しているか否かを判定するので、移動体が旋回しているか否かを旋回判定部１４２が判定する間隔は、一般に、モデル選択部１３２が推定位置などを算出する間隔よりも短い。このため、モデル選択部１３２が算出した最新の推定位置などの時刻ｔ２よりも後に、旋回判定部１４２による判定が変わる場合がある。例えば、この図のように、時刻ｔ２において、移動体が旋回していないと旋回判定部１４２が判定していたが、時刻ｔ１以降は、移動体が旋回していると旋回判定部１４２が判定したとする。その場合、運動状態外挿部１８０は、例えば、次の式の右辺を計算することにより、時刻ｔｐにおける移動体の位置などを予測する。
【数２３】
この式は、追尾部１３０が算出した時刻ｔ２における推定位置などに基づいて、等速直線運動モデルを用いて時刻ｔ１における推定位置などを算出し、算出した時刻ｔ１の推定位置などに基づいて、旋回運動モデルを用いて時刻ｔｐにおける推定することを意味している。
【００７１】
なお、ジャイロ角速度のランダムな観測誤差を取り除くため、カルマンフィルタなどによって平滑化したジャイロ角速度に基づいて、運動状態外挿部１８０は、移動体の位置を予測する構成であってもよい。
【００７２】
目標追尾装置１００は、複数の異なる運動モデルで構成される追尾フィルタ１３１を用いて目標（移動体）の観測値から推定値を算出し、ジャイロの角速度から運動状態を判別して、運動状態に基づいて目標の将来位置を予測する。
運動状態判定部１４０は、ジャイロの角速度を追尾部１３０で算出した角速度を用いてバイアス補正し、バイアス補正後の角速度を用いて運動状態を判別する。
追尾部１３０は、運動状態から適切な運動モデルを選択し、推定値を算出する。
運動状態外挿部１８０は、推定値と運動状態に基づいて時間外挿する。
センサ観測位置から算出した角速度を用いて、ジャイロの角速度に生じるバイアス誤差を除去し、補正後の角速度を用いることで、より早く目標の運動変化を検出し、目標の運動に応じた適切な運動モデルに基づく精度の高い予測をすることができる。
【００７３】
運動状態判定部１４０は、追尾部１３０から得られる推定位置からジャイロの角速度および追尾部の角速度のそれぞれで時間外挿して算出した予測値の差分を予測誤差共分散行列を用いたカイ二乗判定によって、同一とみなされない場合はジャイロの角速度に含まれるバイアス誤差推定を行う。
【００７４】
運動状態判定部１４０は、ジャイロの角速度に含まれるバイアス誤差をカルマンフィルタによって推定する。
【００７５】
運動状態判定部１４０は、ジャイロの角速度の中で運動性能データベースの性能限界値を超える値を除去する。
【００７６】
運動状態判定部１４０は、ジャイロの角速度の観測時刻と追尾部の推定時刻が異なる場合、観測時刻と推定時刻の差を時間外挿することで、ジャイロ角速度の観測時刻に追尾推定値の時刻を合わせる。
【００７７】
追尾部１３０は、運動状態判定部１４０で算出された角速度を用いてモデル推移確率を変更する。
【００７８】
追尾部１３０は、運動状態判定部１４０で算出された角速度が大きい場合は旋回運動モデルもしくは加速度運動モデルへ変更する。
【００７９】
運動状態外挿部１８０は、運動状態判定部１４０で算出された角速度を用いて時間外挿した位置を出力する。
【００８０】
運動状態外挿部１８０は、運動状態判定部１４０で算出された角速度を平滑処理した値を用いて時間外挿した位置を出力する。
【００８１】
運動状態外挿部１８０は、運動状態判定部１４０で旋回と判定された時刻までは等速直線運動モデルに基づく時間外挿を行い，旋回判定された時刻から所望の時刻までは前記運動状態判定部１４０で算出された角速度を用いて時間外挿した位置を出力する。
【００８２】
追尾部１３０は、レーダもしくはＧＰＳなどのセンサから得られる観測位置より複数の運動モデルに基づく追尾処理を実施して目標の推定位置を計算する。運動状態判定部１４０は、追尾部１３０で推定した角速度からジャイロセンサから得られる角速度のバイアス誤差を補正して、その角速度より目標の旋回を判定する。運動状態外挿部１８０は、運動フェーズに基づいた時間外挿によって将来位置を予測する。
これにより、目標が等速直線運動と仮定して将来位置を算出する場合と比較して、目標旋回時でも、位置予測精度を高くすることができる。また、ジャイロセンサから得られる角速度にバイアス誤差がある場合でも、位置予測精度を高くすることができる。
【００８３】
予測装置（目標追尾装置１００）は、位置観測装置（観測装置８１０）が観測した観測対象（移動体）の位置と、角速度観測装置（ジャイロセンサ８２０）が観測した上記観測対象の角速度（ジャイロ角速度）とに基づいて、上記観測対象の将来の位置を予測する。
上記予測装置（１００）は、角速度誤差推定装置（角速度誤差推定部１４６）と、角速度補正装置（角速度補正部１４７）と、予測位置算出装置（モデル選択部１３２、運動状態外挿部１８０）とを有する。
上記角速度誤差推定装置（１４６）は、上記位置観測装置（８１０）が観測した位置に基づいて、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
上記角速度補正装置（角速度補正部１４７）は、上記角速度誤差推定装置（１４６）が推定した誤差に基づいて、上記角速度観測装置（８１０）が観測した角速度を補正する。
上記予測位置算出装置（１３２，１８０）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記観測対象の将来の位置を予測する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【００８４】
上記予測装置（１００）は、角速度誤差記憶装置（バイアス補正値メモリ１５０）を有する。
上記角速度誤差推定装置（１４６）は、所定の条件を満たす場合に、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
上記角速度誤差記憶装置（１５０）は、上記角速度誤差推定装置（１４６）が推定した誤差を記憶する。
上記角速度補正装置（１４７）は、上記角速度誤差記憶装置（１５０）が記憶した誤差に基づいて、上記角速度観測装置（８２０）が観測した角速度を補正する。
所定の条件を満たす場合のみ、角速度観測装置が観測した角速度の誤差を推定し、それ以外の場合は、角速度誤差記憶装置が記憶した誤差を用いて、角速度観測装置が観測した角速度の誤差を補正するので、例えば、位置観測装置が観測した観測位置の精度が低い場合に、角速度の誤差の推定精度が落ちるのを防ぐことができる。
【００８５】
上記予測装置（１００）は、追尾装置（追尾部１３０）と、条件判定装置（条件判定部１４５）とを有する。
上記追尾装置（１３０）は、上記位置観測装置（８１０）が観測した位置に基づいて、上記観測対象の角速度を推定する。
上記条件判定装置（１４５）は、上記角速度観測装置（８２０）が観測した角速度と上記追尾装置（１３０）が推定した角速度とに基づいて、所定の条件を満たすか否かを判定する。
上記角速度誤差推定装置（１４６）は、上記所定の条件を満たすと上記条件判定装置（１４５）が判定した場合に、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
位置観測装置が観測した位置に基づいて観測対象の角速度を推定し、角速度の誤差を推定する条件を満たすか判定するので、例えば、観測対象の角速度の推定精度が低い場合に、角速度の誤差の推定精度が落ちるのを防ぐことができる。
【００８６】
上記予測装置（１００）は、推定角速度位置予測装置（推定角速度位置予測部１４４）と、観測角速度位置予測装置（観測角速度位置予測部１４３）とを有する。
上記推定角速度位置予測装置（１４４）は、上記追尾装置（１３０）が推定した角速度に基づいて、上記観測対象の将来の位置を予測する。
上記観測角速度位置予測装置（１４３）は、上記角速度観測装置（８２０）が観測した角速度に基づいて、上記観測対象の将来の位置を予測する。
上記条件判定装置（１４５）は、上記推定角速度位置予測装置（１４４）が予測した位置と、上記観測角速度位置予測装置（１４３）が予測した位置とに基づいて、上記所定の条件を満たすか否かを判定する。
推定した観測対象の角速度に基づいて予測した観測対象の位置と、観測した角速度に基づいて予測した観測対象の位置とに基づいて、角速度の誤差を推定する条件を満たすか判定するので、例えば、観測対象の位置の予測精度が低い場合に、角速度の誤差の推定精度が落ちるのを防ぐことができる。
【００８７】
上記条件判定装置（１４５）は、上記推定角速度位置予測装置（１４４）が予測した位置と、上記観測角速度位置予測装置（１４３）が予測した位置との間に有意な差があるか否かを検定する。
上記角速度誤差推定装置（１４６）は、上記推定角速度位置予測装置（１４４）が予測した位置と、上記観測角速度位置予測装置（１４３）が予測した位置との間に有意な差があると上記条件判定装置（１４５）が判定した場合に、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
２つの予測位置の間に有意な差があるか否かを検定して、角速度の誤差を推定する条件を満たすか判定するので、角速度の誤差を推定する必要がある場合のみ、角速度の誤差を推定することができ、計算負荷を減らすことができる。
【００８８】
上記予測装置（１００）は、直線位置推定装置（追尾フィルタ１３１）を有する。
上記直線位置推定装置（１３１）は、上記観測対象が直線運動をしているものとして、上記位置観測装置（８１０）が観測した位置に基づいて、上記観測対象の位置を推定する。
上記予測位置算出装置（１３２，１８０）は、上記観測対象が旋回していないと上記旋回判定装置（１４２）が判定した場合に、上記直線位置推定装置（１３１）が推定した位置に基づいて、上記観測対象の将来の位置を予測する。
観測対象が旋回していないと判定した場合は、直線運動モデルに基づいて推定した位置に基づいて、観測対象の将来の位置を予測するので、予測精度を高めることができる。
【００８９】
上記予測装置（１００）は、複数の所定角速度位置推定装置（追尾フィルタ１３１）を有する。
上記複数の所定角速度位置推定装置（１３１）は、それぞれ異なる所定の角速度で上記観測対象が旋回運動しているものとして、上記位置観測装置（８１０）が観測した位置に基づいて、上記観測対象の位置を推定する。
上記予測位置算出装置（１３２，１８０）は、上記角速度補正装置（１４７）が補正した角速度と、上記複数の所定角速度位置推定装置（１３１）が推定した位置とに基づいて、上記観測対象の位置を算出し、算出した上記観測対象の位置に基づいて、上記観測対象の将来の位置を予測する。
所定の角速度の旋回運動モデルに基づいて観測対象の位置を推定するので、計算負荷を減らすことができる。また、補正した角速度と、複数の所定角速度位置推定装置が推定した位置とに基づいて、観測対象の位置を算出するので、観測対象の位置を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【００９０】
上記予測装置（１００）は、重み算出装置（モデル確率更新部２１４）を有する。
上記重み算出装置（２１４）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記複数の所定角速度位置推定装置（１３１）それぞれの重み付け（モデル確率）を算出する。
上記予測位置算出装置（重付平均算出部２１５）は、上記重み算出装置（２１４）が算出した重み付けにしたがって、上記複数の所定角速度位置推定装置（１３１）それぞれが推定した位置を合成して、上記観測対象の位置を算出する。
補正した角速度に基づいて算出した重み付けに基づいて、複数の所定角速度位置推定装置が推定した位置を合成するので、観測対象の位置を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【００９１】
上記予測装置（１００）は、旋回判定装置（旋回判定部１４２）を有する。
上記旋回判定装置（１４２）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記観測対象（移動体）が旋回しているか否かを判定する。
上記予測位置算出装置（１３２，１８０）は、上記旋回判定装置（１４２）が判定した結果に基づいて、上記観測対象の将来の位置を予測する。
旋回判定装置の判定結果に基づいて、観測対象の将来の位置を予測するので、観測対象の将来の位置を高い精度で予測することができる。
【００９２】
上記予測位置算出装置（フィルタ選択部）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記複数の所定角速度位置推定装置（１３１）のなかから、採用する所定角速度位置推定装置（１３１）を選択し、選択した所定角速度位置推定装置（１３１）が推定した位置に基づいて、上記観測対象の将来の位置を算出する。
補正した角速度に基づいて、複数の所定角速度位置推定装置が推定した位置のなかから観測対象の位置を選択するので、観測対象の位置を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【００９３】
予測システム（追尾システム８００）は、観測対象の位置を観測する位置観測装置（８１０）と、上記観測対象の角速度を観測する角速度観測装置（８２０）と、予測装置（１００）とを有する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【００９４】
上記予測装置（１００）が、位置観測装置（８１０）が観測した観測対象の位置と、角速度観測装置（８２０）が観測した上記観測対象の角速度とに基づいて、観測対象の将来の位置を予測する予測方法は、以下の工程を有する。
上記角速度誤差推定装置（１４６）が、上記位置観測装置（８１０）が観測した位置に基づいて、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
上記角速度補正装置（１４７）が、上記角速度誤差推定装置（１４６）が推定した誤差に基づいて、上記角速度観測装置（８２０）が観測した角速度を補正する。
上記予測位置算出装置（１３２，１８０）が、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記観測対象の将来の位置を予測する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【００９５】
複数の運動モデルを持つ追尾装置から適切な運動モデルを選択するので、観測対象の現在の位置を高い精度で推定し、観測対象の将来の位置を高い精度で予測することができる。センサで観測される位置と追尾予測位置との差（残差）を用いて適切な運動モデルを選択する場合と比べて、サンプル数が必要なく、迅速に運動モデルの選択をすることができる。
【００９６】
実施の形態２．
実施の形態２について、図１４を用いて説明する。
なお、実施の形態１と共通する部分については、同一の符号を付し、説明を省略する。
【００９７】
図１４は、この実施の形態における追尾システム８００の構成の一例を示すブロック構成図である。
【００９８】
目標追尾装置１００は、実施の形態１で説明した構成に加えて、更に、地図記憶部１７０を有する。地図記憶部１７０は、移動体が移動する領域の形状など、移動体の運動状態を推定する手がかりとなる情報（地図データ）を記憶している。例えば、移動体が車両である場合、地図記憶部１７０は、道路の形状を記憶している。あるいは、移動体が航空機である場合、地図記憶部１７０は、飛行計画経路を記憶している。例えば、直線道路上や、飛行計画経路の直線区間であれば、移動体は直線運動することが予想される。
【００９９】
バイアス補正部１４１の条件判定部１４５（直線判定装置）は、モデル選択部１３２が算出した移動体の推定位置を、地図記憶部１７０が記憶した情報と照合して、移動体が直線運動することが予想される直線区間内に存在するか否かを判定する。条件判定部１４５は、直線区間内に移動体がある場合、ジャイロ角速度のバイアス誤差を推定すると判定する。
【０１００】
バイアス補正部１４１の角速度誤差推定部１４６は、ジャイロ角速度のバイアス誤差を推定すると条件判定部１４５が判定した場合、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差を推定する。推定の方式は、実施の形態１で説明したカルマンフィルタを用いた方式でもよいが、この実施の形態では、異なる方式について説明する。
角速度誤差推定部１４６は、ジャイロセンサ８２０が観測したジャイロ角速度の平均値を算出し、追尾フィルタ１３１が推定した追尾角速度の平均値を算出し、算出した平均値の差を算出することにより、ジャイロ角速度のバイアス誤差を得る。角速度誤差推定部１４６は、例えば、次の式の右辺を計算することにより、ジャイロ角速度のバイアス誤差Δωを算出する。
【数２４】
ただし、Ｉは、直線区間内に移動体がある間に、ジャイロセンサ８２０がジャイロ角速度ωＪを観測した数である。Ｊは、直線区間内に移動体がある間に、追尾フィルタ１３１が追尾角速度ωＴを推定した数である。
すなわち、この式は、直線区間内に移動体がある間に、ジャイロセンサ８２０が観測したジャイロ角速度ωＪを平均し、追尾フィルタ１３１が推定した追尾角速度ωＴを平均して、２つの平均の差を取ることにより、ジャイロ角速度のバイアス誤差Δωを算出することを意味する。
【０１０１】
モデル選択部１３２の推移確率更新部２１２は、移動体の推定位置を、地図記憶部１７０が記憶した情報と照合して、移動体が存在する区間を判定する。直線区間内に移動体があると判定した場合、推移確率更新部２１２は、推移確率記憶部２１１が記憶した推移確率を更新して、直線運動モデルへの推移確率を高くする。また、推移確率更新部２１２は、カーブ区間内に移動体があると判定した場合、推移確率更新部２１２は、推移確率記憶部２１１が記憶した推移確率を更新して、旋回モデルへの推移確率を高くする。
【０１０２】
なお、推移確率を介さずにモデル確率を更新する構成であってもよい。例えば、直線区間内に移動体があると判定した場合、モデル選択部１３２のモデル確率更新部２１４は、直線運動モデルのモデル確率を１、それ以外の運動モデルのモデル確率を０にする。また、カーブ区間内に移動体があると判定した場合、モデル確率更新部２１４は、直線運動モデルのモデル確率を０、旋回運動モデルのモデル確率を１にする。
【０１０３】
追尾部１３０は、地図データを参照し、直線路では等速直線運動モデル、カーブ路では旋回運動モデルもしくは加速度運動モデルへ変更する。
【０１０４】
上記予測装置（目標追尾装置１００）は、直線判定装置（条件判定部１４５）を有する。
上記直線判定装置（１４５）は、上記位置観測装置（観測装置８１０）が観測した位置に基づいて、上記観測対象（移動体）が直線運動をしているか否かを判定する。
上記角速度誤差推定装置（角速度誤差推定部１４６）は、上記観測対象が直線運動をしていると上記直線判定装置（１４５）が判定した場合に、上記角速度観測装置（ジャイロセンサ８２０）が観測した角速度の誤差を推定する。
観測対象が直線運動をしていると判定した場合に、角速度観測装置が観測した角速度の誤差を推定するので、角速度の誤差を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【０１０５】
実施の形態３．
実施の形態３について、図１５〜図１７を用いて説明する。
なお、実施の形態１及び実施の形態２と共通する部分については、同一の符号を付し、説明を省略する。
【０１０６】
図１５は、この実施の形態における追尾システム８００の構成の一例を示すブロック構成図である。
追尾部１３０は、１つの追尾フィルタ１３１ｂを有する。追尾フィルタ１３１ｂは、等速直線運動モデルに基づいて、移動体の位置などを推定する。追尾フィルタ１３１ｂは、例えば、α−βフィルタ、カルマンフィルタやその他のフィルタなどである。追尾フィルタ１３１ｂとしてα−βフィルタを使うことにより、カルマンフィルタなどよりも計算負荷を軽くすることができる。
【０１０７】
図１６は、この実施の形態におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図である。
観測角速度位置予測部１４３は、追尾フィルタ１３１ｂが推定した追尾位置などと、ジャイロセンサ８２０が観測したジャイロ角速度とに基づいて、移動体がジャイロ角速度で旋回運動をしているという運動モデルにしたがって、移動体の将来の位置を予測する。
バイアス補正部１４１は、推定角速度位置予測部１４４に代えて、直線位置予測部１４８を有する。直線位置予測部１４８は、追尾フィルタ１３１ｂが推定した追尾位置などに基づいて、移動体が等速直線運動をしているという運動モデルにしたがって、移動体の将来の位置を予測する。
【０１０８】
図１７は、この実施の形態における観測角速度位置予測部１４３及び直線位置予測部１４８が予測する予測位置６３１，６３３の関係の一例を示す図である。
観測位置６１１ａ〜６１１ｄは、時刻ｔ１までの時点で、観測装置８１０が観測した移動体６００の観測位置を表わす。このうち、観測位置６１１ｄは、時刻ｔ２に観測した最も新しい観測位置である。
追尾部１３０は、観測位置６１１ａ〜６１１ｄに基づいて、時刻ｔ２における移動体６００の推定位置６２１を算出する。
観測角速度位置予測部１４３は、追尾部１３０が推定した推定位置６２１などに基づいて、時刻ｔ１における移動体６００の推定位置６２２などを算出する。
観測角速度位置予測部１４３は、算出した推定位置６２２などと、ジャイロ角速度とに基づいて、時刻ｔ３における移動体６００の予測位置６３１を算出する。
直線位置予測部１４８は、追尾部１３０が推定した推定位置６２１などに基づいて、時刻ｔ３における移動体６００の予測位置６３３を算出する。
【０１０９】
条件判定部１４５は、カイ二乗検定などにより、観測角速度位置予測部１４３が算出した予測位置６３１と、直線位置予測部１４８が算出した予測位置６３３との間に、有意な差があるか否かを判定する。２つの予測位置６３１，６３３の間に有意な差がある場合、条件判定部１４５は、ジャイロ角速度の誤差を推定すると判定する。
【０１１０】
角速度誤差推定部１４６は、ジャイロ角速度の誤差を推定すると条件判定部１４５が判定した場合に、ジャイロ角速度の誤差を推定する。角速度誤差推定部１４６は、例えば、ジャイロセンサ８２０が観測したジャイロ角速度ωＪをそのまま、ジャイロ角速度のバイアス誤差Δωの観測値とする。すなわち、移動体が直線運動をしていると仮定して、ジャイロ角速度のバイアス誤差Δωを推定する。
【０１１１】
移動体が等速直線運動をしていない場合、追尾フィルタ１３１ｂの運動モデルと実際の移動体の運動とが合致していないので、追尾フィルタ１３１ｂによる推定の精度が落ち、推定の誤差分散が大きくなる。予測位置６３１と予測位置６３３との間には有意な差があるとは言えなくなるので、条件判定部１４５は、ジャイロ角速度の誤差を推定しないと判定する。
逆に言えば、ジャイロ角速度の誤差を推定すると条件判定部１４５が判定した場合、追尾フィルタ１３１ｂの運動モデルが実際の移動体の運動に合致していることを意味するから、移動体は、等速直線運動をしていると考えられる。
【０１１２】
角速度誤差推定部１４６は、例えば、カルマンフィルタを用いて、ジャイロ角速度のバイアス誤差Δωの推定値を算出する。角速度誤差推定部１４６は、例えば、次の式で定義される観測モデルを用いる。
【数２５】
【０１１３】
なお、条件判定部１４５は、実施の形態２で説明したように、地図記憶部１７０が記憶した情報に基づいて、直線区間内に移動体がいるか否かを判定する構成であってもよい。条件判定部１４５は、直線区間内に移動体がいると判定した場合、ジャイロ角速度の誤差を推定すると判定する。
【０１１４】
目標追尾装置１００は、等速直線運動モデルの追尾フィルタ１３１ｂを用いて目標（移動体）の観測値から推定値を算出し、ジャイロの角速度から運動状態を判別して、運動状態に基づいて目標の将来位置を予測する。
運動状態判定部１４０は、ジャイロの角速度を追尾部１３０から得られる推定値を用いてバイアス補正し、バイアス補正後の角速度を用いて運動状態を判別する。
追尾部１３０は、運動状態から等速直線運動モデルに基づいて推定値を算出する。
運動状態外挿部１８０は、推定値と運動状態に基づいて時間外挿する。
【０１１５】
追尾部１３０は、追尾フィルタ１３１ｂとしてα−βフィルタを用いる。
【０１１６】
運動状態判定部１４０は、追尾部１３０から得られる推定位置からジャイロの角速度を用いて時間外挿して算出した予測値と等速直線運動と仮定して時間外挿して算出した予測値の差分を予測誤差分散によるカイ二乗判定によって、同一とみなされない場合はジャイロの角速度に含まれるバイアス誤差推定を行う。
【０１１７】
運動状態判定部１４０は、地図データを用いて目標が直線路に存在している場合はバイアス補正処理を実施し、それ以外の経路ではバイアス補正処理を行わない。
【０１１８】
実施の形態４．
実施の形態４について、図１８を用いて説明する。
なお、実施の形態１乃至実施の形態３と共通する部分については、同一の符号を付し、説明を省略する。
【０１１９】
図１８は、この実施の形態における予測システム８０１の構成の一例を示すシステム構成図である。
予測システム８０１は、複数の観測対象（移動体）の将来の位置を予測して、観測対象同士が衝突する危険があるか否かを判定する。
【０１２０】
予測システム８０１は、位置観測装置８１１を有する。位置観測装置８１１は、観測対象の位置を観測する。位置観測装置８１１は、例えばレーダやＧＰＳなどである。位置観測装置８１１が観測した位置を観測位置と呼ぶ。
【０１２１】
予測システム８０１は、角速度観測装置８２１を有する。角速度観測装置８２１は、観測対象の角速度を観測する。角速度観測装置８２１は、例えばジャイロスコープである。角速度観測装置８２１が観測した角速度を観測角速度と呼ぶ。
【０１２２】
予測システム８０１は、予測装置１０１を有する。予測装置１０１は、位置観測装置８１１が観測した観測位置や角速度観測装置８２１が観測した観測角速度に基づいて、観測対象の現在位置や運動状態を推定する。予測装置１０１は、推定した結果に基づいて、観測対象の将来の位置を予測する。予測装置１０１は、予測した結果に基づいて、観測対象同士が衝突する危険があるか否かを判定する。
予測装置１０１は、例えば、コンピュータである。
予測装置１０１は、図示していない記憶装置を有する。記憶装置は、コンピュータプログラムやデータなどを記憶する。記憶装置は、例えば、揮発性メモリ（ＲＡＭ）、不揮発性メモリ（ＲＯＭ）、磁気ディスク装置、光学ディスク装置などである。
予測装置１０１は、図示していない処理装置を有する。処理装置は、記憶装置が記憶したコンピュータプログラムを実行することにより、記憶装置が記憶したデータを処理し、予測装置１０１全体を制御する。
予測装置１０１は、図示していない入力装置を有する。入力装置は、予測装置１０１の外部からデータ、信号、操作などの情報を入力し、処理装置が処理するデータに変換する。入力装置が変換したデータは、処理装置が直接処理してもよいし、記憶装置が一時的に記憶してもよい。
予測装置１０１は、図示していない出力装置を有する。出力装置は、処理装置が処理したデータや記憶装置が記憶したデータなどを、データ、信号、画像、音声など予測装置１０１の外部に出力できる形式に変換し、外部に出力する。
以下に説明する予測装置１０１の機能ブロックは、記憶装置が記憶したコンピュータプログラムを、処理装置が実行することにより、実現される。なお、予測装置１０１は、一台のコンピュータであってもよいし、予測装置１０１の一または複数の機能ブロックを実現するコンピュータが複数集まったものであってもよい。
なお、予測装置１０１の機能ブロックは、コンピュータにより実現するのではなく、アナログ回路やデジタル回路などの電子回路により実現してもよいし、機械的構成など電気的構成以外の構成により実現してもよい。
【０１２３】
予測装置１０１は、追尾装置２３１を有する。追尾装置２３１は、処理装置がデータを処理することにより、位置観測装置８１１が観測した観測位置に基づいて、観測対象の位置、速度、角速度など観測対象の現在の運動状態を推定する。追尾装置２３１が推定した観測対象の現在の運動状態を表わす数値を推定値と呼ぶ。追尾装置２３１は、例えば、カルマンフィルタを用いて、推定値を推定する。また、追尾装置２３１は、観測対象の現在の運動状態の推定の精度を推定する。追尾装置２３１は、例えば、推定値の誤差の期待値の分散や共分散などを算出する。
【０１２４】
予測装置１０１は、角速度誤差推定装置２４６を有する。角速度誤差推定装置２４６は、処理装置がデータを処理することにより、角速度観測装置８２１が観測した観測角速度と、追尾装置２３１が推定した角速度の推定値とに基づいて、観測角速度の誤差を推定する。角速度誤差推定装置２４６は、例えば、角速度観測装置８２１が観測した観測角速度と、追尾装置２３１が推定した角速度の推定値との差を観測値とし、カルマンフィルタなどにより、観測角速度のバイアス誤差やゲイン誤差などを推定する。角速度誤差推定装置２４６が推定した観測角速度の誤差を、誤差推定値と呼ぶ。
なお、角速度観測装置８２１が観測角速度を観測した時刻と、追尾装置２３１が角速度の推定値を推定する根拠となった観測位置を位置観測装置８１１が観測した時刻とが異なる場合、角速度誤差推定装置２４６は、例えば、位置観測装置８１１が観測位置を観測した時刻を基準として、その前後において追尾装置２３１が観測した観測角速度を平滑処理することにより、位置観測装置８１１が観測位置を観測した時刻における観測角速度を算出する。平滑処理により算出された観測角速度を平滑角速度と呼ぶ。角速度誤差推定装置２４６は、算出した平滑角速度と、追尾装置２３１が推定した角速度の推定値との差に基づいて、観測角速度の誤差を推定する。なお、角速度誤差推定装置２４６は、実施の形態１で説明した方式で、観測時刻を合わせる構成であってもよい。
【０１２５】
予測装置１０１は、条件判定装置２４５を有する。条件判定装置２４５は、処理装置がデータを処理することにより、追尾装置２３１が推定した推定の精度に基づいて、誤差推定値を推定をするか否かを判定する。追尾装置２３１が推定した角速度の推定値の精度が低い場合、それを基準として誤差推定値を推定すると、かえって誤差推定値の精度が落ちる場合があるので、条件判定装置２４５は、誤差推定値を推定しないと判定する。条件判定装置２４５は、例えば、追尾装置２３１が算出した誤差分散の値を所定の閾値と比較して、誤差分散の値が閾値より小さい場合に、誤差推定値を推定すると判定する。
角速度誤差推定装置２４６は、条件判定装置２４５の判定結果に基づき、誤差推定値を推定すると条件判定装置２４５が判定した場合に、観測角速度の誤差を推定する。
【０１２６】
予測装置１０１は、角速度誤差記憶装置２５０を有する。角速度誤差記憶装置２５０は、記憶装置がデータを記憶することにより、角速度誤差推定装置２４６が推定した誤差推定値を記憶する。
【０１２７】
なお、条件判定装置２４５を設けず、角速度誤差推定装置２４６は、常に、誤差推定値を推定する構成であってもよい。角速度誤差推定装置２４６は、追尾装置２３１が推定した推定精度に基づいて、カルマンフィルタで用いる観測誤差分散共分散行列の値を変化させる構成であってもよい。角速度誤差推定装置２４６は、例えば、追尾装置２３１が算出した誤差分散の値を、観測誤差分散共分散行列の要素の値とする。
【０１２８】
予測装置１０１は、角速度補正装置２４７を有する。角速度補正装置２４７は、処理装置がデータを処理することにより、角速度誤差記憶装置２５０が記憶した誤差推定値を使って、角速度観測装置８２１が観測した観測角速度の誤差を補正する。角速度補正装置２４７は、例えば、観測角速度から誤差推定値を差し引いた差を算出することにより、誤差を補正した観測角速度を得る。角速度補正装置２４７が誤差を補正した観測角速度を、補正済角速度と呼ぶ。
【０１２９】
予測装置１０１は、予測位置算出装置２８０を有する。予測位置算出装置２８０は、処理装置がデータを処理することにより、追尾装置２３１が推定した観測対象の現在の運動状態と、角速度補正装置２４７が補正した補正済角速度とに基づいて、観測対象の将来の運動状態を予測する。予測位置算出装置２８０が予測した観測対象の将来の運動状態を表わす数値を予測値と呼ぶ。
【０１３０】
予測装置１０１は、衝突危険判定装置２９０を有する。衝突危険判定装置２９０は、処理装置がデータを処理することにより、予測位置算出装置２８０が予測した観測対象の将来の運動状態に基づいて、複数の観測対象同士が衝突する危険があるか否かを判定する。衝突危険判定装置２９０は、衝突する危険があると判定した場合、そのことを警告する警告メッセージを出力する。衝突危険判定装置２９０が出力した警告メッセージは、例えば、管制画面に表示され、あるいは、観測対象に対して送信されるなどして、衝突の危険を回避する行動を促す。
【０１３１】
上記予測装置１０１は、衝突危険判定装置２９０を有し、複数の観測対象それぞれについて、上記観測対象の将来の位置を予測する。
上記衝突危険判定装置２９０は、上記予測位置算出装置２８０が予測した位置に基づいて、上記複数の観測対象が衝突する危険があるか否かを判定する。
観測対象の将来の位置を高い精度で予測するので、衝突の危険を高い精度で判定することができる。
【０１３２】
上記コンピュータプログラムは、コンピュータが実行することにより、上記コンピュータが予測装置１０１として機能する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる予測装置を実現することができる。
【符号の説明】
【０１３３】
１００ 目標追尾装置、１０１ 予測装置、１３０ 追尾部、１３１ 追尾フィルタ、１３２ モデル選択部、１４０ 運動状態判定部、１４１ バイアス補正部、１４２ 旋回判定部、１４３ 観測角速度位置予測部、１４４ 推定角速度位置予測部、１４５ 条件判定部、１４６ 角速度誤差推定部、１４７ 角速度補正部、１４８ 直線位置予測部、１５０ バイアス補正値メモリ、１６０ 運動性能記憶部、１７０ 地図記憶部、１８０ 運動状態外挿部、１９１ 最大角速度取得部、１９２ 異常値判定部、１９３ 閾値比較部、１９５ 回数判定部、１９６ 判定結果出力部、２１１ 推移確率記憶部、２１２ 推移確率更新部、２１３ モデル確率記憶部、２１４ モデル確率更新部、２１５ 重付平均算出部、２３１ 追尾装置、２４５ 条件判定装置、２４６ 角速度誤差推定装置、２４７ 角速度補正装置、２５０ 角速度誤差記憶装置、２８０ 予測位置算出装置、２９０ 衝突危険判定装置、６００ 移動体、６０６ 真の角速度、６１１ 観測位置、６１６ ジャイロ角速度、６２１，６２２ 推定位置、６２６ 追尾角速度、６３１〜６３３ 予測位置、８００ 追尾システム、８０１ 予測システム、８１０ 観測装置、８１１ 位置観測装置、８２０ ジャイロセンサ、８２１ 角速度観測装置。
【技術分野】
【０００１】
この発明は、移動する観測対象の将来の位置を予測する予測装置に関する。
【背景技術】
【０００２】
観測対象の将来の位置を予測するには、観測対象の現在の位置や速度などを精度よく推定する必要がある。
観測対象の位置を観測する方式には、レーダや全地球測位システム（ＧＰＳ）受信機などの位置観測装置を用いる方式がある。いずれの方式も観測誤差があるので、カルマンフィルタなどの追尾フィルタを用いて、観測対象の現在の位置や速度などを推定する技術がある。
カルマンフィルタなどの追尾フィルタは、観測対象が等速直線運動など所定の運動モデルにしたがって運動していることを前提とする場合がある。運動モデルが観測対象の実際の運動に合致していない場合は推定精度が低くなるので、複数の運動モデルを切り替えて観測対象の現在の位置や速度などを推定する技術がある。
また、観測対象の角速度を観測する方式には、ジャイロセンサなどの角速度観測装置を用いる方式がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平１１−１２０５００号公報
【特許文献２】特開２００５−２７４３００号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
観測対象の運動状態が変化した場合、追尾フィルタは一般に応答が遅いので、運動モデルをすぐに切り替えることはできない。角速度観測装置は一般に応答が速いが、観測誤差があるので、そのままでは、運動モデルの選択に用いることはできない。
この発明は、例えば、上記のような課題を解決するためになされたものであり、観測対象の将来の位置を高い精度で予測することを目的とする。
【課題を解決するための手段】
【０００５】
この発明にかかる予測装置は、位置観測装置が観測した観測対象の位置と、角速度観測装置が観測した上記観測対象の角速度とに基づいて、上記観測対象の将来の位置を予測する予測装置において、
上記予測装置は、角速度誤差推定装置と、角速度補正装置と、予測位置算出装置とを有し、
上記角速度誤差推定装置は、上記位置観測装置が観測した位置に基づいて、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度補正装置は、上記角速度誤差推定装置が推定した誤差に基づいて、上記角速度観測装置が観測した角速度を補正し、
上記予測位置算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記観測対象の将来の位置を予測することを特徴とする。
【発明の効果】
【０００６】
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【図面の簡単な説明】
【０００７】
【図１】実施の形態１における追尾システム８００の構成の一例を示すブロック構成図。
【図２】実施の形態１におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図。
【図３】実施の形態１における観測装置８１０が観測位置を観測する時刻と、ジャイロセンサ８２０がジャイロ角速度を観測する時刻との間の関係の一例を示す図。
【図４】実施の形態１における観測角速度位置予測部１４３及び推定角速度位置予測部１４４が予測する予測位置６３１，６３２の関係の一例を示す図。
【図５】実施の形態１における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の一例を示す図。
【図６】実施の形態１における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の別の例を示す図。
【図７】実施の形態１におけるバイアス補正処理Ｓ７１０の流れの一例を示すフローチャート図。
【図８】実施の形態１における旋回判定部１４２の構成の一例を示す詳細ブロック図。
【図９】実施の形態１における旋回判定部１４２の動作を説明するための図。
【図１０】実施の形態１におけるモデル選択部１３２の構成の一例を示す詳細ブロック図。
【図１１】実施の形態１における推移確率更新部２１２の動作の例を説明するための図。
【図１２】実施の形態１における推移確率更新部２１２が更新する推移確率の例を示す図。
【図１３】実施の形態１における運動状態外挿部１８０の動作を説明するための図。
【図１４】実施の形態２における追尾システム８００の構成の一例を示すブロック構成図。
【図１５】実施の形態３における追尾システム８００の構成の一例を示すブロック構成図。
【図１６】実施の形態３におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図。
【図１７】実施の形態３における観測角速度位置予測部１４３及び直線位置予測部１４８が予測する予測位置６３１，６３３の関係の一例を示す図。
【図１８】実施の形態４における予測システム８０１の構成の一例を示すシステム構成図。
【発明を実施するための形態】
【０００８】
実施の形態１．
実施の形態１について、図１〜図１３を用いて説明する。
【０００９】
図１は、この実施の形態における追尾システム８００の構成の一例を示すブロック構成図である。
【００１０】
追尾システム８００（予測システム）は、観測装置８１０（位置観測装置）を有する。観測装置８１０は、航空機、船舶、車両などの移動体（観測対象）の位置を観測する。観測装置８１０が観測した移動体の位置を「観測位置」と呼ぶ。観測装置８１０は、例えば、レーダ、ＧＰＳ受信機などのセンサである。観測装置８１０は、例えばレーダのように、移動体を外部から観測する装置であってもよいし、例えばＧＰＳ受信機のように、移動体に搭載された装置であってもよい。
【００１１】
追尾システム８００は、ジャイロセンサ８２０（角速度観測装置）を有する。ジャイロセンサ８２０は、観測対象である移動体に搭載されている。ジャイロセンサ８２０は、搭載された移動体の角速度を観測する。ジャイロセンサ８２０が観測した角速度を「ジャイロ角速度」（ＪＹＲＯ角速度）と呼ぶ。
【００１２】
追尾システム８００は、目標追尾装置１００（追尾部１３０、運動状態判定部１４０、バイアス補正値メモリ１５０、運動性能記憶部１６０、運動状態外挿部１８０）を有する。目標追尾装置１００は、観測装置８１０及びジャイロセンサ８２０の観測結果に基づいて、移動体の将来の位置を高精度に推定する。
【００１３】
目標追尾装置１００は、バイアス補正値メモリ１５０（角速度誤差記憶装置）を有する。バイアス補正値メモリ１５０は、後述するバイアス補正部１４１が算出したバイアス誤差を記憶する。
目標追尾装置１００は、運動性能記憶部１６０を有する。運動性能記憶部１６０は、移動体の最高速度、最大加速度、最大角速度など移動体の運動性能を記憶している。運動性能記憶部１６０は、例えば、移動体の種別と、その種別に属する移動体の運動性能との組を、データベース形式（運動性能データベース）で記憶している。
【００１４】
目標追尾装置１００（予測装置）は、追尾部１３０を有する。
追尾部１３０は、複数の追尾フィルタ１３１（追尾装置）を有する。なお、符号にアルファベット小文字を添えて「追尾フィルタ１３１ａ」「追尾フィルタ１３１ｂ」などと記載することにより、複数の追尾フィルタ１３１を区別する場合がある。
追尾フィルタ１３１は、所定の運動モデルに基づいて、追尾処理をする。すなわち、追尾フィルタ１３１は、移動体が所定の運動モデルにしたがって運動しているものと仮定して、観測装置８１０が観測した観測位置に基づいて、移動体の位置や速度などを推定する。以下、追尾フィルタ１３１が推定した移動体の位置や速度などの推定値を「追尾位置」「追尾速度」などと呼ぶ。追尾フィルタ１３１は、例えばカルマンフィルタ、アルファ−ベータ（α−β）フィルタ、アルファ−ベータ−ガンマ（α−β−γ）フィルタやその他のフィルタなどである。
各追尾フィルタ１３１ａ〜１３１ｃは、それぞれ異なる運動モデルを持っている。各追尾フィルタ１３１ａ〜１３１ｃは、それぞれが持つ運動モデルにしたがって移動体が運動しているものと仮定して、移動体の位置や速度などを推定する。例えば、追尾フィルタ１３１ａは、運動モデルとして旋回運動モデルを持っている。すなわち、追尾フィルタ１３１ａは、移動体が旋回運動をしているものと仮定して、移動体の位置・速度・角速度などを推定する。また、例えば、追尾フィルタ１３１ｂは、運動モデルとして等速直線運動モデルを持っている。すなわち、追尾フィルタ１３１ｂ（直線位置推定装置）は、移動体が等速直線運動をしているものと仮定して、追尾位置・速度などを推定する。また、例えば、追尾フィルタ１３１ｃは、運動モデルとして等加速度運動モデルを持っている。すなわち、追尾フィルタ１３１ｃは、移動体が等加速度運動をしているものと仮定して、移動体の位置・速度・加速度などを推定する。
なお、旋回運動モデルを持つ追尾フィルタ１３１は、移動体が所定の角速度で旋回運動をしていると仮定する構成（所定角速度位置推定装置）であってもよい。その場合、複数の追尾フィルタ１３１が旋回運動モデルを持ち、それぞれが異なる角速度を仮定する構成であってもよい。
等加速度運動モデルを持つ追尾フィルタ１３１も同様に、移動体が所定の加速度で等加速度運動をしていると仮定する構成であってもよい。その場合、複数の追尾フィルタ１３１が等加速度運動モデルを持ち、それぞれが異なる加速度を仮定する構成であってもよい。
【００１５】
追尾部１３０は、モデル選択部１３２（予測位置算出装置）を有する。モデル選択部１３２は、後述する旋回判定部１４２の判定結果などに基づいて、移動体の実際の運動に最もよく合致している運動モデルを選択する。モデル選択部１３２は，選択した運動モデルに基づいて、移動体の位置・速度・角速度・加速度などの推定値を算出する。以下、モデル選択部１３２が算出した移動体の位置・角速度などの推定値を「推定位置」「推定角速度」などと呼ぶ。
例えば、モデル選択部１３２は、複数の追尾フィルタ１３１がそれぞれ持っている運動モデルが、移動体の実際の運動に合致している確率を算出し、算出した確率に基づいて、各追尾フィルタ１３１ａ〜１３１ｃが推定した追尾位置などに重み付けをして合成し、推定位置を算出する。
あるいは、モデル選択部１３２は、複数の追尾フィルタ１３１のなかから、移動体の実際の運動に運動モデルが合致している確率が最も高い追尾フィルタ１３１を選択し、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとして採用する。
【００１６】
また、追尾フィルタ１３１は、推定した追尾位置や追尾速度などがどの程度の誤差を含むかを推定する。例えば、追尾フィルタ１３１は、追尾位置や追尾速度などの誤差分散や誤差共分散を算出する。
モデル選択部１３２は、追尾フィルタ１３１が算出した誤差分散や誤差共分散に基づいて、推定位置や推定速度などの誤差分散や誤差共分散を算出する。例えば、モデル選択部１３２は、複数の追尾フィルタ１３１が算出した誤差分散や誤差共分散を合成して、推定位置や推定速度などの誤差分散や誤差共分散を算出する。あるいは、モデル選択部１３２は、選択した追尾フィルタ１３１が算出した誤差分散や誤差共分散を、推定位置や推定速度などの誤差分散や誤差共分散として採用する。
【００１７】
目標追尾装置１００は、運動状態判定部１４０を有する。運動状態判定部１４０は、ジャイロセンサ８２０が観測したジャイロ角速度に基づいて、移動体の運動状態を判定する。
運動状態判定部１４０は、バイアス補正部１４１（角速度誤差推定装置および角速度補正装置）を有する。バイアス補正部１４１は、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差を推定する。バイアス補正部１４１は、推定したバイアス誤差に基づいて、ジャイロ角速度を補正し、補正したジャイロ角速度を出力する。バイアス補正部１４１は、例えば、追尾フィルタ１３１が推定した追尾角速度（ＴＲＫ角速度）、モデル選択部１３２が算出した推定角速度などに基づいて、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差を推定する。
運動状態判定部１４０は、旋回判定部１４２（旋回判定装置）を有する。旋回判定部１４２は、バイアス補正部１４１が補正したジャイロ角速度や、運動性能記憶部１６０が記憶した移動体の運動性能などにに基づいて、移動体が旋回しているか否かを判定する。
【００１８】
目標追尾装置１００は、運動状態外挿部１８０（予測位置算出装置）を有する。運動状態外挿部１８０は、移動体の将来の位置を予測する。運動状態外挿部１８０が予測した移動体の位置を「予測位置」と呼ぶ。運動状態外挿部１８０は、モデル選択部１３２が算出した推定位置・推定速度などや、旋回判定部１４２の判定結果などに基づいて、予測位置を算出する。
【００１９】
図２は、この実施の形態におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図である。
バイアス補正部１４１は、旋回運動モデルを持つ追尾フィルタ１３１ａが推定した追尾角速度を入力する。バイアス補正部１４１は、モデル選択部１３２が算出した推定位置・推定速度・誤差分散・誤差共分散を入力する。バイアス補正部１４１は、ジャイロセンサ８２０が観測したジャイロ角速度を入力する。なお、ジャイロセンサ８２０が観測したジャイロ角速度をバイアス補正部１４１が直接入力する代わりに、ジャイロセンサ８２０が観測したジャイロ角速度を平滑処理する平滑処理部（追尾フィルタ）を設け、平滑処理部が平滑処理したジャイロ角速度を、バイアス補正部１４１が入力する構成としてもよい。
【００２０】
バイアス補正部１４１は、観測角速度位置予測部１４３（観測角速度位置予測装置）を有する。観測角速度位置予測部１４３は、モデル選択部１３２が算出した推定位置や推定速度などに基づいて、移動体の将来の位置を予測する。ただし、観測角速度位置予測部１４３は、移動体の角速度として、モデル選択部１３２が算出した推定角速度ではなく、バイアス補正部１４１が入力したジャイロ角速度を用いる。観測角速度位置予測部１４３は、ジャイロ角速度に基づいて、移動体の将来の位置を予測する。
【００２１】
図３は、この実施の形態における観測装置８１０が観測位置を観測する時刻と、ジャイロセンサ８２０がジャイロ角速度を観測する時刻との間の関係の一例を示す図である。
一般に、ジャイロセンサ８２０の観測間隔は、観測装置８１０の観測間隔より短い。ある時点において、ジャイロセンサ８２０が最も新しいジャイロ角速度を観測した時刻をｔ１とし、観測装置８１０が最も新しい観測位置を観測した時刻をｔ２とする。一般に、時刻ｔ１と時刻ｔ２とは一致せず、時刻ｔ１のほうが時刻ｔ２よりも後の時刻である。
追尾フィルタ１３１は、観測装置８１０が観測した観測位置に基づいて、追尾位置などを推定するので、追尾フィルタ１３１が推定した追尾位置などは、時刻ｔ２におけるものである。モデル選択部１３２が算出した推定位置なども、同様に、時刻ｔ２におけるものである。
モデル選択部１３２が算出した推定位置などと、ジャイロセンサ８２０が観測したジャイロ角速度とを組み合わせて、移動体の将来の位置を予測するためには、推定位置などの時刻と、ジャイロ角速度の時刻とが一致している必要がある。
【００２２】
このため、観測角速度位置予測部１４３は、例えば、時刻ｔ２における推定位置などに基づいて外挿処理を行い、時刻ｔ１における推定位置などを算出する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ１における推定位置などを算出する。
【数１１】
ただし、山型付きのｘ（文中において「ｘ＾」と表記する。）は、推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。推定値ベクトルｘ＾は、時刻ｔの関数である。推定値ベクトルｘ＾の要素は、所定の座標系で表した時刻ｔにおける推定位置の座標などである。推定値ベクトルｘ＾の次数ｋは、推定値の数である。例えば、３次元座標で表された位置および速度を推定値とする場合、次数ｋは６である。
Ｆは、状態遷移行列である。状態遷移行列Ｆは、ｋ次の正方行列である。状態遷移行列Ｆは、時間Δｔの関数である。状態遷移行列Ｆは、所定の運動モデルにおいて、ある時刻における移動体の位置などを表わすベクトルを、時間Δｔが経過した後における移動体の位置などを表わすベクトルへ写す写像を表わす。
すなわち、この式は、時刻ｔ２における推定値ベクトルｘ＾（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を作用させて、時刻ｔ１における推定値ベクトルｘ＾（ｔ１）を算出することを意味している。
なお、状態遷移行列Ｆの運動モデルは、モデル選択部１３２が選択した追尾フィルタ１３１の運動モデルであってもよいし、等速直線運動モデルや旋回運動モデルなどあらかじめ定めた所定の運動モデルであってもよい。通常、時刻ｔ１と時刻ｔ２との差は十分小さいので、運動モデルの差による違いはほとんどない。
【００２３】
観測角速度位置予測部１４３は、算出した時刻ｔ１における推定位置などに基づいて、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ１から所定の時間ΔＴが経過した時刻ｔ３（すなわち、ｔ３＝ｔ１＋ΔＴ）における移動体の位置などを予測する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ３における移動体の位置などを算出する。
【数１２】
ただし、ｘ＾Ｊは、時刻ｔ１におけるジャイロ角速度で推定角速度を置き換えた推定値ベクトルである。ＦＪは、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。
すなわち、この式は、時刻ｔ１における推定値ベクトルｘ＾Ｊ（ｔ１）に、移動体が時刻ｔ１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪを作用させて、時刻ｔ３における推定値ベクトルｘ＾Ｊ（ｔ３）を算出することを意味している。
【００２４】
また、観測角速度位置予測部１４３は、モデル選択部１３２が算出した誤差分散や誤差共分散に基づいて、予測した移動体の将来の位置の誤差分散や誤差共分散を推定する。
【００２５】
推定位置などと同様、誤差分散などの時刻とジャイロ角速度の時刻とを一致させるため、観測角速度位置予測部１４３は、例えば、時刻ｔ２における誤差分散などに基づいて、時刻ｔ１における誤差分散などを算出する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ１における誤差分散などを算出する。
【数１３】
ただし、Ｐは、推定値ベクトルｘ＾の誤差の分散共分散行列である。分散共分散行列Ｐは、ｋ次の正方行列である。分散共分散行列Ｐは、時刻ｔの関数である。分散共分散行列Ｐの要素は、時刻ｔにおける推定値ベクトルの各要素の誤差の間の分散または共分散である。
上付きのＴは、行列の転置を表わす。
Ｑは、システム雑音の分散共分散行列である。分散共分散行列Ｑは、ｋ次の正方行列である。分散共分散行列Ｑは、時間Δｔの関数である。分散共分散行列Ｑの要素は、時間Δｔが経過する間に発生するシステム雑音の分散または共分散である。
すなわち、この式は、時刻ｔ２における推定誤差の分散共分散行列Ｐ（ｔ２）に、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）の転置行列を左から作用させ、時間（ｔ１−ｔ２）の経過を表わす状態遷移行列Ｆ（ｔ１−ｔ２）を右から作用させたものに、時間（ｔ１−ｔ２）の経過により発生したシステム雑音の分散共分散行列Ｑ（ｔ１−ｔ２）を加えて、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）を算出することを意味している。
【００２６】
観測角速度位置予測部１４３は、算出した時刻ｔ１における誤差分散などに基づいて、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルにしたがい、時刻ｔ３における誤差分散などを算出する。
例えば、観測角速度位置予測部１４３は、次の式の右辺を計算することにより、時刻ｔ３における誤差分散などを算出する。
【数１４】
ただし、ＰＪは、時刻ｔ１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
この式は、時刻ｔ１における推定誤差の分散共分散行列Ｐ（ｔ１）に、移動体が時刻ｔ１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）の転置行列を左から作用させ、移動体が時刻ｔ１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列ＦＪ（ΔＴ）を右から作用させたものに、時間ΔＴの経過により発生したシステム雑音の分散共分散行列Ｑ（ΔＴ）を加えて、時刻ｔ３についての予測誤差の分散共分散行列ＰＪ（ｔ３）を算出することを意味している。
【００２７】
観測角速度位置予測部１４３は、例えば上記のようにして、時刻ｔ３における移動体の位置や誤差分散などを予測する。
なお、観測角速度位置予測部１４３は、バイアス補正部１４１が入力したジャイロ角速度ではなく、後述する角速度補正部１４７が補正したジャイロ角速度に基づいて、移動体の位置や誤差分散などを予測する構成であってもよい。
【００２８】
バイアス補正部１４１は、推定角速度位置予測部１４４（推定角速度位置予測装置）を有する。推定角速度位置予測部１４４は、モデル選択部１３２が算出した推定位置や推定速度などに基づいて、移動体の将来の位置を予測する。ただし、推定角速度位置予測部１４４は、移動体の角速度として、モデル選択部１３２が算出した推定角速度ではなく、追尾フィルタ１３１ａが推定した追尾角速度を用いる。推定角速度位置予測部１４４は、追尾角速度に基づいて、移動体の将来の位置を予測する。
また、推定角速度位置予測部１４４は、モデル選択部１３２が算出した誤差分散や誤差共分散に基づいて、予測した移動体の将来の位置の誤差分散や誤差共分散を推定する。
すなわち、観測角速度位置予測部１４３が推定角速度としてジャイロ角速度を用いるのに対し、推定角速度位置予測部１４４は、推定角速度として追尾角速度を用いて、観測角速度位置予測部１４３と同様の処理をする。なお、追尾フィルタ１３１ａが推定した追尾角速度の時刻と、モデル選択部１３２が算出した推定位置などの時刻とは、同じであるから、時刻を一致させるための計算は不要である。
【００２９】
推定角速度位置予測部１４４は、例えば、次の式の右辺を計算することにより、時刻ｔ３における推定位置や誤差分散などを算出する。
【数１５】
ただし、ｘ＾Ｔは、時刻ｔ２における追尾角速度で推定角速度を置き換えた推定値ベクトルである。ＦＴは、時刻ｔ２における追尾角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。ＰＴは、時刻ｔ２における追尾角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
【００３０】
推定角速度位置予測部１４４は、例えば上記のようにして、移動体の位置や誤差分散などを予測する。
なお、推定角速度位置予測部１４４は、追尾フィルタ１３１ａが推定した追尾角速度ではなく、モデル選択部１３２が算出した推定角速度に基づいて（すなわち、推定角速度を追尾角速度で置き換えることなくそのまま用いて）、移動体の位置や誤差分散などを予測する構成であってもよい。
【００３１】
バイアス補正部１４１は、条件判定部１４５（条件判定装置）を有する。条件判定部１４５は、ジャイロセンサ８２０が観測したジャイロ角速度の誤差を推定する条件（所定の条件）が満たされているか否かを判定する。
【００３２】
図４は、この実施の形態における観測角速度位置予測部１４３及び推定角速度位置予測部１４４が予測する予測位置６３１，６３２の関係の一例を示す図である。
観測位置６１１ａ〜６１１ｄは、時刻ｔ１までの時点で、観測装置８１０が観測した移動体６００の観測位置を表わす。このうち、観測位置６１１ｄは、時刻ｔ２に観測した最も新しい観測位置である。
追尾部１３０は、観測位置６１１ａ〜６１１ｄに基づいて、時刻ｔ２における移動体６００の推定位置６２１を算出する。
観測角速度位置予測部１４３は、追尾部１３０が推定した推定位置６２１などに基づいて、時刻ｔ１における移動体６００の推定位置６２２などを算出する。
観測角速度位置予測部１４３は、算出した推定位置６２２などと、ジャイロ角速度とに基づいて、時刻ｔ３における移動体６００の予測位置６３１を算出する。
推定角速度位置予測部１４４は、追尾部１３０が推定した推定位置６２１などと、追尾フィルタ１３１ａが推定した追尾角速度とに基づいて、時刻ｔ３における移動体６００の予測位置６３２を算出する。
【００３３】
観測装置８１０が観測した観測位置やジャイロセンサ８２０が観測したジャイロ角速度に誤差がなければ、ジャイロ角速度と追尾角速度とは一致する。したがって、観測角速度位置予測部１４３が予測した移動体６００の予測位置６３１と推定角速度位置予測部１４４が予測した移動体６００の予測位置６３２とは一致するはずである。
しかし、実際には、誤差の影響により、予測位置６３１と予測位置６３２とは一致しない。
【００３４】
ジャイロセンサ８２０が観測したジャイロ角速度には、ランダムな誤差と、時刻にかかわらずほぼ一定なバイアス誤差とが含まれる。また、モデル選択部１３２が推定した推定位置などにも、ランダムな誤差が含まれる。
このうち、ランダムな誤差の影響は、観測角速度位置予測部１４３や推定角速度位置予測部１４４が予測した誤差分散などによりある程度予想できる。
したがって、誤差分散などにより予想される範囲を超えて、予測した予測位置６３１と予測位置６３２とが異なる場合には、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差の影響によるものと考えられる。
【００３５】
条件判定部１４５は、例えば、観測角速度位置予測部１４３が予測した予測位置６３１と推定角速度位置予測部１４４が予測した予測位置６３２との差が、予想される範囲内であるか否かを判定し、予想される範囲を超えている場合に、ジャイロ角速度の誤差を推定する条件が満たされていると判定する。
この判定には、統計学上の手法を用いることができる。条件判定部１４５は、例えばカイ二乗検定により、予測位置６３１と予測位置６３２との差が、予想される範囲内であるか否かを判定する。例えば、条件判定部１４５は、次の式の右辺を計算することにより、検定値を算出する。
【数１６】
ただし、εは、検定値である。
条件判定部１４５は、算出した検定値εを、所定の閾値εｔｈと比較する。閾値εｔｈは、所定の有意水準に基づいて、例えばカイ二乗分布表から求めたものである。検定値εが閾値εｔｈ以下である場合、条件判定部１４５は、予測位置６３１と予測位置６３２との差が、予想される範囲内であると判定する。検定値εが閾値εｔｈより大きい場合、条件判定部１４５は、予測位置６３１と予測位置６３２との差が、予想される範囲を超えていると判定する。
【００３６】
図５は、この実施の形態における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の一例を示す図である。
横軸は、時刻ｔを表わす。縦軸は、角速度ωを表わす。
例えば、移動体６００が、直進運動をしたのち、ある時刻を境に、旋回運動に移ったとする。
移動体６００の真の角速度６０６は、直進運動中は０であり、旋回運動中はある値で一定であるものとする。
ジャイロセンサ８２０にバイアス誤差がない場合であっても、それ以外の誤差要因により、ジャイロセンサ８２０が観測するジャイロ角速度６１６は、真の角速度６０６と完全には一致しない。しかし、ジャイロ角速度６１６は、真の角速度６０６の変化に遅れることなく、追随して変化する。
これに対し、追尾フィルタ１３１ａが推定する追尾角速度６２６は、算出間隔が、ジャイロ角速度６１６の観測間隔よりも長く、ランダムな誤差の影響が大きい。また、追尾角速度６２６は、真の角速度６０６の変化に対して、すぐには追随せず、ゆっくりと変化する。したがって、真の角速度６０６が変化した直後は、過渡応答により、真の角速度６０６と追尾角速度６２６との差が大きくなる。
【００３７】
このように、旋回時には、追尾角速度の過渡応答による差が生じるので、予測位置６３１と予測位置６３２との差が、予想される範囲を超えていると条件判定部１４５が判定する場合がある。このため、条件判定部１４５は、バイアス補正区間を設定する。バイアス補正区間とは、時系列的に並べられた複数の判定結果のうち、最新の判定結果を含む連続したＮ回分の判定結果（Ｎは所定の正の整数。）のことである。条件判定部１４５は、バイアス補正区間のなかで、予測位置６３１と予測位置６３２との差が予想される範囲を超えていると判定した回数を数える。条件判定部１４５は、この回数がＭ回以上（Ｍは、Ｎ以下の所定の正の整数。）である場合に、一定のバイアスが生じているものとみなし、バイアス誤差を推定する条件が満たされていると判定する。
【００３８】
バイアス補正部１４１は、角速度誤差推定部１４６（角速度誤差推定装置）を有する。角速度誤差推定部１４６は、バイアス誤差を推定する条件が満たされていると条件判定部１４５が判定した場合に、ジャイロセンサ８２０が観測したジャイロ角速度の誤差を推定する。
【００３９】
角速度誤差推定部１４６は、例えば、カルマンフィルタを用いて、ジャイロセンサ８２０のバイアス誤差を推定する。角速度誤差推定部１４６は、例えば、次の式で定義される観測モデルを用いる。
【数１７】
ただし、Δωｋは、時刻ｋにおけるバイアス誤差の真の値を表わす。ｚｋは、時刻ｋにおけるバイアス誤差の観測値を表わす。ｖｋは、時刻ｋにおけるバイアス誤差の観測誤差を表わす。ωＪ，ｋは、時刻ｋにおいてジャイロセンサ８２０が観測したジャイロ角速度を表わす。ωＴ，ｋは、時刻ｋについて追尾フィルタ１３１ａが推定した追尾角速度を表わす。ｖＪ，ｋは、時刻ｋにおいてジャイロセンサ８２０が観測したジャイロ角速度の観測誤差を表わす。ｖＴ，ｋは、時刻ｋについて追尾フィルタ１３１ａが推定した追尾角速度の推定誤差を表わす。
【００４０】
また、角速度誤差推定部１４６は、例えば、次の式で定義される状態方程式を用いる。
【数１８】
ただし、ｗｋは、時刻ｋ−１から時刻ｋまでの間に発生するシステム雑音を表わす実数である。
【００４１】
図６は、この実施の形態における真の角速度６０６とジャイロ角速度６１６と追尾角速度６２６との関係の別の例を示す図である。
この例において、ジャイロセンサ８２０が観測するジャイロ角速度６１６は、バイアス誤差Δωを含む。バイアス誤差Δωは、時刻にかかわらず、基本的に不変である。
【００４２】
角速度誤差推定部１４６は、例えば上記のようにして、バイアス誤差Δωを推定する。
バイアス補正値メモリ１５０は、角速度誤差推定部１４６が推定したバイアス誤差Δωを記憶する。
【００４３】
角速度補正部１４７は、バイアス補正値メモリ１５０が記憶したバイアス誤差Δωを用いて、バイアス補正部１４１が入力したジャイロ角速度ωＪを補正する。例えば、角速度補正部１４７は、ジャイロ角速度ωＪからバイアス誤差Δωを差し引いた差（ωＪ−Δω）を計算することにより、補正したジャイロ角速度を得る。
角速度誤差推定部１４６がバイアス誤差Δωを推定した場合、角速度誤差推定部１４６が推定したバイアス誤差Δωをバイアス補正値メモリ１５０が記憶する。角速度補正部１４７は、そのバイアス誤差Δωを用いて、ジャイロ角速度ωＪの誤差を補正する。
角速度誤差推定部１４６がバイアス誤差Δωを推定しなかった場合は、角速度誤差推定部１４６が以前に推定したバイアス誤差Δωをバイアス補正値メモリ１５０が記憶している。角速度補正部１４７は、角速度誤差推定部１４６が以前に推定したバイアス誤差Δωを用いて、ジャイロ角速度ωＪの誤差を補正する。
角速度補正部１４７は、補正したジャイロ角速度を出力する。
【００４４】
図７は、この実施の形態におけるバイアス補正処理Ｓ７１０の流れの一例を示すフローチャート図である。
バイアス補正処理Ｓ７１０において、バイアス補正部１４１は、ジャイロ角速度のバイアス誤差を補正する。
【００４５】
バイアス補正処理Ｓ７１０は、予測位置算出工程Ｓ７１１を有する。予測位置算出工程Ｓ７１１は、バイアス補正処理Ｓ７１０の最初に実行される。
予測位置算出工程Ｓ７１１において、観測角速度位置予測部１４３は、ジャイロ角速度ωＪに基づいて、予測位置や予測誤差分散などを算出する。また、推定角速度位置予測部１４４は、追尾角速度ωＴに基づいて、予測位置や予測誤差分散などを算出する。
【００４６】
バイアス補正処理Ｓ７１０は、条件判定工程Ｓ７１２を有する。条件判定工程Ｓ７１２は、予測位置算出工程Ｓ７１１のあとに実行される。
条件判定工程Ｓ７１２において、条件判定部１４５は、予測位置算出工程Ｓ７１１で観測角速度位置予測部１４３及び推定角速度位置予測部１４４が算出した予測位置や予測誤差分散などに基づいて、検定値εを算出する。条件判定部１４５は、算出した検定値εを閾値εｔｈと比較して、判定式を満たすか否かを判定する。条件判定部１４５は、判定結果を記憶する。条件判定部１４５は、記憶した判定結果のなかから、今回の判定結果を含む過去Ｎ回分の判定結果を取得して、判定式を満たさない（すなわち、検定値εが閾値εｔｈより大きい）と判定した回数を数える。条件判定部１４５は、数えた回数を整数Ｍと比較する。条件判定部１４５は、比較の結果に基づいて、条件分岐する。
【００４７】
バイアス補正処理Ｓ７１０は、バイアス誤差推定処理Ｓ７１３を有する。バイアス誤差推定処理Ｓ７１３は、条件判定工程Ｓ７１２において、判定式を満たさなかった回数がＭ回以上であると条件判定部１４５が判定した場合に実行される。
バイアス誤差推定処理Ｓ７１３において、角速度誤差推定部１４６は、ジャイロ角速度ωＪのバイアス誤差Δωを推定する。
【００４８】
バイアス補正処理Ｓ７１０は、バイアス誤差記憶工程Ｓ７１４を有する。バイアス誤差記憶工程Ｓ７１４は、バイアス誤差推定処理Ｓ７１３のあとに実行される。
バイアス誤差記憶工程Ｓ７１４において、バイアス補正値メモリ１５０は、バイアス誤差推定処理Ｓ７１３で角速度誤差推定部１４６が推定したバイアス誤差Δωを記憶する。
【００４９】
バイアス補正処理Ｓ７１０は、バイアス誤差読出工程Ｓ７１５を有する。バイアス誤差読出工程Ｓ７１５は、条件判定工程Ｓ７１２において、判定式を満たさなかった回数がＭ回未満であると条件判定部１４５が判定した場合に実行される。
バイアス誤差読出工程Ｓ７１５において、角速度補正部１４７は、バイアス補正値メモリ１５０が記憶したバイアス誤差Δωを読み出す。
【００５０】
バイアス補正処理Ｓ７１０は、ジャイロ角速度補正工程Ｓ７１６を有する。ジャイロ角速度補正工程Ｓ７１６は、バイアス誤差記憶工程Ｓ７１４のあと及びバイアス誤差読出工程Ｓ７１５のあとに実行される。
ジャイロ角速度補正工程Ｓ７１６において、角速度補正部１４７は、バイアス誤差推定処理Ｓ７１３で角速度誤差推定部１４６が推定したバイアス誤差Δωあるいは角速度誤差推定部１４６で読み出したバイアス誤差Δωに基づいて、ジャイロ角速度ωＪを補正する。
【００５１】
バイアス補正処理Ｓ７１０は、ジャイロ角速度補正工程Ｓ７１６までで終わりとなる。
【００５２】
図８は、この実施の形態における旋回判定部１４２の構成の一例を示す詳細ブロック図である。
【００５３】
旋回判定部１４２は、最大角速度取得部１９１を有する。最大角速度取得部１９１は、運動性能記憶部１６０が記憶した移動体の運動性能のなかから、移動体の性能限界値である最大の角速度ωｍａｘを取得する。
【００５４】
旋回判定部１４２は、異常値判定部１９２を有する。異常値判定部１９２は、最大角速度取得部１９１が取得した角速度ωｍａｘに基づいて、バイアス補正部１４１が出力した（補正済の）ジャイロ角速度ωＪが異常値であるか否かを判定する。例えば、異常値判定部１９２は、ジャイロ角速度ωＪの絶対値が角速度ωｍａｘより大きい場合に、ジャイロ角速度ωＪが異常値であると判定する。異常値判定部１９２は、異常値であると判定したジャイロ角速度ωＪを除去し、異常値でないと判定したジャイロ角速度ωＪだけを出力する。
【００５５】
旋回判定部１４２は、閾値比較部１９３を有する。閾値比較部１９３は、異常値判定部１９２が出力したジャイロ角速度ωＪを、所定の閾値ωｍｉｎと比較して、どちらが大きいか判定する。
なお、閾値ωｍｉｎは、あらかじめ定めた定数である構成でもよいし、ジャイロ角速度の誤差の標準偏差を算出し、算出した標準偏差（もしくはその定数倍）を閾値ωｍｉｎとする構成でもよい。
【００５６】
旋回判定部１４２は、回数判定部１９５を有する。回数判定部１９５は、ジャイロ角速度ωＪが閾値ωｍｉｎ以上であると閾値比較部１９３が連続して判定した回数を算出する。
例えば、回数判定部１９５は、連続判定回数を記憶しておく。ジャイロ角速度ωＪが閾値ωｍｉｎ以上であると閾値比較部１９３が判定した場合、回数判定部１９５は、記憶した連続判定回数を１増加させる。ジャイロ角速度ωＪが閾値ωｍｉｎ未満であると閾値比較部１９３が判定した場合、回数判定部１９５は、記憶した連続判定回数を０に初期化する。
回数判定部１９５は、算出した連続判定回数を所定の整数Ｍ’と比較して、どちらが大きいか判定する。連続判定回数が整数Ｍ’以上である場合、旋回判定部１４２は移動体が旋回していると判定する。
【００５７】
旋回判定部１４２は、判定結果出力部１９６を有する。連続判定回数が整数Ｍ’以上であると回数判定部１９５が判定した場合、判定結果出力部１９６は、移動体が旋回しているという判定結果を出力する。なお、判定結果出力部１９６は、異常値判定部１９２が出力したジャイロ角速度を移動体の角速度として出力することにより、移動体が旋回していると判定した判定結果を表わす構成としてもよい。移動体が旋回していないと判定した場合は、ジャイロ角速度を出力しないことにより、その判定結果を表わす構成でもよいし、移動体の角速度として０を出力することにより、移動体が旋回していないと判定した判定結果を表わす構成でもよい。
【００５８】
図９は、この実施の形態における旋回判定部１４２の動作を説明するための図である。
ジャイロ角速度ωＪが角速度ωｍａｘより大きい場合は、異常値判定部１９２が除去し、その時刻におけるジャイロ角速度は、欠測扱いとなる。閾値比較部１９３は、異常値判定部１９２が出力したジャイロ角速度を閾値ωｍｉｎと比較する。閾値比較部１９３の比較結果に基づいて、回数判定部１９５が連続判定回数を算出し、判定結果出力部１９６が出力する判定結果が定まる。なお、異常値判定部１９２が除去した場合も含めてジャイロ角速度が欠測の場合は、閾値比較部１９３が比較そのものを行わないので、回数判定部１９５による連続判定回数の算出には影響しない。
【００５９】
図１０は、この実施の形態におけるモデル選択部１３２の構成の一例を示す詳細ブロック図である。
【００６０】
モデル選択部１３２は、モデル確率記憶部２１３を有する。モデル確率記憶部２１３は、追尾フィルタ１３１ａ〜１３１ｃそれぞれについて、その追尾フィルタ１３１が持っている運動モデルが、移動体の実際の運動に合致している確率を記憶している。モデル確率記憶部２１３が記憶している確率を「モデル確率」と呼ぶ。
【００６１】
モデル選択部１３２は、重付平均算出部２１５を有する。重付平均算出部２１５は、モデル確率記憶部２１３が記憶したモデル確率に基づいて、各追尾フィルタ１３１ａ〜１３１ｃが推定した追尾位置などを合成して、推定位置などを算出する。重付平均算出部２１５は、例えば、次の式の右辺を計算することにより、推定位置などを算出する。
【数１９】
ただし、ｘ＾は、重付平均算出部２１５が算出する推定位置などを表わす推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。Ｘ＾は、追尾値行列である。追尾値行列Ｘ＾は、ｋ行ｊ列の行列である。追尾値行列Ｘ＾のｉ列目（ｉは１以上ｊ以下の整数。）は、ｉ番目の追尾フィルタ１３１の追尾値ベクトルｘ＾ｉである。追尾値行列Ｘ＾の列数ｊは、運動モデルの数である。追尾値ベクトルｘ＾ｉは、ｋ次の列ベクトルである。追尾値ベクトルの各要素は、その追尾フィルタ１３１が推定した推定位置の座標などである。ｐは、モデル確率ベクトルである。モデル確率ベクトルは、ｊ次の列ベクトルである。モデル確率ベクトルのｉ行目の要素は、ｉ番目の追尾フィルタ１３１が持っている運動モデルのモデル確率である。
この式は、各運動モデルの追尾値を、モデル確率により重み付けして合成して、推定値を算出することを意味する。
【００６２】
モデル選択部１３２は、推移確率記憶部２１１を有する。推移確率記憶部２１１は、２つの追尾フィルタ１３１のペアそれぞれについて、移動体の実際の運動が、第一の追尾フィルタ１３１が持っている運動モデルに合致している状態から、第二の追尾フィルタ１３１が持っている運動モデルに合致している状態へ推移する確率（モデル間の推移確率パラメータ）を記憶している。推移確率記憶部２１１が記憶している確率を「推移確率」と呼ぶ。
【００６３】
モデル選択部１３２は、モデル確率更新部２１４（重み算出装置）を有する。モデル確率更新部２１４は、推移確率記憶部２１１が記憶した推移確率に基づいて、モデル確率記憶部２１３が記憶したモデル確率を更新する。モデル確率更新部２１４は、例えば、次の式の右辺を計算することにより、モデル確率を更新する。
【数２０】
ただし、ＰＴ，ｋは、時刻ｋにおける推移確率行列である。推移確率行列Ｐは、ｊ次の正方行列である。推移確率行列ＰＴ，ｋのａ行ｂ列の要素（ａ及びｂは、１以上ｊ以下の整数。）は、ｂ番目の追尾フィルタ１３１からａ番目の追尾フィルタ１３１への推移確率である。なお、ａとｂとが等しい場合は、その追尾フィルタ１３１の運動モデルが維持される確率を表わす。推移確率行列ＰＴ，ｋの次数ｊは、運動モデルの数である。
【００６４】
モデル選択部１３２は、推移確率更新部２１２を有する。推移確率更新部２１２は、運動状態判定部１４０が判定した運動状態に基づいて、推移確率記憶部２１１が記憶した推移確率を更新する。
【００６５】
図１１は、この実施の形態における推移確率更新部２１２の動作の例を説明するための図である。
推移確率更新部２１２は、旋回判定部１４２が出力したジャイロ角速度を入力する。推移確率更新部２１２は、入力したジャイロ角速度を、所定の累積区間において積分する。
【００６６】
図１２は、この実施の形態における推移確率更新部２１２が更新する推移確率の例を示す図である。
例えば、運動モデルが２つあり、運動モデルＡは等速直線運動モデル、運動モデルＢは旋回運動モデルであるとする。推移確率更新部２１２は、例えば、ジャイロ角速度を積分した値を用いて、運動モデルＡから運動モデルＢへの推移確率ｐＡ→Ｂを段階的に変化させる。時間方向の累積値を用いることにより、ジャイロ角速度のデータが欠落した場合に、推移確率が急激に変動するのを防ぐ。推移確率更新部２１２は、例えば、次の式の右辺を計算することにより、推移確率ｐＡ→Ｂを算出する。
【数２１】
ただし、ｐｍｉｎは、推移確率の下限値である。ｐｍａｘは、推移確率の上限値である。下限値ｐｍｉｎ及び上限値ｐｍａｘは、あらかじめ定めた０以上１以下の実数である。
この式によれば、累積区間におけるジャイロ角速度ωＪの合計が、移動体の性能限界値である角速度の最大値ωｍａｘ以上の場合、推移確率更新部２１２は、推移確率ｐＡ→Ｂを上限値ｐｍａｘとする。また、累積区間におけるジャイロ角速度ωＪの合計が０の場合、推移確率更新部２１２は、推移確率ｐＡ→Ｂを下限値ｐｍｉｎとする。累積区間におけるジャイロ角速度ωＪの合計が０と最大値ωｍａｘとの間である場合、推移確率更新部２１２は、推移確率ｐＡ→Ｂを、下限値ｐｍｉｎと上限値ｐｍａｘとの間の値とする。
【００６７】
なお、モデル選択部１３２は、追尾フィルタ１３１が推定した追尾位置などを重み付け平均して、推定位置などを算出するのではなく、追尾フィルタ１３１のなかから１つを選択して、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとする構成であってもよい。
例えば、重付平均算出部２１５に代えて、フィルタ選択部を設ける。フィルタ選択部は、モデル確率記憶部２１３が記憶したモデル確率に基づいて、モデル確率が最も高い運動モデルを持つ追尾フィルタ１３１を選択する。フィルタ選択部は、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとして出力する。
モデル選択部１３２は、更に、簡略化した構成として、推移確率パラメータを用いずにモデルを切り替える構成であってもよい。例えば、モデル選択部１３２は、累積区間の間に旋回判定部１４２が判定した判定結果に基づいて、移動体が旋回していると判定した回数の割合を算出する。モデル選択部１３２は、算出した割合が所定の割合を超える場合、旋回運動モデルを持つ追尾フィルタ１３１を選択し、算出した割合が所定の割合未満の場合、等速直線運動モデルを持つ追尾フィルタ１３１を選択する。モデル選択部１３２は、選択した追尾フィルタ１３１が推定した追尾位置などを、推定位置などとして出力する。
【００６８】
運動状態外挿部１８０は、モデル選択部１３２が算出した推定位置などと、旋回判定部１４２が出力したジャイロ角速度ωＪとに基づいて、移動体の将来の位置を予測する。運動状態外挿部１８０は、例えば、次の式の右辺を計算することにより、時刻ｔｐにおける移動体の位置などの予測値を算出する。
【数２２】
ただし、Ｆ０は、移動体が等速直線運動をしているという運動モデルにおける状態遷移行列である。ＦＪは、移動体が角速度ωＪで旋回運動をしているという運動モデルにおける状態遷移行列である。ｔ２は、モデル選択部１３２が算出した最新の推定位置などの時刻である。
この式は、追尾部１３０が算出した最新の推定位置などに基づいて、移動体が旋回していないと旋回判定部１４２が判定した場合は、等速直線運動モデルを用いて移動体の運動状態を外挿し、移動体が旋回していると旋回判定部１４２が判定した場合は、旋回モデルを用いて移動体の運動状態を外挿することにより、移動体の位置を予測することを意味する。
【００６９】
図１３は、この実施の形態における運動状態外挿部１８０の動作を説明するための図である。
【００７０】
旋回判定部１４２は、ジャイロセンサ８２０がジャイロ角速度を観測するたびに、移動体が旋回しているか否かを判定するので、移動体が旋回しているか否かを旋回判定部１４２が判定する間隔は、一般に、モデル選択部１３２が推定位置などを算出する間隔よりも短い。このため、モデル選択部１３２が算出した最新の推定位置などの時刻ｔ２よりも後に、旋回判定部１４２による判定が変わる場合がある。例えば、この図のように、時刻ｔ２において、移動体が旋回していないと旋回判定部１４２が判定していたが、時刻ｔ１以降は、移動体が旋回していると旋回判定部１４２が判定したとする。その場合、運動状態外挿部１８０は、例えば、次の式の右辺を計算することにより、時刻ｔｐにおける移動体の位置などを予測する。
【数２３】
この式は、追尾部１３０が算出した時刻ｔ２における推定位置などに基づいて、等速直線運動モデルを用いて時刻ｔ１における推定位置などを算出し、算出した時刻ｔ１の推定位置などに基づいて、旋回運動モデルを用いて時刻ｔｐにおける推定することを意味している。
【００７１】
なお、ジャイロ角速度のランダムな観測誤差を取り除くため、カルマンフィルタなどによって平滑化したジャイロ角速度に基づいて、運動状態外挿部１８０は、移動体の位置を予測する構成であってもよい。
【００７２】
目標追尾装置１００は、複数の異なる運動モデルで構成される追尾フィルタ１３１を用いて目標（移動体）の観測値から推定値を算出し、ジャイロの角速度から運動状態を判別して、運動状態に基づいて目標の将来位置を予測する。
運動状態判定部１４０は、ジャイロの角速度を追尾部１３０で算出した角速度を用いてバイアス補正し、バイアス補正後の角速度を用いて運動状態を判別する。
追尾部１３０は、運動状態から適切な運動モデルを選択し、推定値を算出する。
運動状態外挿部１８０は、推定値と運動状態に基づいて時間外挿する。
センサ観測位置から算出した角速度を用いて、ジャイロの角速度に生じるバイアス誤差を除去し、補正後の角速度を用いることで、より早く目標の運動変化を検出し、目標の運動に応じた適切な運動モデルに基づく精度の高い予測をすることができる。
【００７３】
運動状態判定部１４０は、追尾部１３０から得られる推定位置からジャイロの角速度および追尾部の角速度のそれぞれで時間外挿して算出した予測値の差分を予測誤差共分散行列を用いたカイ二乗判定によって、同一とみなされない場合はジャイロの角速度に含まれるバイアス誤差推定を行う。
【００７４】
運動状態判定部１４０は、ジャイロの角速度に含まれるバイアス誤差をカルマンフィルタによって推定する。
【００７５】
運動状態判定部１４０は、ジャイロの角速度の中で運動性能データベースの性能限界値を超える値を除去する。
【００７６】
運動状態判定部１４０は、ジャイロの角速度の観測時刻と追尾部の推定時刻が異なる場合、観測時刻と推定時刻の差を時間外挿することで、ジャイロ角速度の観測時刻に追尾推定値の時刻を合わせる。
【００７７】
追尾部１３０は、運動状態判定部１４０で算出された角速度を用いてモデル推移確率を変更する。
【００７８】
追尾部１３０は、運動状態判定部１４０で算出された角速度が大きい場合は旋回運動モデルもしくは加速度運動モデルへ変更する。
【００７９】
運動状態外挿部１８０は、運動状態判定部１４０で算出された角速度を用いて時間外挿した位置を出力する。
【００８０】
運動状態外挿部１８０は、運動状態判定部１４０で算出された角速度を平滑処理した値を用いて時間外挿した位置を出力する。
【００８１】
運動状態外挿部１８０は、運動状態判定部１４０で旋回と判定された時刻までは等速直線運動モデルに基づく時間外挿を行い，旋回判定された時刻から所望の時刻までは前記運動状態判定部１４０で算出された角速度を用いて時間外挿した位置を出力する。
【００８２】
追尾部１３０は、レーダもしくはＧＰＳなどのセンサから得られる観測位置より複数の運動モデルに基づく追尾処理を実施して目標の推定位置を計算する。運動状態判定部１４０は、追尾部１３０で推定した角速度からジャイロセンサから得られる角速度のバイアス誤差を補正して、その角速度より目標の旋回を判定する。運動状態外挿部１８０は、運動フェーズに基づいた時間外挿によって将来位置を予測する。
これにより、目標が等速直線運動と仮定して将来位置を算出する場合と比較して、目標旋回時でも、位置予測精度を高くすることができる。また、ジャイロセンサから得られる角速度にバイアス誤差がある場合でも、位置予測精度を高くすることができる。
【００８３】
予測装置（目標追尾装置１００）は、位置観測装置（観測装置８１０）が観測した観測対象（移動体）の位置と、角速度観測装置（ジャイロセンサ８２０）が観測した上記観測対象の角速度（ジャイロ角速度）とに基づいて、上記観測対象の将来の位置を予測する。
上記予測装置（１００）は、角速度誤差推定装置（角速度誤差推定部１４６）と、角速度補正装置（角速度補正部１４７）と、予測位置算出装置（モデル選択部１３２、運動状態外挿部１８０）とを有する。
上記角速度誤差推定装置（１４６）は、上記位置観測装置（８１０）が観測した位置に基づいて、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
上記角速度補正装置（角速度補正部１４７）は、上記角速度誤差推定装置（１４６）が推定した誤差に基づいて、上記角速度観測装置（８１０）が観測した角速度を補正する。
上記予測位置算出装置（１３２，１８０）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記観測対象の将来の位置を予測する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【００８４】
上記予測装置（１００）は、角速度誤差記憶装置（バイアス補正値メモリ１５０）を有する。
上記角速度誤差推定装置（１４６）は、所定の条件を満たす場合に、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
上記角速度誤差記憶装置（１５０）は、上記角速度誤差推定装置（１４６）が推定した誤差を記憶する。
上記角速度補正装置（１４７）は、上記角速度誤差記憶装置（１５０）が記憶した誤差に基づいて、上記角速度観測装置（８２０）が観測した角速度を補正する。
所定の条件を満たす場合のみ、角速度観測装置が観測した角速度の誤差を推定し、それ以外の場合は、角速度誤差記憶装置が記憶した誤差を用いて、角速度観測装置が観測した角速度の誤差を補正するので、例えば、位置観測装置が観測した観測位置の精度が低い場合に、角速度の誤差の推定精度が落ちるのを防ぐことができる。
【００８５】
上記予測装置（１００）は、追尾装置（追尾部１３０）と、条件判定装置（条件判定部１４５）とを有する。
上記追尾装置（１３０）は、上記位置観測装置（８１０）が観測した位置に基づいて、上記観測対象の角速度を推定する。
上記条件判定装置（１４５）は、上記角速度観測装置（８２０）が観測した角速度と上記追尾装置（１３０）が推定した角速度とに基づいて、所定の条件を満たすか否かを判定する。
上記角速度誤差推定装置（１４６）は、上記所定の条件を満たすと上記条件判定装置（１４５）が判定した場合に、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
位置観測装置が観測した位置に基づいて観測対象の角速度を推定し、角速度の誤差を推定する条件を満たすか判定するので、例えば、観測対象の角速度の推定精度が低い場合に、角速度の誤差の推定精度が落ちるのを防ぐことができる。
【００８６】
上記予測装置（１００）は、推定角速度位置予測装置（推定角速度位置予測部１４４）と、観測角速度位置予測装置（観測角速度位置予測部１４３）とを有する。
上記推定角速度位置予測装置（１４４）は、上記追尾装置（１３０）が推定した角速度に基づいて、上記観測対象の将来の位置を予測する。
上記観測角速度位置予測装置（１４３）は、上記角速度観測装置（８２０）が観測した角速度に基づいて、上記観測対象の将来の位置を予測する。
上記条件判定装置（１４５）は、上記推定角速度位置予測装置（１４４）が予測した位置と、上記観測角速度位置予測装置（１４３）が予測した位置とに基づいて、上記所定の条件を満たすか否かを判定する。
推定した観測対象の角速度に基づいて予測した観測対象の位置と、観測した角速度に基づいて予測した観測対象の位置とに基づいて、角速度の誤差を推定する条件を満たすか判定するので、例えば、観測対象の位置の予測精度が低い場合に、角速度の誤差の推定精度が落ちるのを防ぐことができる。
【００８７】
上記条件判定装置（１４５）は、上記推定角速度位置予測装置（１４４）が予測した位置と、上記観測角速度位置予測装置（１４３）が予測した位置との間に有意な差があるか否かを検定する。
上記角速度誤差推定装置（１４６）は、上記推定角速度位置予測装置（１４４）が予測した位置と、上記観測角速度位置予測装置（１４３）が予測した位置との間に有意な差があると上記条件判定装置（１４５）が判定した場合に、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
２つの予測位置の間に有意な差があるか否かを検定して、角速度の誤差を推定する条件を満たすか判定するので、角速度の誤差を推定する必要がある場合のみ、角速度の誤差を推定することができ、計算負荷を減らすことができる。
【００８８】
上記予測装置（１００）は、直線位置推定装置（追尾フィルタ１３１）を有する。
上記直線位置推定装置（１３１）は、上記観測対象が直線運動をしているものとして、上記位置観測装置（８１０）が観測した位置に基づいて、上記観測対象の位置を推定する。
上記予測位置算出装置（１３２，１８０）は、上記観測対象が旋回していないと上記旋回判定装置（１４２）が判定した場合に、上記直線位置推定装置（１３１）が推定した位置に基づいて、上記観測対象の将来の位置を予測する。
観測対象が旋回していないと判定した場合は、直線運動モデルに基づいて推定した位置に基づいて、観測対象の将来の位置を予測するので、予測精度を高めることができる。
【００８９】
上記予測装置（１００）は、複数の所定角速度位置推定装置（追尾フィルタ１３１）を有する。
上記複数の所定角速度位置推定装置（１３１）は、それぞれ異なる所定の角速度で上記観測対象が旋回運動しているものとして、上記位置観測装置（８１０）が観測した位置に基づいて、上記観測対象の位置を推定する。
上記予測位置算出装置（１３２，１８０）は、上記角速度補正装置（１４７）が補正した角速度と、上記複数の所定角速度位置推定装置（１３１）が推定した位置とに基づいて、上記観測対象の位置を算出し、算出した上記観測対象の位置に基づいて、上記観測対象の将来の位置を予測する。
所定の角速度の旋回運動モデルに基づいて観測対象の位置を推定するので、計算負荷を減らすことができる。また、補正した角速度と、複数の所定角速度位置推定装置が推定した位置とに基づいて、観測対象の位置を算出するので、観測対象の位置を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【００９０】
上記予測装置（１００）は、重み算出装置（モデル確率更新部２１４）を有する。
上記重み算出装置（２１４）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記複数の所定角速度位置推定装置（１３１）それぞれの重み付け（モデル確率）を算出する。
上記予測位置算出装置（重付平均算出部２１５）は、上記重み算出装置（２１４）が算出した重み付けにしたがって、上記複数の所定角速度位置推定装置（１３１）それぞれが推定した位置を合成して、上記観測対象の位置を算出する。
補正した角速度に基づいて算出した重み付けに基づいて、複数の所定角速度位置推定装置が推定した位置を合成するので、観測対象の位置を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【００９１】
上記予測装置（１００）は、旋回判定装置（旋回判定部１４２）を有する。
上記旋回判定装置（１４２）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記観測対象（移動体）が旋回しているか否かを判定する。
上記予測位置算出装置（１３２，１８０）は、上記旋回判定装置（１４２）が判定した結果に基づいて、上記観測対象の将来の位置を予測する。
旋回判定装置の判定結果に基づいて、観測対象の将来の位置を予測するので、観測対象の将来の位置を高い精度で予測することができる。
【００９２】
上記予測位置算出装置（フィルタ選択部）は、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記複数の所定角速度位置推定装置（１３１）のなかから、採用する所定角速度位置推定装置（１３１）を選択し、選択した所定角速度位置推定装置（１３１）が推定した位置に基づいて、上記観測対象の将来の位置を算出する。
補正した角速度に基づいて、複数の所定角速度位置推定装置が推定した位置のなかから観測対象の位置を選択するので、観測対象の位置を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【００９３】
予測システム（追尾システム８００）は、観測対象の位置を観測する位置観測装置（８１０）と、上記観測対象の角速度を観測する角速度観測装置（８２０）と、予測装置（１００）とを有する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【００９４】
上記予測装置（１００）が、位置観測装置（８１０）が観測した観測対象の位置と、角速度観測装置（８２０）が観測した上記観測対象の角速度とに基づいて、観測対象の将来の位置を予測する予測方法は、以下の工程を有する。
上記角速度誤差推定装置（１４６）が、上記位置観測装置（８１０）が観測した位置に基づいて、上記角速度観測装置（８２０）が観測した角速度の誤差を推定する。
上記角速度補正装置（１４７）が、上記角速度誤差推定装置（１４６）が推定した誤差に基づいて、上記角速度観測装置（８２０）が観測した角速度を補正する。
上記予測位置算出装置（１３２，１８０）が、上記角速度補正装置（１４７）が補正した角速度に基づいて、上記観測対象の将来の位置を予測する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる。
【００９５】
複数の運動モデルを持つ追尾装置から適切な運動モデルを選択するので、観測対象の現在の位置を高い精度で推定し、観測対象の将来の位置を高い精度で予測することができる。センサで観測される位置と追尾予測位置との差（残差）を用いて適切な運動モデルを選択する場合と比べて、サンプル数が必要なく、迅速に運動モデルの選択をすることができる。
【００９６】
実施の形態２．
実施の形態２について、図１４を用いて説明する。
なお、実施の形態１と共通する部分については、同一の符号を付し、説明を省略する。
【００９７】
図１４は、この実施の形態における追尾システム８００の構成の一例を示すブロック構成図である。
【００９８】
目標追尾装置１００は、実施の形態１で説明した構成に加えて、更に、地図記憶部１７０を有する。地図記憶部１７０は、移動体が移動する領域の形状など、移動体の運動状態を推定する手がかりとなる情報（地図データ）を記憶している。例えば、移動体が車両である場合、地図記憶部１７０は、道路の形状を記憶している。あるいは、移動体が航空機である場合、地図記憶部１７０は、飛行計画経路を記憶している。例えば、直線道路上や、飛行計画経路の直線区間であれば、移動体は直線運動することが予想される。
【００９９】
バイアス補正部１４１の条件判定部１４５（直線判定装置）は、モデル選択部１３２が算出した移動体の推定位置を、地図記憶部１７０が記憶した情報と照合して、移動体が直線運動することが予想される直線区間内に存在するか否かを判定する。条件判定部１４５は、直線区間内に移動体がある場合、ジャイロ角速度のバイアス誤差を推定すると判定する。
【０１００】
バイアス補正部１４１の角速度誤差推定部１４６は、ジャイロ角速度のバイアス誤差を推定すると条件判定部１４５が判定した場合、ジャイロセンサ８２０が観測したジャイロ角速度のバイアス誤差を推定する。推定の方式は、実施の形態１で説明したカルマンフィルタを用いた方式でもよいが、この実施の形態では、異なる方式について説明する。
角速度誤差推定部１４６は、ジャイロセンサ８２０が観測したジャイロ角速度の平均値を算出し、追尾フィルタ１３１が推定した追尾角速度の平均値を算出し、算出した平均値の差を算出することにより、ジャイロ角速度のバイアス誤差を得る。角速度誤差推定部１４６は、例えば、次の式の右辺を計算することにより、ジャイロ角速度のバイアス誤差Δωを算出する。
【数２４】
ただし、Ｉは、直線区間内に移動体がある間に、ジャイロセンサ８２０がジャイロ角速度ωＪを観測した数である。Ｊは、直線区間内に移動体がある間に、追尾フィルタ１３１が追尾角速度ωＴを推定した数である。
すなわち、この式は、直線区間内に移動体がある間に、ジャイロセンサ８２０が観測したジャイロ角速度ωＪを平均し、追尾フィルタ１３１が推定した追尾角速度ωＴを平均して、２つの平均の差を取ることにより、ジャイロ角速度のバイアス誤差Δωを算出することを意味する。
【０１０１】
モデル選択部１３２の推移確率更新部２１２は、移動体の推定位置を、地図記憶部１７０が記憶した情報と照合して、移動体が存在する区間を判定する。直線区間内に移動体があると判定した場合、推移確率更新部２１２は、推移確率記憶部２１１が記憶した推移確率を更新して、直線運動モデルへの推移確率を高くする。また、推移確率更新部２１２は、カーブ区間内に移動体があると判定した場合、推移確率更新部２１２は、推移確率記憶部２１１が記憶した推移確率を更新して、旋回モデルへの推移確率を高くする。
【０１０２】
なお、推移確率を介さずにモデル確率を更新する構成であってもよい。例えば、直線区間内に移動体があると判定した場合、モデル選択部１３２のモデル確率更新部２１４は、直線運動モデルのモデル確率を１、それ以外の運動モデルのモデル確率を０にする。また、カーブ区間内に移動体があると判定した場合、モデル確率更新部２１４は、直線運動モデルのモデル確率を０、旋回運動モデルのモデル確率を１にする。
【０１０３】
追尾部１３０は、地図データを参照し、直線路では等速直線運動モデル、カーブ路では旋回運動モデルもしくは加速度運動モデルへ変更する。
【０１０４】
上記予測装置（目標追尾装置１００）は、直線判定装置（条件判定部１４５）を有する。
上記直線判定装置（１４５）は、上記位置観測装置（観測装置８１０）が観測した位置に基づいて、上記観測対象（移動体）が直線運動をしているか否かを判定する。
上記角速度誤差推定装置（角速度誤差推定部１４６）は、上記観測対象が直線運動をしていると上記直線判定装置（１４５）が判定した場合に、上記角速度観測装置（ジャイロセンサ８２０）が観測した角速度の誤差を推定する。
観測対象が直線運動をしていると判定した場合に、角速度観測装置が観測した角速度の誤差を推定するので、角速度の誤差を高い精度で推定することができ、観測対象の将来の位置を高い精度で予測することができる。
【０１０５】
実施の形態３．
実施の形態３について、図１５〜図１７を用いて説明する。
なお、実施の形態１及び実施の形態２と共通する部分については、同一の符号を付し、説明を省略する。
【０１０６】
図１５は、この実施の形態における追尾システム８００の構成の一例を示すブロック構成図である。
追尾部１３０は、１つの追尾フィルタ１３１ｂを有する。追尾フィルタ１３１ｂは、等速直線運動モデルに基づいて、移動体の位置などを推定する。追尾フィルタ１３１ｂは、例えば、α−βフィルタ、カルマンフィルタやその他のフィルタなどである。追尾フィルタ１３１ｂとしてα−βフィルタを使うことにより、カルマンフィルタなどよりも計算負荷を軽くすることができる。
【０１０７】
図１６は、この実施の形態におけるバイアス補正部１４１の構成の一例を示す詳細ブロック図である。
観測角速度位置予測部１４３は、追尾フィルタ１３１ｂが推定した追尾位置などと、ジャイロセンサ８２０が観測したジャイロ角速度とに基づいて、移動体がジャイロ角速度で旋回運動をしているという運動モデルにしたがって、移動体の将来の位置を予測する。
バイアス補正部１４１は、推定角速度位置予測部１４４に代えて、直線位置予測部１４８を有する。直線位置予測部１４８は、追尾フィルタ１３１ｂが推定した追尾位置などに基づいて、移動体が等速直線運動をしているという運動モデルにしたがって、移動体の将来の位置を予測する。
【０１０８】
図１７は、この実施の形態における観測角速度位置予測部１４３及び直線位置予測部１４８が予測する予測位置６３１，６３３の関係の一例を示す図である。
観測位置６１１ａ〜６１１ｄは、時刻ｔ１までの時点で、観測装置８１０が観測した移動体６００の観測位置を表わす。このうち、観測位置６１１ｄは、時刻ｔ２に観測した最も新しい観測位置である。
追尾部１３０は、観測位置６１１ａ〜６１１ｄに基づいて、時刻ｔ２における移動体６００の推定位置６２１を算出する。
観測角速度位置予測部１４３は、追尾部１３０が推定した推定位置６２１などに基づいて、時刻ｔ１における移動体６００の推定位置６２２などを算出する。
観測角速度位置予測部１４３は、算出した推定位置６２２などと、ジャイロ角速度とに基づいて、時刻ｔ３における移動体６００の予測位置６３１を算出する。
直線位置予測部１４８は、追尾部１３０が推定した推定位置６２１などに基づいて、時刻ｔ３における移動体６００の予測位置６３３を算出する。
【０１０９】
条件判定部１４５は、カイ二乗検定などにより、観測角速度位置予測部１４３が算出した予測位置６３１と、直線位置予測部１４８が算出した予測位置６３３との間に、有意な差があるか否かを判定する。２つの予測位置６３１，６３３の間に有意な差がある場合、条件判定部１４５は、ジャイロ角速度の誤差を推定すると判定する。
【０１１０】
角速度誤差推定部１４６は、ジャイロ角速度の誤差を推定すると条件判定部１４５が判定した場合に、ジャイロ角速度の誤差を推定する。角速度誤差推定部１４６は、例えば、ジャイロセンサ８２０が観測したジャイロ角速度ωＪをそのまま、ジャイロ角速度のバイアス誤差Δωの観測値とする。すなわち、移動体が直線運動をしていると仮定して、ジャイロ角速度のバイアス誤差Δωを推定する。
【０１１１】
移動体が等速直線運動をしていない場合、追尾フィルタ１３１ｂの運動モデルと実際の移動体の運動とが合致していないので、追尾フィルタ１３１ｂによる推定の精度が落ち、推定の誤差分散が大きくなる。予測位置６３１と予測位置６３３との間には有意な差があるとは言えなくなるので、条件判定部１４５は、ジャイロ角速度の誤差を推定しないと判定する。
逆に言えば、ジャイロ角速度の誤差を推定すると条件判定部１４５が判定した場合、追尾フィルタ１３１ｂの運動モデルが実際の移動体の運動に合致していることを意味するから、移動体は、等速直線運動をしていると考えられる。
【０１１２】
角速度誤差推定部１４６は、例えば、カルマンフィルタを用いて、ジャイロ角速度のバイアス誤差Δωの推定値を算出する。角速度誤差推定部１４６は、例えば、次の式で定義される観測モデルを用いる。
【数２５】
【０１１３】
なお、条件判定部１４５は、実施の形態２で説明したように、地図記憶部１７０が記憶した情報に基づいて、直線区間内に移動体がいるか否かを判定する構成であってもよい。条件判定部１４５は、直線区間内に移動体がいると判定した場合、ジャイロ角速度の誤差を推定すると判定する。
【０１１４】
目標追尾装置１００は、等速直線運動モデルの追尾フィルタ１３１ｂを用いて目標（移動体）の観測値から推定値を算出し、ジャイロの角速度から運動状態を判別して、運動状態に基づいて目標の将来位置を予測する。
運動状態判定部１４０は、ジャイロの角速度を追尾部１３０から得られる推定値を用いてバイアス補正し、バイアス補正後の角速度を用いて運動状態を判別する。
追尾部１３０は、運動状態から等速直線運動モデルに基づいて推定値を算出する。
運動状態外挿部１８０は、推定値と運動状態に基づいて時間外挿する。
【０１１５】
追尾部１３０は、追尾フィルタ１３１ｂとしてα−βフィルタを用いる。
【０１１６】
運動状態判定部１４０は、追尾部１３０から得られる推定位置からジャイロの角速度を用いて時間外挿して算出した予測値と等速直線運動と仮定して時間外挿して算出した予測値の差分を予測誤差分散によるカイ二乗判定によって、同一とみなされない場合はジャイロの角速度に含まれるバイアス誤差推定を行う。
【０１１７】
運動状態判定部１４０は、地図データを用いて目標が直線路に存在している場合はバイアス補正処理を実施し、それ以外の経路ではバイアス補正処理を行わない。
【０１１８】
実施の形態４．
実施の形態４について、図１８を用いて説明する。
なお、実施の形態１乃至実施の形態３と共通する部分については、同一の符号を付し、説明を省略する。
【０１１９】
図１８は、この実施の形態における予測システム８０１の構成の一例を示すシステム構成図である。
予測システム８０１は、複数の観測対象（移動体）の将来の位置を予測して、観測対象同士が衝突する危険があるか否かを判定する。
【０１２０】
予測システム８０１は、位置観測装置８１１を有する。位置観測装置８１１は、観測対象の位置を観測する。位置観測装置８１１は、例えばレーダやＧＰＳなどである。位置観測装置８１１が観測した位置を観測位置と呼ぶ。
【０１２１】
予測システム８０１は、角速度観測装置８２１を有する。角速度観測装置８２１は、観測対象の角速度を観測する。角速度観測装置８２１は、例えばジャイロスコープである。角速度観測装置８２１が観測した角速度を観測角速度と呼ぶ。
【０１２２】
予測システム８０１は、予測装置１０１を有する。予測装置１０１は、位置観測装置８１１が観測した観測位置や角速度観測装置８２１が観測した観測角速度に基づいて、観測対象の現在位置や運動状態を推定する。予測装置１０１は、推定した結果に基づいて、観測対象の将来の位置を予測する。予測装置１０１は、予測した結果に基づいて、観測対象同士が衝突する危険があるか否かを判定する。
予測装置１０１は、例えば、コンピュータである。
予測装置１０１は、図示していない記憶装置を有する。記憶装置は、コンピュータプログラムやデータなどを記憶する。記憶装置は、例えば、揮発性メモリ（ＲＡＭ）、不揮発性メモリ（ＲＯＭ）、磁気ディスク装置、光学ディスク装置などである。
予測装置１０１は、図示していない処理装置を有する。処理装置は、記憶装置が記憶したコンピュータプログラムを実行することにより、記憶装置が記憶したデータを処理し、予測装置１０１全体を制御する。
予測装置１０１は、図示していない入力装置を有する。入力装置は、予測装置１０１の外部からデータ、信号、操作などの情報を入力し、処理装置が処理するデータに変換する。入力装置が変換したデータは、処理装置が直接処理してもよいし、記憶装置が一時的に記憶してもよい。
予測装置１０１は、図示していない出力装置を有する。出力装置は、処理装置が処理したデータや記憶装置が記憶したデータなどを、データ、信号、画像、音声など予測装置１０１の外部に出力できる形式に変換し、外部に出力する。
以下に説明する予測装置１０１の機能ブロックは、記憶装置が記憶したコンピュータプログラムを、処理装置が実行することにより、実現される。なお、予測装置１０１は、一台のコンピュータであってもよいし、予測装置１０１の一または複数の機能ブロックを実現するコンピュータが複数集まったものであってもよい。
なお、予測装置１０１の機能ブロックは、コンピュータにより実現するのではなく、アナログ回路やデジタル回路などの電子回路により実現してもよいし、機械的構成など電気的構成以外の構成により実現してもよい。
【０１２３】
予測装置１０１は、追尾装置２３１を有する。追尾装置２３１は、処理装置がデータを処理することにより、位置観測装置８１１が観測した観測位置に基づいて、観測対象の位置、速度、角速度など観測対象の現在の運動状態を推定する。追尾装置２３１が推定した観測対象の現在の運動状態を表わす数値を推定値と呼ぶ。追尾装置２３１は、例えば、カルマンフィルタを用いて、推定値を推定する。また、追尾装置２３１は、観測対象の現在の運動状態の推定の精度を推定する。追尾装置２３１は、例えば、推定値の誤差の期待値の分散や共分散などを算出する。
【０１２４】
予測装置１０１は、角速度誤差推定装置２４６を有する。角速度誤差推定装置２４６は、処理装置がデータを処理することにより、角速度観測装置８２１が観測した観測角速度と、追尾装置２３１が推定した角速度の推定値とに基づいて、観測角速度の誤差を推定する。角速度誤差推定装置２４６は、例えば、角速度観測装置８２１が観測した観測角速度と、追尾装置２３１が推定した角速度の推定値との差を観測値とし、カルマンフィルタなどにより、観測角速度のバイアス誤差やゲイン誤差などを推定する。角速度誤差推定装置２４６が推定した観測角速度の誤差を、誤差推定値と呼ぶ。
なお、角速度観測装置８２１が観測角速度を観測した時刻と、追尾装置２３１が角速度の推定値を推定する根拠となった観測位置を位置観測装置８１１が観測した時刻とが異なる場合、角速度誤差推定装置２４６は、例えば、位置観測装置８１１が観測位置を観測した時刻を基準として、その前後において追尾装置２３１が観測した観測角速度を平滑処理することにより、位置観測装置８１１が観測位置を観測した時刻における観測角速度を算出する。平滑処理により算出された観測角速度を平滑角速度と呼ぶ。角速度誤差推定装置２４６は、算出した平滑角速度と、追尾装置２３１が推定した角速度の推定値との差に基づいて、観測角速度の誤差を推定する。なお、角速度誤差推定装置２４６は、実施の形態１で説明した方式で、観測時刻を合わせる構成であってもよい。
【０１２５】
予測装置１０１は、条件判定装置２４５を有する。条件判定装置２４５は、処理装置がデータを処理することにより、追尾装置２３１が推定した推定の精度に基づいて、誤差推定値を推定をするか否かを判定する。追尾装置２３１が推定した角速度の推定値の精度が低い場合、それを基準として誤差推定値を推定すると、かえって誤差推定値の精度が落ちる場合があるので、条件判定装置２４５は、誤差推定値を推定しないと判定する。条件判定装置２４５は、例えば、追尾装置２３１が算出した誤差分散の値を所定の閾値と比較して、誤差分散の値が閾値より小さい場合に、誤差推定値を推定すると判定する。
角速度誤差推定装置２４６は、条件判定装置２４５の判定結果に基づき、誤差推定値を推定すると条件判定装置２４５が判定した場合に、観測角速度の誤差を推定する。
【０１２６】
予測装置１０１は、角速度誤差記憶装置２５０を有する。角速度誤差記憶装置２５０は、記憶装置がデータを記憶することにより、角速度誤差推定装置２４６が推定した誤差推定値を記憶する。
【０１２７】
なお、条件判定装置２４５を設けず、角速度誤差推定装置２４６は、常に、誤差推定値を推定する構成であってもよい。角速度誤差推定装置２４６は、追尾装置２３１が推定した推定精度に基づいて、カルマンフィルタで用いる観測誤差分散共分散行列の値を変化させる構成であってもよい。角速度誤差推定装置２４６は、例えば、追尾装置２３１が算出した誤差分散の値を、観測誤差分散共分散行列の要素の値とする。
【０１２８】
予測装置１０１は、角速度補正装置２４７を有する。角速度補正装置２４７は、処理装置がデータを処理することにより、角速度誤差記憶装置２５０が記憶した誤差推定値を使って、角速度観測装置８２１が観測した観測角速度の誤差を補正する。角速度補正装置２４７は、例えば、観測角速度から誤差推定値を差し引いた差を算出することにより、誤差を補正した観測角速度を得る。角速度補正装置２４７が誤差を補正した観測角速度を、補正済角速度と呼ぶ。
【０１２９】
予測装置１０１は、予測位置算出装置２８０を有する。予測位置算出装置２８０は、処理装置がデータを処理することにより、追尾装置２３１が推定した観測対象の現在の運動状態と、角速度補正装置２４７が補正した補正済角速度とに基づいて、観測対象の将来の運動状態を予測する。予測位置算出装置２８０が予測した観測対象の将来の運動状態を表わす数値を予測値と呼ぶ。
【０１３０】
予測装置１０１は、衝突危険判定装置２９０を有する。衝突危険判定装置２９０は、処理装置がデータを処理することにより、予測位置算出装置２８０が予測した観測対象の将来の運動状態に基づいて、複数の観測対象同士が衝突する危険があるか否かを判定する。衝突危険判定装置２９０は、衝突する危険があると判定した場合、そのことを警告する警告メッセージを出力する。衝突危険判定装置２９０が出力した警告メッセージは、例えば、管制画面に表示され、あるいは、観測対象に対して送信されるなどして、衝突の危険を回避する行動を促す。
【０１３１】
上記予測装置１０１は、衝突危険判定装置２９０を有し、複数の観測対象それぞれについて、上記観測対象の将来の位置を予測する。
上記衝突危険判定装置２９０は、上記予測位置算出装置２８０が予測した位置に基づいて、上記複数の観測対象が衝突する危険があるか否かを判定する。
観測対象の将来の位置を高い精度で予測するので、衝突の危険を高い精度で判定することができる。
【０１３２】
上記コンピュータプログラムは、コンピュータが実行することにより、上記コンピュータが予測装置１０１として機能する。
角速度観測装置が観測した角速度の誤差を補正した精度の高い角速度に基づいて、観測対象の将来の位置を予測するので、観測対象が旋回している場合でも観測対象の将来の位置を高い精度で予測することができる予測装置を実現することができる。
【符号の説明】
【０１３３】
１００ 目標追尾装置、１０１ 予測装置、１３０ 追尾部、１３１ 追尾フィルタ、１３２ モデル選択部、１４０ 運動状態判定部、１４１ バイアス補正部、１４２ 旋回判定部、１４３ 観測角速度位置予測部、１４４ 推定角速度位置予測部、１４５ 条件判定部、１４６ 角速度誤差推定部、１４７ 角速度補正部、１４８ 直線位置予測部、１５０ バイアス補正値メモリ、１６０ 運動性能記憶部、１７０ 地図記憶部、１８０ 運動状態外挿部、１９１ 最大角速度取得部、１９２ 異常値判定部、１９３ 閾値比較部、１９５ 回数判定部、１９６ 判定結果出力部、２１１ 推移確率記憶部、２１２ 推移確率更新部、２１３ モデル確率記憶部、２１４ モデル確率更新部、２１５ 重付平均算出部、２３１ 追尾装置、２４５ 条件判定装置、２４６ 角速度誤差推定装置、２４７ 角速度補正装置、２５０ 角速度誤差記憶装置、２８０ 予測位置算出装置、２９０ 衝突危険判定装置、６００ 移動体、６０６ 真の角速度、６１１ 観測位置、６１６ ジャイロ角速度、６２１，６２２ 推定位置、６２６ 追尾角速度、６３１〜６３３ 予測位置、８００ 追尾システム、８０１ 予測システム、８１０ 観測装置、８１１ 位置観測装置、８２０ ジャイロセンサ、８２１ 角速度観測装置。
【特許請求の範囲】
【請求項１】
位置観測装置が観測した観測対象の位置と、角速度観測装置が観測した上記観測対象の角速度とに基づいて、上記観測対象の将来の位置を予測する予測装置において、
上記予測装置は、角速度誤差推定装置と、角速度補正装置と、予測位置算出装置とを有し、
上記角速度誤差推定装置は、上記位置観測装置が観測した位置に基づいて、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度補正装置は、上記角速度誤差推定装置が推定した誤差に基づいて、上記角速度観測装置が観測した角速度を補正し、
上記予測位置算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記観測対象の将来の位置を予測することを特徴とする予測装置。
【請求項２】
上記予測装置は、角速度誤差記憶装置を有し、
上記角速度誤差推定装置は、所定の条件を満たす場合に、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度誤差記憶装置は、上記角速度誤差推定装置が推定した誤差を記憶し、
上記角速度補正装置は、上記角速度誤差記憶装置が記憶した誤差に基づいて、上記角速度観測装置が観測した角速度を補正することを特徴とする請求項１に記載の予測装置。
【請求項３】
上記予測装置は、追尾装置と、条件判定装置とを有し、
上記追尾装置は、上記位置観測装置が観測した位置に基づいて、上記観測対象の角速度を推定し、
上記条件判定装置は、上記角速度観測装置が観測した角速度と上記追尾装置が推定した角速度とに基づいて、所定の条件を満たすか否かを判定し、
上記角速度誤差推定装置は、上記所定の条件を満たすと上記条件判定装置が判定した場合に、上記角速度観測装置が観測した角速度の誤差を推定することを特徴とする請求項２に記載の予測装置。
【請求項４】
上記予測装置は、推定角速度位置予測装置と、観測角速度位置予測装置とを有し、
上記推定角速度位置予測装置は、上記追尾装置が推定した角速度に基づいて、上記観測対象の将来の位置を予測し、
上記観測角速度位置予測装置は、上記角速度観測装置が観測した角速度に基づいて、上記観測対象の将来の位置を予測し、
上記条件判定装置は、上記推定角速度位置予測装置が予測した位置と、上記観測角速度位置予測装置が予測した位置とに基づいて、上記所定の条件を満たすか否かを判定することを特徴とする請求項３に記載の予測装置。
【請求項５】
上記条件判定装置は、上記推定角速度位置予測装置が予測した位置と、上記観測角速度位置予測装置が予測した位置との間に有意な差があるか否かを検定し、
上記角速度誤差推定装置は、上記推定角速度位置予測装置が予測した位置と、上記観測角速度位置予測装置が予測した位置との間に有意な差があると上記条件判定装置が判定した場合に、上記角速度観測装置が観測した角速度の誤差を推定することを特徴とする請求項４に記載の予測装置。
【請求項６】
上記予測装置は、直線判定装置を有し、
上記直線判定装置は、上記位置観測装置が観測した位置に基づいて、上記観測対象が直線運動をしているか否かを判定し、
上記角速度誤差推定装置は、上記観測対象が直線運動をしていると上記直線判定装置が判定した場合に、上記角速度観測装置が観測した角速度の誤差を推定することを特徴とする請求項２乃至請求項５のいずれかに記載の予測装置。
【請求項７】
上記予測装置は、旋回判定装置を有し、
上記旋回判定装置は、上記角速度補正装置が補正した角速度に基づいて、上記観測対象が旋回しているか否かを判定し、
上記予測位置算出装置は、上記旋回判定装置が判定した結果に基づいて、上記観測対象の将来の位置を予測することを特徴とする請求項１乃至請求項６のいずれかに記載の予測装置。
【請求項８】
上記予測装置は、直線位置推定装置を有し、
上記直線位置推定装置は、上記観測対象が直線運動をしているものとして、上記位置観測装置が観測した位置に基づいて、上記観測対象の位置を推定し、
上記予測位置算出装置は、上記観測対象が旋回していないと上記旋回判定装置が判定した場合に、上記直線位置推定装置が推定した位置に基づいて、上記観測対象の将来の位置を予測することを特徴とする請求項７に記載の予測装置。
【請求項９】
上記予測装置は、複数の所定角速度位置推定装置を有し、
上記複数の所定角速度位置推定装置は、それぞれ異なる所定の角速度で上記観測対象が旋回運動しているものとして、上記位置観測装置が観測した位置に基づいて、上記観測対象の位置を推定し、
上記予測位置算出装置は、上記角速度補正装置が補正した角速度と、上記複数の所定角速度位置推定装置が推定した位置とに基づいて、上記観測対象の位置を算出し、算出した上記観測対象の位置に基づいて、上記観測対象の将来の位置を予測することを特徴とする請求項１乃至請求項８のいずれかに記載の予測装置。
【請求項１０】
上記予測装置は、重み算出装置を有し、
上記重み算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記複数の所定角速度位置推定装置それぞれの重み付けを算出し、
上記予測位置算出装置は、上記重み算出装置が算出した重み付けにしたがって、上記複数の所定角速度位置推定装置それぞれが推定した位置を合成して、上記観測対象の位置を算出することを特徴とする請求項９に記載の予測装置。
【請求項１１】
上記予測位置算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記複数の所定角速度位置推定装置のなかから、採用する所定角速度位置推定装置を選択し、選択した所定角速度位置推定装置が推定した位置に基づいて、上記観測対象の将来の位置を算出することを特徴とする請求項９または請求項１０に記載の予測装置。
【請求項１２】
上記予測装置は、衝突危険判定装置を有し、複数の観測対象それぞれについて、上記観測対象の将来の位置を予測し、
上記衝突危険判定装置は、上記予測位置算出装置が予測した位置に基づいて、上記複数の観測対象が衝突する危険があるか否かを判定することを特徴とする請求項１乃至請求項１１のいずれかに記載の予測装置。
【請求項１３】
観測対象の位置を観測する位置観測装置と、上記観測対象の角速度を観測する角速度観測装置と、請求項１乃至請求項１２のいずれかに記載の予測装置とを有することを特徴とする予測システム。
【請求項１４】
コンピュータが実行することにより、上記コンピュータが請求項１乃至請求項１２のいずれかに記載の予測装置として機能することを特徴とするコンピュータプログラム。
【請求項１５】
角速度誤差推定装置と、角速度補正装置と、予測位置算出装置とを有する予測装置が、位置観測装置が観測した観測対象の位置と、角速度観測装置が観測した上記観測対象の角速度とに基づいて、観測対象の将来の位置を予測する予測方法において、
上記角速度誤差推定装置が、上記位置観測装置が観測した位置に基づいて、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度補正装置が、上記角速度誤差推定装置が推定した誤差に基づいて、上記角速度観測装置が観測した角速度を補正し、
上記予測位置算出装置が、上記角速度補正装置が補正した角速度に基づいて、上記観測対象の将来の位置を予測することを特徴とする予測方法。
【請求項１】
位置観測装置が観測した観測対象の位置と、角速度観測装置が観測した上記観測対象の角速度とに基づいて、上記観測対象の将来の位置を予測する予測装置において、
上記予測装置は、角速度誤差推定装置と、角速度補正装置と、予測位置算出装置とを有し、
上記角速度誤差推定装置は、上記位置観測装置が観測した位置に基づいて、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度補正装置は、上記角速度誤差推定装置が推定した誤差に基づいて、上記角速度観測装置が観測した角速度を補正し、
上記予測位置算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記観測対象の将来の位置を予測することを特徴とする予測装置。
【請求項２】
上記予測装置は、角速度誤差記憶装置を有し、
上記角速度誤差推定装置は、所定の条件を満たす場合に、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度誤差記憶装置は、上記角速度誤差推定装置が推定した誤差を記憶し、
上記角速度補正装置は、上記角速度誤差記憶装置が記憶した誤差に基づいて、上記角速度観測装置が観測した角速度を補正することを特徴とする請求項１に記載の予測装置。
【請求項３】
上記予測装置は、追尾装置と、条件判定装置とを有し、
上記追尾装置は、上記位置観測装置が観測した位置に基づいて、上記観測対象の角速度を推定し、
上記条件判定装置は、上記角速度観測装置が観測した角速度と上記追尾装置が推定した角速度とに基づいて、所定の条件を満たすか否かを判定し、
上記角速度誤差推定装置は、上記所定の条件を満たすと上記条件判定装置が判定した場合に、上記角速度観測装置が観測した角速度の誤差を推定することを特徴とする請求項２に記載の予測装置。
【請求項４】
上記予測装置は、推定角速度位置予測装置と、観測角速度位置予測装置とを有し、
上記推定角速度位置予測装置は、上記追尾装置が推定した角速度に基づいて、上記観測対象の将来の位置を予測し、
上記観測角速度位置予測装置は、上記角速度観測装置が観測した角速度に基づいて、上記観測対象の将来の位置を予測し、
上記条件判定装置は、上記推定角速度位置予測装置が予測した位置と、上記観測角速度位置予測装置が予測した位置とに基づいて、上記所定の条件を満たすか否かを判定することを特徴とする請求項３に記載の予測装置。
【請求項５】
上記条件判定装置は、上記推定角速度位置予測装置が予測した位置と、上記観測角速度位置予測装置が予測した位置との間に有意な差があるか否かを検定し、
上記角速度誤差推定装置は、上記推定角速度位置予測装置が予測した位置と、上記観測角速度位置予測装置が予測した位置との間に有意な差があると上記条件判定装置が判定した場合に、上記角速度観測装置が観測した角速度の誤差を推定することを特徴とする請求項４に記載の予測装置。
【請求項６】
上記予測装置は、直線判定装置を有し、
上記直線判定装置は、上記位置観測装置が観測した位置に基づいて、上記観測対象が直線運動をしているか否かを判定し、
上記角速度誤差推定装置は、上記観測対象が直線運動をしていると上記直線判定装置が判定した場合に、上記角速度観測装置が観測した角速度の誤差を推定することを特徴とする請求項２乃至請求項５のいずれかに記載の予測装置。
【請求項７】
上記予測装置は、旋回判定装置を有し、
上記旋回判定装置は、上記角速度補正装置が補正した角速度に基づいて、上記観測対象が旋回しているか否かを判定し、
上記予測位置算出装置は、上記旋回判定装置が判定した結果に基づいて、上記観測対象の将来の位置を予測することを特徴とする請求項１乃至請求項６のいずれかに記載の予測装置。
【請求項８】
上記予測装置は、直線位置推定装置を有し、
上記直線位置推定装置は、上記観測対象が直線運動をしているものとして、上記位置観測装置が観測した位置に基づいて、上記観測対象の位置を推定し、
上記予測位置算出装置は、上記観測対象が旋回していないと上記旋回判定装置が判定した場合に、上記直線位置推定装置が推定した位置に基づいて、上記観測対象の将来の位置を予測することを特徴とする請求項７に記載の予測装置。
【請求項９】
上記予測装置は、複数の所定角速度位置推定装置を有し、
上記複数の所定角速度位置推定装置は、それぞれ異なる所定の角速度で上記観測対象が旋回運動しているものとして、上記位置観測装置が観測した位置に基づいて、上記観測対象の位置を推定し、
上記予測位置算出装置は、上記角速度補正装置が補正した角速度と、上記複数の所定角速度位置推定装置が推定した位置とに基づいて、上記観測対象の位置を算出し、算出した上記観測対象の位置に基づいて、上記観測対象の将来の位置を予測することを特徴とする請求項１乃至請求項８のいずれかに記載の予測装置。
【請求項１０】
上記予測装置は、重み算出装置を有し、
上記重み算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記複数の所定角速度位置推定装置それぞれの重み付けを算出し、
上記予測位置算出装置は、上記重み算出装置が算出した重み付けにしたがって、上記複数の所定角速度位置推定装置それぞれが推定した位置を合成して、上記観測対象の位置を算出することを特徴とする請求項９に記載の予測装置。
【請求項１１】
上記予測位置算出装置は、上記角速度補正装置が補正した角速度に基づいて、上記複数の所定角速度位置推定装置のなかから、採用する所定角速度位置推定装置を選択し、選択した所定角速度位置推定装置が推定した位置に基づいて、上記観測対象の将来の位置を算出することを特徴とする請求項９または請求項１０に記載の予測装置。
【請求項１２】
上記予測装置は、衝突危険判定装置を有し、複数の観測対象それぞれについて、上記観測対象の将来の位置を予測し、
上記衝突危険判定装置は、上記予測位置算出装置が予測した位置に基づいて、上記複数の観測対象が衝突する危険があるか否かを判定することを特徴とする請求項１乃至請求項１１のいずれかに記載の予測装置。
【請求項１３】
観測対象の位置を観測する位置観測装置と、上記観測対象の角速度を観測する角速度観測装置と、請求項１乃至請求項１２のいずれかに記載の予測装置とを有することを特徴とする予測システム。
【請求項１４】
コンピュータが実行することにより、上記コンピュータが請求項１乃至請求項１２のいずれかに記載の予測装置として機能することを特徴とするコンピュータプログラム。
【請求項１５】
角速度誤差推定装置と、角速度補正装置と、予測位置算出装置とを有する予測装置が、位置観測装置が観測した観測対象の位置と、角速度観測装置が観測した上記観測対象の角速度とに基づいて、観測対象の将来の位置を予測する予測方法において、
上記角速度誤差推定装置が、上記位置観測装置が観測した位置に基づいて、上記角速度観測装置が観測した角速度の誤差を推定し、
上記角速度補正装置が、上記角速度誤差推定装置が推定した誤差に基づいて、上記角速度観測装置が観測した角速度を補正し、
上記予測位置算出装置が、上記角速度補正装置が補正した角速度に基づいて、上記観測対象の将来の位置を予測することを特徴とする予測方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【公開番号】特開２０１１−２２０７２７（Ｐ２０１１−２２０７２７Ａ）
【公開日】平成２３年１１月４日（２０１１．１１．４）
【国際特許分類】
【出願番号】特願２０１０−８７５１６（Ｐ２０１０−８７５１６）
【出願日】平成２２年４月６日（２０１０．４．６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２３年１１月４日（２０１１．１１．４）
【国際特許分類】
【出願日】平成２２年４月６日（２０１０．４．６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]