JP6946813B2

JP6946813B2 - 状態推定装置及びプログラム

Info

Publication number: JP6946813B2
Application number: JP2017141131A
Authority: JP
Inventors: 峻田口
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2017-07-20
Filing date: 2017-07-20
Publication date: 2021-10-06
Anticipated expiration: 2037-07-20
Also published as: JP2019020338A

Description

本発明は、状態推定装置及びプログラムに関する。

従来より、観測を正常値と異常値の確率分布として表現する研究がなされている（非特許文献１）。また、複数の仮説を統合して、複数の仮説の追跡に利用する技術がある（非特許文献２）。

また、ＧＰＳ観測の異常値を判定し、取り除く技術がある（特許文献１）。

M．Wuthrich et al． "Robust Gaussian filtering using a pseudo measurement"， 2016 ACC (2016) Andrew R． Runnalls， "A Kullback-Leibler Approach to Gaussian Mixture Reduction"， IEEE Trans． on Aerospace and Electronic Systems， 2007， 43．3，pp989-999．

特開２０１２−０６３３１３号広報

上記非特許文献１の技術は、重ね合わせた分布をもとの１つの分布としてしか追跡しないため、瞬間的な異常値は判定できるが、時間的追跡をしなければ異常値を判定することができない観測値に対して有用でない。

また、非特許文献２では、観測を正常値と異常値の確率分布として表現するものではないため、非特許文献１に適用して、時系列的に異常値を取り除くことは困難である。

特許文献１では、異常値を取り除いているが、非特許文献１と同様に時間的追跡をしなければ異常値を判定することができない観測値に対して有用でない。

本発明は、上記問題点を解決するためになされたもので、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる状態推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために、本発明の状態推定装置は、センサの観測値に基づいて推定対象の状態を推定する状態推定装置であって、前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部と、前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部と、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部と、前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部と、を含んで構成されている。

また、本発明のプログラムは、センサの観測値に基づいて推定対象の状態を推定するためのプログラムであって、コンピュータを、前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部、前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部、及び前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部として機能させるためのプログラムである。

本発明の状態予測装置及びプログラムによれば、状態予測部が、前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測し、仮説生成部が、前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する。

そして、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部と、前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部する。

このように、推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測し、仮説の各々について、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、生成された仮説の数が規定数より多い場合には、規定数以下になるように仮説を統合することにより、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる。

また、本発明の状態推定装置は、前記仮説生成部によって生成された複数の仮説のうち、前記観測値が正常値であると仮定して更新された状態を表す仮説については、前記観測値が正常値である場合の観測分布に基づいて、前記仮説の重みを算出し、前記観測値が異常値であると仮定して更新された状態を表す仮説については、前記観測値が異常値である場合の観測分布に基づいて仮説の重みを算出する重み算出部を更に含み、前記状態推定部は、前記仮説の各々が表す状態と、前記仮説の各々の重みとに基づいて、前記推定対象の状態を推定することができる。

また、本発明の状態推定装置の前記推定部は、前記仮説統合部は、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説のうちの２つの仮説を統合することを繰り返すことができる。

また、本発明の状態推定装置の前記仮説統合部は、前記仮説統合部は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を統合することができる。

また、本発明の状態推定装置は、前記センサが、ＧＰＳセンサであり、前記推定対象の状態が、自装置の位置であるとすることができる。

また、本発明の状態推定装置は、前記仮説が表す状態が、ガウス分布で表現されることができる。

以上説明したように、本発明の状態推定装置及びプログラムによれば、推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測し、仮説の各々について、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、生成された仮説の数が規定数より多い場合には、規定数以下になるように仮説を統合することにより、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる、という効果が得られる。

本発明の第１の実施の形態における概略構成を示すブロック図である。観測分布の例を示す図である。仮説を適宜統合する例を示す図である。観測毎に仮説の数が変化する例を示す図である。本発明の第１の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態におけるＣＡＮデータによる状態更新処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態におけるＧＰＳデータによる状態更新処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

＜本発明の実施の形態の概要＞

本発明の実施の形態では、車両に搭載されたＣＡＮ受信機及びＧＰＳセンサからの観測値に基づいて、車両の位置を推定する状態推定装置に本発明を適用した場合を例に説明する。

本実施形態では、複数のセンサ間でフレーム同期をしないアプローチをとっている。ＧＰＳセンサやＣＡＮ経由で得られる車速計等の車載センサのセンシング結果である観測値を受信したタイミング毎に、受信した観測値に基づいて、車両の位置を示す状態量の仮説を生成し、車両の位置を推定する。

ここで、時間的追跡をしなければ、観測値がマルチパス等によって異常値を含むことを判定できないが、１台のセンサの観測値を使って逐次車両の位置を推定するため、異常値を判定することができない、という問題がある。

この問題を解決するために、本発明の実施の形態では、観測値が正常値である場合と、観測値が異常値である場合の仮説をそれぞれ生成する。

また、仮説の数が増加し過ぎた場合には、計算が困難になったり、不可能になったりするため、適宜仮説を統合する。

＜状態推定システムのシステム構成＞

図１に示すように、本発明の実施の形態に係る状態推定システムは、ＣＡＮ受信機１００と、ＧＰＳセンサ２００と、状態推定装置３００とを備えて構成される。

ＣＡＮ受信機１００は、ＣＡＮ（ＣｏｎｔｒｏｌｌｅｒＡｒｅａＮｅｔｗｏｒｋ）から得られる車載センサ（車速計、ジャイロ）が観測した観測値を受信する。受信した観測値のうち、少なくとも車両速度及びヨーレートを含んだものをＣＡＮデータとする。

そして、ＣＡＮ受信機１００は、受信したＣＡＮデータを状態推定装置３００に出力する。

ＧＰＳセンサ２００は、各ＧＰＳ衛星から得られる観測値であるＧＰＳデータを受信する。ＧＰＳデータには、疑似距離及びドップラー周波数が含まれている。そして、ＧＰＳセンサ２００は、受信したＧＰＳデータを状態推定装置３００に出力する。

状態推定装置３００は、センサの観測値に基づいて自車両の位置を推定する装置である。本発明の実施の形態では、状態推定装置３００の内部において、自車両の位置を含む、以下の状態ｘを推定する。

ここで、ｘ、ｙ、ｚは地球中心座標での位置を表し、

はＧＰＳ受信機の時刻のずれ量を表し、

はヨーレートセンサの零点のずれ量（オフセット）を表す。

本実施の形態では、状態推定装置３００はガウシアンフィルタリングを用いて、状態ｘをガウス分布として推定する。このため、状態ｘとして、確率分布の平均

と分散Ｖとを推定する。

状態推定装置３００は、ＣＰＵと、ＲＡＭと、後述する状態推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

状態推定装置３００は、観測値受信部３１０と、状態予測部３２０と、仮説生成部３３０と、重み算出部３４０と、仮説統合部３５０と、状態推定部３６０と、出力部３７０とを備えて構成される。

観測値受信部３１０は、ＣＡＮ受信機１００から出力されたＣＡＮデータ、及びＧＰＳセンサ２００から出力されたＧＰＳデータを受信する。

ここで、ｙ_ｓａｔはＧＰＳデータ、ｙ_ＣＡＮはＣＡＮデータにおける観測値である。

そして、観測値受信部３１０は、各データを受信する毎に、受信したデータを状態予測部３２０へ送る。

状態予測部３２０は、ＧＰＳデータ又はＣＡＮデータが入力される毎に、推定対象の状態を表す仮説の各々について、観測時点における、当該仮説が表す状態を予測する。

具体的には、状態予測部３２０は、ＧＰＳデータ又はＣＡＮデータを観測値受信部３１０から受け取ると、各データが入力される毎に、推定対象である自車両の位置を含む状態ｘの仮説の各々について、以下のように観測時点における状態ｘを予測する。

まず、状態予測部３２０は、前回生成された仮説の全てを、仮説統合部３５０から取得しておく。

そして、状態予測部３２０は、取得した仮説の各々について、等速予測モデルなどの物理モデルを用いて当該状態ｘの予測を行う。本実施形態では、下記の予測モデルを用いて観測時点における仮説の状態ｘを予測する。

ここで、Ｑは予測モデルの誤差共分散行列、ｋはステップを表し、Δｔ_ｋはステップ間の時間幅を表す。また、ｖ_ｘｙｚはｘｙｚ座標での速度ベクトル、Ｖｖ_ｘｙｚはその分散共分散行列を表す。

また、

は、前回生成された仮説の状態における確率分布の平均及び分散を表す。

仮説生成部３３０は、ＧＰＳデータが入力された場合には、仮説の各々について、入力された観測値及び状態予測部３２０による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成する。

ＧＰＳデータによる状態の更新では、ＧＰＳ衛星から受信した信号がマルチパス等の影響により、異常な値になっている可能性がある。そこで、本実施の形態の仮説生成部３３０では、ＧＰＳデータによる観測値が正常値の場合と異常値である場合の２つの仮説をそれぞれ生成し、正常・異常のそれぞれの可能性を考慮しながら状態を追跡する。

具体的には、仮説生成部３３０は、仮説毎に、状態予測部３２０が求めた当該仮説の状態の予測結果

と、観測値ｙ_ｓａｔから、下記のように観測値が正常値であると仮定して更新された状態の仮説と、異常値であると仮定して更新された状態の仮説を求める。

ここで、ｘ^（１） _ｋ｜ｋは観測値が正常値の場合の仮説の状態における確率分布の平均であり、Ｖ^（１） _ｋ｜ｋは観測値が正常の場合の仮説の状態における分散である。また、ｘ^（２） _ｋ｜ｋは観測値が異常値の場合の仮説の状態における確率分布の平均であり、Ｖ^（２） _ｋ｜ｋは、観測値が異常値の場合の仮説の状態における分散である。また、Ｒは観測誤差分散行列である。

また、ｈ_ｓａｔはＧＰＳ衛星の観測モデルであり、以下の式で表される。

ここで、ｒ^＊は衛星とＧＰＳセンサ２００との距離、Ｉは電離層遅延、Ｔは対流圏遅延、Ｃは光速、ｆ_１はＬ１ドップラーシフト周波数を表す。

観測値が異常値の場合の仮説は、観測値ｙ_ｓａｔが異常値である場合を想定しているため、観測値ｙ_ｓａｔを使用しない仮説となる。

また、ＣＡＮデータは異常値が無い観測値であるため、仮説生成部３３０は、ＣＡＮデータが入力された場合には、仮説の各々について、入力された観測値及び状態予測部３２０による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説を生成する。

具体的には、仮説生成部３３０は、状態予測部３２０によって予測された仮説の状態ｘにおける確率分布の平均及び分散

と、観測値ｙ_ＣＡＮから、下記のように仮説が表す状態を更新する。

そして、仮説生成部３３０は、生成した全ての仮説を、重み算出部３４０へ渡す。

重み算出部３４０は、仮説生成部３３０によって生成された複数の仮説のうち、観測値が正常値であると仮定して更新された状態を表す仮説については、観測値が正常値である場合の観測分布に基づいて、仮説の重みを算出し、観測値が異常値であると仮定して更新された状態を表す仮説については、観測値が異常値である場合の観測分布に基づいて仮説の重みを算出する。

具体的には、重み算出部３４０は、観測値が正常値である場合の観測分布として正規分布を用い、観測値が異常値である場合の観測分布としてコーシー分布を用い、下記式により、仮説の重みを算出する。

ここで、ｐ（ｘ^（１） _ｋ｜ｋ）が、観測値が正常値であると仮定して更新された状態を表す仮説の重み、ｐ（ｘ^（２） _ｋ｜ｋ）が、観測値が異常値であると仮定して更新された状態を表す仮説の重みである。

また、ｐ（ｘ_{ｋ｜ｋ−１}）は、１時刻前（ｋ−１）に更新された当該仮説の重みである。また、Ｗ_ｅｒｒは、異常値である観測値の重み係数を表す定数であり、ｅ_ｓａｔは、ＧＰＳデータとＧＰＳ衛星の観測モデルとの差である。

また、Ｈ_ｓａｔは、ＧＰＳ衛星の観測モデルを、状態予測部３２０が観測時刻における状態ｘの予測の平均

でベクトル微分した行列である。Ｒ_ｂｏｄｙは、正常値の観測分布の誤差分散、Ｒ_ｔａｉｌは、異常値の観測分布の誤差分散を表す。

図２は、観測値が正常値である場合の観測分布と観測値が異常値である場合の観測分布とを示している。図２に示すように、観測値が正常値である場合の観測分布では、中心付近での値の確率密度が高い。一方、観測値が異常値である場合の観測分布では、中心より離れた値の確率密度が比較的高い。

すなわち、重み算出部３４０は、観測分布から得られる確率密度を用いて、仮説の重みを算出する。

ここでは、観測値が正常値である場合の観測分布を正規分布としてモデル化し、観測値が異常値である場合の観測分布をコーシー分布としてモデル化しているが、観測値が異常値である場合の観測分布が、観測値が正常値である場合の観測分布よりも裾の重い分布であれば、他の分布を用いてモデル化してもよい。

また、重み算出部３４０は、ＣＡＮデータが入力された場合には、観測値が正常値である場合の観測分布（正規分布）を用いて、各仮説の重みを算出する。

仮説統合部３５０は、仮説生成部３３０によって生成された仮説の数が規定数より多い場合に、規定数以下になるように仮説を統合する。

具体的には、仮説統合部３５０は、仮説生成部３３０によって生成された仮説の数が規定数より多い場合に、規定数以下になるように複数の仮説のうちの２つの仮説を統合することを繰り返す。このとき、仮説統合部３５０は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を統合する。

図３に示すように、仮説生成部３３０によって正常値の場合と異常値のそれぞれの仮説が生成されると、仮説が倍々に増え続けるため、計算できなくなってしまう。このため、観測する度に増える仮説を、適宜統合して、適正な数を保つようにする。

また、単に仮説を減らしたとすると、時系列に追跡する仮説において、有効な仮説をも減らしてしまう可能性がある。

そこで、仮説統合部３５０では、統合しても影響が少ない仮説を統合する。

まず、仮説統合部３５０は、全ての仮説の重みの和が１になるように、各仮説の正規化を行う。具体的には、各仮説の重みの比を計算して正規化を行う。

そして、仮説統合部３５０は、仮説の数が規定数以下になるまで、（１）全ての仮説から最も評価関数を小さくする２つの仮説の組を探索し、（２）探索された２つの仮説を統合して、１つの仮説とする。

ｉ番目の仮説とｊ番目の仮説との組み合わせに対する評価関数としては、例えば、カルバックライブラー情報量の差の上限を元に導出された下記の評価関数Ｂを用いることができる。

ここで、Ｖ^{（ｉ）（ｊ）} _ｋ｜ｋは、ｉ番目の仮説とｊ番目の仮説とを統合した後の分散共分散行列である。

上記の評価関数では、２つの仮説が統合された場合の統合後の仮説のカルバックライブラー情報量と、２つの仮説のカルバックライブラー情報量との差が小さい場合、統合後の仮説と元の２つの仮説とは、確率分布として類似していることを示す。

すなわち、統合後の仮説と確率分布が類似する２つの仮説は、統合してもほとんど分布の形に影響が無いため、この２つの仮説を統合することにより、仮説の数を適切に減少させることができる。

仮説は確率分布で表されるため、２つ仮説を統合する場合には、２つの確率分布の混合分布として統合することができる。

また、統合後の仮説の重みとして、統合前の２つの仮説の重みを加算する。仮説の重みは正規化されているため、そのまま足し合わせることで、統合後の仮説の重みとすることができる。

このように、仮説統合部３５０は、仮説の数が規定数より多い場合に、規定数以下になるように複数の仮説のうちの２つの仮説を統合することを繰り返す。

図４は観測毎に仮説の数が変化する例を示す図である。図４では、規定数を３として、ＧＰＳデータを受信した場合の観測が行われた場合に仮説の数が３を超えると、仮説統合部３５０は、適宜仮説を統合して、仮説の数が３になるように調整している。

また、ＣＡＮデータを受信した場合の観測が行われた場合には、仮説の数は増減しないため、仮説統合部３５０は、仮説の統合を行わない。

そして、仮説統合部３５０は、全ての仮説とそれらの重みを状態推定部３６０に送る。また、全ての仮説を、状態予測部３２０に送る。

状態推定部３６０は、仮説の各々が表す状態と、当該仮説の各々の重みとに基づいて、推定対象の状態を推定する。

具体的には、状態推定部３６０は、仮説統合部３５０から複数の仮説と当該仮説の重みとを受け取ると、複数の仮説の各々が表す状態を重み付け加算することで、状態ｘを推定する。

そして、出力部３７０は、状態推定部３６０が推定した状態ｘに含まれる自車両の位置（緯度・経度）を出力する。

＜状態推定装置３００の動作＞

次に、図５を参照して、本実施形態の状態推定装置３００の状態推定処理ルーチンについて説明する。

まず、ステップＳ１００において、観測値受信部３１０は、ＣＡＮデータを受信したか否かを判定する。

ＣＡＮデータを受信していた場合（ステップＳ１００のＹＥＳ）、ステップＳ１０２において、状態予測部３２０が、推定対象の状態を表す仮説の各々について、受信したＣＡＮデータの観測時点における、当該仮説が表す状態を予測する。

ステップＳ１０４において、受信したＣＡＮデータの観測値ｙ_ＣＡＮと、上記ステップＳ１０２による予測結果とに基づいて、仮説の各々の状態を更新する状態更新処理を行う。

そして、ステップＳ１０６において、状態推定部３６０は、仮説の各々が表す状態と、仮説の各々の重みとに基づいて、状態を推定し、推定した状態に含まれる自車両の位置を出力して、ステップＳ１００に戻る。

ＣＡＮデータを受信していない場合（ステップＳ１００のＮＯ）、ステップＳ１０８において、観測値受信部３１０は、ＧＰＳデータを受信したか否かを判定する。

ＧＰＳデータを受信していた場合（ステップＳ１０８のＹＥＳ）、ステップＳ１１０において、状態予測部３２０が、推定対象の状態を表す仮説の各々について、受信したＧＰＳデータの観測時点における、当該仮説が表す状態を予測する。

ステップＳ１１２において、ＧＰＳデータの観測値ｙ_ｓａｔと、上記ステップＳ１１０による予測結果とに基づいて、仮説の各々の状態を更新する状態更新処理を行う。

そして、ステップＳ１１４において、状態推定部３６０は、仮説の各々が表す状態と、仮説の各々の重みとに基づいて、状態を推定し、推定した状態に含まれる自車両の位置を出力して、ステップＳ１００に戻る。

また、ＧＰＳデータを受信していない場合（ステップＳ１０８のＮＯ）、再度ステップＳ１００に戻り、ＣＡＮデータ又はＧＰＳデータを受信するまで、観測値受信部３１０は、待機する。

また、上記ステップＳ１０４は、図６に示すＣＡＮデータによる状態更新処理ルーチンによって実現される。

ステップＳ２００において、仮説生成部３３０は、仮説の各々について、ＣＡＮデータの観測値ｙ_ＣＡＮと、上記ステップＳ１０２による予測結果とに基づいて、観測値ｙ_ＣＡＮが正常値であると仮定して、当該仮説が表す状態を更新する。

ステップＳ２１０において、重み算出部３４０は、上記ステップＳ２００で更新された仮説の各々について、観測値ｙ_ＣＡＮが正常値である場合の観測分布に基づいて、当該仮説の重みを算出して、リターンする。

また、上記ステップＳ１１２は、図７に示すＧＰＳデータによる状態更新処理ルーチンにより実現される。

ステップＳ３００において、仮説生成部３３０は、仮説の各々について、ＧＰＳデータの観測値ｙ_ｓａｔと、上記ステップＳ１１０による予測結果とに基づいて、観測値ｙ_ｓａｔが正常値であると仮定して更新された状態を表す仮説と、観測値ｙ_ｓａｔが異常値であると仮定して更新された状態を表す仮説とを生成する。

ステップＳ３１０において、重み算出部３４０は、上記ステップＳ３００で生成された複数の仮説のうち、観測値ｙ_ｓａｔが正常値であると仮定して更新された状態を表す仮説の各々については、観測値ｙ_ｓａｔが正常値である場合の観測分布に基づいて、当該仮説の重みを算出し、観測値ｙ_ｓａｔが異常値であると仮定して更新された状態を表す仮説の各々については、観測値ｙ_ｓａｔが異常値である場合の観測分布に基づいて当該仮説の重みを算出する。

ステップＳ３２０において、仮説統合部３５０は、上記ステップＳ３００で生成された仮説の数が、規定数以下か否かを判定する。

仮説の数が規定数よりも大きい場合（ステップＳ３２０のＮＯ）、ステップＳ３３０において、仮説統合部３５０は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を探索する。

そして、ステップＳ３４０において、仮説統合部３５０は、探索した２つの仮説を統合する。

ステップＳ３５０において、重み算出部３４０は、仮説統合部３５０が統合した仮説の重みを算出し、ステップＳ３２０へ戻る。

仮説の数が規定数以下である場合（ステップＳ３２０のＹＥＳ）、リターンする。

以上説明したように、本発明の実施の形態に係る状態推定装置によれば、推定対象の状態を表す仮説の各々について、観測時点における、当該仮説が表す状態を予測し、仮説の各々について、入力された観測値及び状態予測部による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、生成された仮説の数が規定数より多い場合には、規定数以下になるように仮説を統合するため、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる。

また、異常値が混在するセンサデータのうち、特に時間的に追跡しないと異常が判定できないデータに対して、観測値が正常値の場合と異常値の場合、双方の仮説を追跡することにより、ロバストに状態を推定することができる。特に、マルチパス（建物などに反射した電波）が発生するＧＰＳデータによる測位において有効である。

また、観測毎に仮説が増大してしまうため、それらの仮説を適宜統合することにより、仮説が増大することにより弊害を防ぐことができる。

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

例えば、上記実施の形態では、センサとして、ＧＰＳセンサとＣＡＮセンサとを用いた場合を例に説明したが、センサとして、ＧＰＳセンサのみを用いて構成してもよい。

また、センサとして、ミリ波センサ、レーザー、カメラなどを用いて、物体検出を行うための物体検出装置に、本発明を適用してもよい。この場合には、推定対象の状態が、検出された物体の位置や大きさなどを含んでいればよい。

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

１００ＣＡＮ受信機
２００ＧＰＳセンサ
３００状態推定装置
３１０観測値受信部
３２０状態予測部
３３０仮説生成部
３４０算出部
３５０仮説統合部
３６０状態推定部
３７０出力部

Claims

センサの観測値に基づいて推定対象の状態を推定する状態推定装置であって、
前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部と、
前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部と、
前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部と、
複数の前記仮説のうち、前記観測値が正常値であると仮定して更新された状態を表す仮説については、前記観測値が正常値である場合の観測分布に基づいて、前記仮説の重みを算出し、前記観測値が異常値であると仮定して更新された状態を表す仮説については、前記観測値が異常値である場合の観測分布として正常値の観測分布よりも裾の重い分布に基づいて仮説の重みを算出する重み算出部と、
前記仮説の各々が表す状態と、前記仮説の各々の重みとに基づいて、前記推定対象の状態を推定する状態推定部と、
を含む状態推定装置。
前記仮説の各々が表す状態は、ガウシアンフィルタリングを用いて、ガウス分布で表現される請求項１記載の状態推定装置。
前記仮説統合部は、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説のうちの２つの仮説を統合することを繰り返す請求項１又は２記載の状態推定装置。
前記仮説統合部は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を統合する請求項１乃至３の何れか１項記載の状態推定装置。
前記センサが、ＧＰＳセンサであり、
前記推定対象の状態が、自装置の位置である請求項１乃至４の何れか１項記載の状態推定装置。
センサの観測値に基づいて推定対象の状態を推定するためのプログラムであって、
コンピュータを、
前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部、
前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部、
前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部、
複数の前記仮説のうち、前記観測値が正常値であると仮定して更新された状態を表す仮説については、前記観測値が正常値である場合の観測分布に基づいて、前記仮説の重みを算出し、前記観測値が異常値であると仮定して更新された状態を表す仮説については、前記観測値が異常値である場合の観測分布として正常値の観測分布よりも裾の重い分布に基づいて仮説の重みを算出する重み算出部、及び
前記仮説の各々が表す状態と、前記仮説の各々の重みとに基づいて、前記推定対象の状態を推定する状態推定部
として機能させるためのプログラム。