(57)【要約】【課題】時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができるようにする。【解決手段】センサの観測値に基づいて推定対象の状態を推定する状態推定装置３００であって、状態予測部３２０が、センサによる観測値が入力される毎に、推定対象の状態を表す仮説の各々について、観測時点における、当該仮説が表す状態を予測し、仮説生成部３３０が、仮説の各々について、入力された観測値及び状態予測部３２０による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、仮説統合部３５０が、仮説生成部３３０によって生成された仮説の数が規定数より多い場合に、規定数以下になるように仮説を統合し、状態推定部３６０が、仮説の各々が表す状態に基づいて、推定対象の状態を推定する。【選択図】図１
2019020338.tif 000002
【特許請求の範囲】
【請求項１】
センサの観測値に基づいて推定対象の状態を推定する状態推定装置であって、
前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部と、
前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部と、
前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部と、
前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部と、
を含む状態推定装置。
【請求項２】
前記仮説生成部によって生成された複数の仮説のうち、前記観測値が正常値であると仮定して更新された状態を表す仮説については、前記観測値が正常値である場合の観測分布に基づいて、前記仮説の重みを算出し、前記観測値が異常値であると仮定して更新された状態を表す仮説については、前記観測値が異常値である場合の観測分布に基づいて仮説の重みを算出する重み算出部を更に含み、
前記状態推定部は、前記仮説の各々が表す状態と、前記仮説の各々の重みとに基づいて、前記推定対象の状態を推定する請求項１記載の状態推定装置。
【請求項３】
前記仮説統合部は、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説のうちの２つの仮説を統合することを繰り返す請求項１又は２記載の状態推定装置。
【請求項４】
前記仮説統合部は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を統合する請求項１乃至３の何れか１項記載の状態推定装置。
【請求項５】
前記センサが、ＧＰＳセンサであり、
前記推定対象の状態が、自装置の位置である請求項１乃至４の何れか１項記載の状態推定装置。
【請求項６】
前記仮説が表す状態は、ガウス分布で表現される請求項１乃至５の何れか１項記載の状態推定装置。
【請求項７】
センサの観測値に基づいて推定対象の状態を推定するためのプログラムであって、
コンピュータを、
前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部、
前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部、
前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部、及び
前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、観測を正常値と異常値の確率分布として表現する研究がなされている（非特許文献１）。また、複数の仮説を統合して、複数の仮説の追跡に利用する技術がある（非特許文献２）。
【０００３】
また、ＧＰＳ観測の異常値を判定し、取り除く技術がある（特許文献１）。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】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．
【特許文献】
【０００５】
【特許文献１】特開２０１２−０６３３１３号広報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記非特許文献１の技術は、重ね合わせた分布をもとの１つの分布としてしか追跡しないため、瞬間的な異常値は判定できるが、時間的追跡をしなければ異常値を判定することができない観測値に対して有用でない。
【０００７】
また、非特許文献２では、観測を正常値と異常値の確率分布として表現するものではないため、非特許文献１に適用して、時系列的に異常値を取り除くことは困難である。
【０００８】
特許文献１では、異常値を取り除いているが、非特許文献１と同様に時間的追跡をしなければ異常値を判定することができない観測値に対して有用でない。
【０００９】
本発明は、上記問題点を解決するためになされたもので、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる状態推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記の目的を達成するために、本発明の状態推定装置は、センサの観測値に基づいて推定対象の状態を推定する状態推定装置であって、前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部と、前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部と、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部と、前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部と、を含んで構成されている。
【００１１】
また、本発明のプログラムは、センサの観測値に基づいて推定対象の状態を推定するためのプログラムであって、コンピュータを、前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測する状態予測部、前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部、及び前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部として機能させるためのプログラムである。
【００１２】
本発明の状態予測装置及びプログラムによれば、状態予測部が、前記センサによる観測値が入力される毎に、前記推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測し、仮説生成部が、前記仮説の各々について、前記入力された観測値及び前記状態予測部による予測結果に基づいて、前記観測値が正常値であると仮定して更新された状態を表す仮説と、前記観測値が異常値であると仮定して更新された状態を表す仮説とを生成する。
【００１３】
そして、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説を統合する仮説統合部と、前記仮説の各々が表す状態に基づいて、前記推定対象の状態を推定する状態推定部する。
【００１４】
このように、推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測し、仮説の各々について、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、生成された仮説の数が規定数より多い場合には、規定数以下になるように仮説を統合することにより、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる。
【００１５】
また、本発明の状態推定装置は、前記仮説生成部によって生成された複数の仮説のうち、前記観測値が正常値であると仮定して更新された状態を表す仮説については、前記観測値が正常値である場合の観測分布に基づいて、前記仮説の重みを算出し、前記観測値が異常値であると仮定して更新された状態を表す仮説については、前記観測値が異常値である場合の観測分布に基づいて仮説の重みを算出する重み算出部を更に含み、前記状態推定部は、前記仮説の各々が表す状態と、前記仮説の各々の重みとに基づいて、前記推定対象の状態を推定することができる。
【００１６】
また、本発明の状態推定装置の前記推定部は、前記仮説統合部は、前記仮説生成部によって生成された仮説の数が規定数より多い場合に、前記規定数以下になるように前記仮説のうちの２つの仮説を統合することを繰り返すことができる。
【００１７】
また、本発明の状態推定装置の前記仮説統合部は、前記仮説統合部は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を統合することができる。
【００１８】
また、本発明の状態推定装置は、前記センサが、ＧＰＳセンサであり、前記推定対象の状態が、自装置の位置であるとすることができる。
【００１９】
また、本発明の状態推定装置は、前記仮説が表す状態が、ガウス分布で表現されることができる。
【発明の効果】
【００２０】
以上説明したように、本発明の状態推定装置及びプログラムによれば、推定対象の状態を表す仮説の各々について、観測時点における、前記仮説が表す状態を予測し、仮説の各々について、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、生成された仮説の数が規定数より多い場合には、規定数以下になるように仮説を統合することにより、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２１】
【図１】本発明の第１の実施の形態における概略構成を示すブロック図である。
【図２】観測分布の例を示す図である。
【図３】仮説を適宜統合する例を示す図である。
【図４】観測毎に仮説の数が変化する例を示す図である。
【図５】本発明の第１の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。
【図６】本発明の第１の実施の形態におけるＣＡＮデータによる状態更新処理ルーチンの内容を示すフローチャートである。
【図７】本発明の第１の実施の形態におけるＧＰＳデータによる状態更新処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２２】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２３】
＜本発明の実施の形態の概要＞
【００２４】
本発明の実施の形態では、車両に搭載されたＣＡＮ受信機及びＧＰＳセンサからの観測値に基づいて、車両の位置を推定する状態推定装置に本発明を適用した場合を例に説明する。
【００２５】
本実施形態では、複数のセンサ間でフレーム同期をしないアプローチをとっている。ＧＰＳセンサやＣＡＮ経由で得られる車速計等の車載センサのセンシング結果である観測値を受信したタイミング毎に、受信した観測値に基づいて、車両の位置を示す状態量の仮説を生成し、車両の位置を推定する。
【００２６】
ここで、時間的追跡をしなければ、観測値がマルチパス等によって異常値を含むことを判定できないが、１台のセンサの観測値を使って逐次車両の位置を推定するため、異常値を判定することができない、という問題がある。
【００２７】
この問題を解決するために、本発明の実施の形態では、観測値が正常値である場合と、観測値が異常値である場合の仮説をそれぞれ生成する。
【００２８】
また、仮説の数が増加し過ぎた場合には、計算が困難になったり、不可能になったりするため、適宜仮説を統合する。
【００２９】
＜状態推定システムのシステム構成＞
【００３０】
図１に示すように、本発明の実施の形態に係る状態推定システムは、ＣＡＮ受信機１００と、ＧＰＳセンサ２００と、状態推定装置３００とを備えて構成される。
【００３１】
ＣＡＮ受信機１００は、ＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）から得られる車載センサ（車速計、ジャイロ）が観測した観測値を受信する。受信した観測値のうち、少なくとも車両速度及びヨーレートを含んだものをＣＡＮデータとする。
【００３２】
そして、ＣＡＮ受信機１００は、受信したＣＡＮデータを状態推定装置３００に出力する。
【００３３】
ＧＰＳセンサ２００は、各ＧＰＳ衛星から得られる観測値であるＧＰＳデータを受信する。ＧＰＳデータには、疑似距離及びドップラー周波数が含まれている。そして、ＧＰＳセンサ２００は、受信したＧＰＳデータを状態推定装置３００に出力する。
【００３４】
状態推定装置３００は、センサの観測値に基づいて自車両の位置を推定する装置である。本発明の実施の形態では、状態推定装置３００の内部において、自車両の位置を含む、以下の状態ｘを推定する。
【００３５】
【数１】
2019020338.tif 000003
【００３６】
ここで、ｘ、ｙ、ｚは地球中心座標での位置を表し、
2019020338.tif 000004
はＧＰＳ受信機の時刻のずれ量を表し、
2019020338.tif 000005
はヨーレートセンサの零点のずれ量（オフセット）を表す。
【００３７】
本実施の形態では、状態推定装置３００はガウシアンフィルタリングを用いて、状態ｘをガウス分布として推定する。このため、状態ｘとして、確率分布の平均
2019020338.tif 000006
と分散Ｖとを推定する。
【００３８】
状態推定装置３００は、ＣＰＵと、ＲＡＭと、後述する状態推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００３９】
状態推定装置３００は、観測値受信部３１０と、状態予測部３２０と、仮説生成部３３０と、重み算出部３４０と、仮説統合部３５０と、状態推定部３６０と、出力部３７０とを備えて構成される。
【００４０】
観測値受信部３１０は、ＣＡＮ受信機１００から出力されたＣＡＮデータ、及びＧＰＳセンサ２００から出力されたＧＰＳデータを受信する。
【００４１】
【数２】
2019020338.tif 000007
【００４２】
ここで、ｙｓａｔはＧＰＳデータ、ｙＣＡＮはＣＡＮデータにおける観測値である。
【００４３】
そして、観測値受信部３１０は、各データを受信する毎に、受信したデータを状態予測部３２０へ送る。
【００４４】
状態予測部３２０は、ＧＰＳデータ又はＣＡＮデータが入力される毎に、推定対象の状態を表す仮説の各々について、観測時点における、当該仮説が表す状態を予測する。
【００４５】
具体的には、状態予測部３２０は、ＧＰＳデータ又はＣＡＮデータを観測値受信部３１０から受け取ると、各データが入力される毎に、推定対象である自車両の位置を含む状態ｘの仮説の各々について、以下のように観測時点における状態ｘを予測する。
【００４６】
まず、状態予測部３２０は、前回生成された仮説の全てを、仮説統合部３５０から取得しておく。
【００４７】
そして、状態予測部３２０は、取得した仮説の各々について、等速予測モデルなどの物理モデルを用いて当該状態ｘの予測を行う。本実施形態では、下記の予測モデルを用いて観測時点における仮説の状態ｘを予測する。
【００４８】
【数３】
2019020338.tif 000008
【００４９】
ここで、Ｑは予測モデルの誤差共分散行列、ｋはステップを表し、Δｔｋはステップ間の時間幅を表す。また、ｖｘｙｚはｘｙｚ座標での速度ベクトル、Ｖｖｘｙｚはその分散共分散行列を表す。
【００５０】
また、
2019020338.tif 000009
は、前回生成された仮説の状態における確率分布の平均及び分散を表す。
【００５１】
仮説生成部３３０は、ＧＰＳデータが入力された場合には、仮説の各々について、入力された観測値及び状態予測部３２０による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成する。
【００５２】
ＧＰＳデータによる状態の更新では、ＧＰＳ衛星から受信した信号がマルチパス等の影響により、異常な値になっている可能性がある。そこで、本実施の形態の仮説生成部３３０では、ＧＰＳデータによる観測値が正常値の場合と異常値である場合の２つの仮説をそれぞれ生成し、正常・異常のそれぞれの可能性を考慮しながら状態を追跡する。
【００５３】
具体的には、仮説生成部３３０は、仮説毎に、状態予測部３２０が求めた当該仮説の状態の予測結果
2019020338.tif 000010
と、観測値ｙｓａｔから、下記のように観測値が正常値であると仮定して更新された状態の仮説と、異常値であると仮定して更新された状態の仮説を求める。
【００５４】
【数４】
2019020338.tif 000011
【００５５】
ここで、ｘ（１）ｋ｜ｋは観測値が正常値の場合の仮説の状態における確率分布の平均であり、Ｖ（１）ｋ｜ｋは観測値が正常の場合の仮説の状態における分散である。また、ｘ（２）ｋ｜ｋは観測値が異常値の場合の仮説の状態における確率分布の平均であり、Ｖ（２）ｋ｜ｋは、観測値が異常値の場合の仮説の状態における分散である。また、Ｒは観測誤差分散行列である。
【００５６】
また、ｈｓａｔはＧＰＳ衛星の観測モデルであり、以下の式で表される。
【００５７】
【数５】
2019020338.tif 000012
【００５８】
ここで、ｒ＊は衛星とＧＰＳセンサ２００との距離、Ｉは電離層遅延、Ｔは対流圏遅延、Ｃは光速、ｆ１はＬ１ドップラーシフト周波数を表す。
【００５９】
観測値が異常値の場合の仮説は、観測値ｙｓａｔが異常値である場合を想定しているため、観測値ｙｓａｔを使用しない仮説となる。
【００６０】
また、ＣＡＮデータは異常値が無い観測値であるため、仮説生成部３３０は、ＣＡＮデータが入力された場合には、仮説の各々について、入力された観測値及び状態予測部３２０による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説を生成する。
【００６１】
具体的には、仮説生成部３３０は、状態予測部３２０によって予測された仮説の状態ｘにおける確率分布の平均及び分散
2019020338.tif 000013
と、観測値ｙＣＡＮから、下記のように仮説が表す状態を更新する。
【００６２】
【数６】
2019020338.tif 000014
【００６３】
そして、仮説生成部３３０は、生成した全ての仮説を、重み算出部３４０へ渡す。
【００６４】
重み算出部３４０は、仮説生成部３３０によって生成された複数の仮説のうち、観測値が正常値であると仮定して更新された状態を表す仮説については、観測値が正常値である場合の観測分布に基づいて、仮説の重みを算出し、観測値が異常値であると仮定して更新された状態を表す仮説については、観測値が異常値である場合の観測分布に基づいて仮説の重みを算出する。
【００６５】
具体的には、重み算出部３４０は、観測値が正常値である場合の観測分布として正規分布を用い、観測値が異常値である場合の観測分布としてコーシー分布を用い、下記式により、仮説の重みを算出する。
【００６６】
【数７】
2019020338.tif 000015
【００６７】
ここで、ｐ（ｘ（１）ｋ｜ｋ）が、観測値が正常値であると仮定して更新された状態を表す仮説の重み、ｐ（ｘ（２）ｋ｜ｋ）が、観測値が異常値であると仮定して更新された状態を表す仮説の重みである。
【００６８】
また、ｐ（ｘｋ｜ｋ−１）は、１時刻前（ｋ−１）に更新された当該仮説の重みである。また、Ｗｅｒｒは、異常値である観測値の重み係数を表す定数であり、ｅｓａｔは、ＧＰＳデータとＧＰＳ衛星の観測モデルとの差である。
【００６９】
また、Ｈｓａｔは、ＧＰＳ衛星の観測モデルを、状態予測部３２０が観測時刻における状態ｘの予測の平均
2019020338.tif 000016
でベクトル微分した行列である。Ｒｂｏｄｙは、正常値の観測分布の誤差分散、Ｒｔａｉｌは、異常値の観測分布の誤差分散を表す。
【００７０】
図２は、観測値が正常値である場合の観測分布と観測値が異常値である場合の観測分布とを示している。図２に示すように、観測値が正常値である場合の観測分布では、中心付近での値の確率密度が高い。一方、観測値が異常値である場合の観測分布では、中心より離れた値の確率密度が比較的高い。
【００７１】
すなわち、重み算出部３４０は、観測分布から得られる確率密度を用いて、仮説の重みを算出する。
【００７２】
ここでは、観測値が正常値である場合の観測分布を正規分布としてモデル化し、観測値が異常値である場合の観測分布をコーシー分布としてモデル化しているが、観測値が異常値である場合の観測分布が、観測値が正常値である場合の観測分布よりも裾の重い分布であれば、他の分布を用いてモデル化してもよい。
【００７３】
また、重み算出部３４０は、ＣＡＮデータが入力された場合には、観測値が正常値である場合の観測分布（正規分布）を用いて、各仮説の重みを算出する。
【００７４】
仮説統合部３５０は、仮説生成部３３０によって生成された仮説の数が規定数より多い場合に、規定数以下になるように仮説を統合する。
【００７５】
具体的には、仮説統合部３５０は、仮説生成部３３０によって生成された仮説の数が規定数より多い場合に、規定数以下になるように複数の仮説のうちの２つの仮説を統合することを繰り返す。このとき、仮説統合部３５０は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を統合する。
【００７６】
図３に示すように、仮説生成部３３０によって正常値の場合と異常値のそれぞれの仮説が生成されると、仮説が倍々に増え続けるため、計算できなくなってしまう。このため、観測する度に増える仮説を、適宜統合して、適正な数を保つようにする。
【００７７】
また、単に仮説を減らしたとすると、時系列に追跡する仮説において、有効な仮説をも減らしてしまう可能性がある。
【００７８】
そこで、仮説統合部３５０では、統合しても影響が少ない仮説を統合する。
【００７９】
まず、仮説統合部３５０は、全ての仮説の重みの和が１になるように、各仮説の正規化を行う。具体的には、各仮説の重みの比を計算して正規化を行う。
【００８０】
そして、仮説統合部３５０は、仮説の数が規定数以下になるまで、（１）全ての仮説から最も評価関数を小さくする２つの仮説の組を探索し、（２）探索された２つの仮説を統合して、１つの仮説とする。
【００８１】
ｉ番目の仮説とｊ番目の仮説との組み合わせに対する評価関数としては、例えば、カルバックライブラー情報量の差の上限を元に導出された下記の評価関数Ｂを用いることができる。
【００８２】
【数８】
2019020338.tif 000017
【００８３】
ここで、Ｖ（ｉ）（ｊ）ｋ｜ｋは、ｉ番目の仮説とｊ番目の仮説とを統合した後の分散共分散行列である。
【００８４】
上記の評価関数では、２つの仮説が統合された場合の統合後の仮説のカルバックライブラー情報量と、２つの仮説のカルバックライブラー情報量との差が小さい場合、統合後の仮説と元の２つの仮説とは、確率分布として類似していることを示す。
【００８５】
すなわち、統合後の仮説と確率分布が類似する２つの仮説は、統合してもほとんど分布の形に影響が無いため、この２つの仮説を統合することにより、仮説の数を適切に減少させることができる。
【００８６】
仮説は確率分布で表されるため、２つ仮説を統合する場合には、２つの確率分布の混合分布として統合することができる。
【００８７】
また、統合後の仮説の重みとして、統合前の２つの仮説の重みを加算する。仮説の重みは正規化されているため、そのまま足し合わせることで、統合後の仮説の重みとすることができる。
【００８８】
このように、仮説統合部３５０は、仮説の数が規定数より多い場合に、規定数以下になるように複数の仮説のうちの２つの仮説を統合することを繰り返す。
【００８９】
図４は観測毎に仮説の数が変化する例を示す図である。図４では、規定数を３として、ＧＰＳデータを受信した場合の観測が行われた場合に仮説の数が３を超えると、仮説統合部３５０は、適宜仮説を統合して、仮説の数が３になるように調整している。
【００９０】
また、ＣＡＮデータを受信した場合の観測が行われた場合には、仮説の数は増減しないため、仮説統合部３５０は、仮説の統合を行わない。
【００９１】
そして、仮説統合部３５０は、全ての仮説とそれらの重みを状態推定部３６０に送る。また、全ての仮説を、状態予測部３２０に送る。
【００９２】
状態推定部３６０は、仮説の各々が表す状態と、当該仮説の各々の重みとに基づいて、推定対象の状態を推定する。
【００９３】
具体的には、状態推定部３６０は、仮説統合部３５０から複数の仮説と当該仮説の重みとを受け取ると、複数の仮説の各々が表す状態を重み付け加算することで、状態ｘを推定する。
【００９４】
そして、出力部３７０は、状態推定部３６０が推定した状態ｘに含まれる自車両の位置（緯度・経度）を出力する。
【００９５】
＜状態推定装置３００の動作＞
【００９６】
次に、図５を参照して、本実施形態の状態推定装置３００の状態推定処理ルーチンについて説明する。
【００９７】
まず、ステップＳ１００において、観測値受信部３１０は、ＣＡＮデータを受信したか否かを判定する。
【００９８】
ＣＡＮデータを受信していた場合（ステップＳ１００のＹＥＳ）、ステップＳ１０２において、状態予測部３２０が、推定対象の状態を表す仮説の各々について、受信したＣＡＮデータの観測時点における、当該仮説が表す状態を予測する。
【００９９】
ステップＳ１０４において、受信したＣＡＮデータの観測値ｙＣＡＮと、上記ステップＳ１０２による予測結果とに基づいて、仮説の各々の状態を更新する状態更新処理を行う。
【０１００】
そして、ステップＳ１０６において、状態推定部３６０は、仮説の各々が表す状態と、仮説の各々の重みとに基づいて、状態を推定し、推定した状態に含まれる自車両の位置を出力して、ステップＳ１００に戻る。
【０１０１】
ＣＡＮデータを受信していない場合（ステップＳ１００のＮＯ）、ステップＳ１０８において、観測値受信部３１０は、ＧＰＳデータを受信したか否かを判定する。
【０１０２】
ＧＰＳデータを受信していた場合（ステップＳ１０８のＹＥＳ）、ステップＳ１１０において、状態予測部３２０が、推定対象の状態を表す仮説の各々について、受信したＧＰＳデータの観測時点における、当該仮説が表す状態を予測する。
【０１０３】
ステップＳ１１２において、ＧＰＳデータの観測値ｙｓａｔと、上記ステップＳ１１０による予測結果とに基づいて、仮説の各々の状態を更新する状態更新処理を行う。
【０１０４】
そして、ステップＳ１１４において、状態推定部３６０は、仮説の各々が表す状態と、仮説の各々の重みとに基づいて、状態を推定し、推定した状態に含まれる自車両の位置を出力して、ステップＳ１００に戻る。
【０１０５】
また、ＧＰＳデータを受信していない場合（ステップＳ１０８のＮＯ）、再度ステップＳ１００に戻り、ＣＡＮデータ又はＧＰＳデータを受信するまで、観測値受信部３１０は、待機する。
【０１０６】
また、上記ステップＳ１０４は、図６に示すＣＡＮデータによる状態更新処理ルーチンによって実現される。
【０１０７】
ステップＳ２００において、仮説生成部３３０は、仮説の各々について、ＣＡＮデータの観測値ｙＣＡＮと、上記ステップＳ１０２による予測結果とに基づいて、観測値ｙＣＡＮが正常値であると仮定して、当該仮説が表す状態を更新する。
【０１０８】
ステップＳ２１０において、重み算出部３４０は、上記ステップＳ２００で更新された仮説の各々について、観測値ｙＣＡＮが正常値である場合の観測分布に基づいて、当該仮説の重みを算出して、リターンする。
【０１０９】
また、上記ステップＳ１１２は、図７に示すＧＰＳデータによる状態更新処理ルーチンにより実現される。
【０１１０】
ステップＳ３００において、仮説生成部３３０は、仮説の各々について、ＧＰＳデータの観測値ｙｓａｔと、上記ステップＳ１１０による予測結果とに基づいて、観測値ｙｓａｔが正常値であると仮定して更新された状態を表す仮説と、観測値ｙｓａｔが異常値であると仮定して更新された状態を表す仮説とを生成する。
【０１１１】
ステップＳ３１０において、重み算出部３４０は、上記ステップＳ３００で生成された複数の仮説のうち、観測値ｙｓａｔが正常値であると仮定して更新された状態を表す仮説の各々については、観測値ｙｓａｔが正常値である場合の観測分布に基づいて、当該仮説の重みを算出し、観測値ｙｓａｔが異常値であると仮定して更新された状態を表す仮説の各々については、観測値ｙｓａｔが異常値である場合の観測分布に基づいて当該仮説の重みを算出する。
【０１１２】
ステップＳ３２０において、仮説統合部３５０は、上記ステップＳ３００で生成された仮説の数が、規定数以下か否かを判定する。
【０１１３】
仮説の数が規定数よりも大きい場合（ステップＳ３２０のＮＯ）、ステップＳ３３０において、仮説統合部３５０は、統合後の仮説と、統合前の２つの仮説とが類似することを表す評価関数の値が最も高い値を示す２つの仮説を探索する。
【０１１４】
そして、ステップＳ３４０において、仮説統合部３５０は、探索した２つの仮説を統合する。
【０１１５】
ステップＳ３５０において、重み算出部３４０は、仮説統合部３５０が統合した仮説の重みを算出し、ステップＳ３２０へ戻る。
【０１１６】
仮説の数が規定数以下である場合（ステップＳ３２０のＹＥＳ）、リターンする。
【０１１７】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、推定対象の状態を表す仮説の各々について、観測時点における、当該仮説が表す状態を予測し、仮説の各々について、入力された観測値及び状態予測部による予測結果に基づいて、観測値が正常値であると仮定して更新された状態を表す仮説と、観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、生成された仮説の数が規定数より多い場合には、規定数以下になるように仮説を統合するため、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる。
【０１１８】
また、異常値が混在するセンサデータのうち、特に時間的に追跡しないと異常が判定できないデータに対して、観測値が正常値の場合と異常値の場合、双方の仮説を追跡することにより、ロバストに状態を推定することができる。特に、マルチパス（建物などに反射した電波）が発生するＧＰＳデータによる測位において有効である。
【０１１９】
また、観測毎に仮説が増大してしまうため、それらの仮説を適宜統合することにより、仮説が増大することにより弊害を防ぐことができる。
【０１２０】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１２１】
例えば、上記実施の形態では、センサとして、ＧＰＳセンサとＣＡＮセンサとを用いた場合を例に説明したが、センサとして、ＧＰＳセンサのみを用いて構成してもよい。
【０１２２】
また、センサとして、ミリ波センサ、レーザー、カメラなどを用いて、物体検出を行うための物体検出装置に、本発明を適用してもよい。この場合には、推定対象の状態が、検出された物体の位置や大きさなどを含んでいればよい。
【０１２３】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０１２４】
１００ ＣＡＮ受信機
２００ ＧＰＳセンサ
３００ 状態推定装置
３１０ 観測値受信部
３２０ 状態予測部
３３０ 仮説生成部
３４０ 算出部
３５０ 仮説統合部
３６０ 状態推定部
３７０ 出力部
【図１】
2019020338.tif 000018
【図２】
2019020338.tif 000019
【図３】
2019020338.tif 000020
【図４】
2019020338.tif 000021
【図５】
2019020338.tif 000022
【図６】
2019020338.tif 000023
【図７】
2019020338.tif 000024