JP7491065B2

JP7491065B2 - 状態推定装置、及び状態推定方法、状態推定プログラム

Info

Publication number: JP7491065B2
Application number: JP2020097782A
Authority: JP
Inventors: 峻田口
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2020-06-04
Filing date: 2020-06-04
Publication date: 2024-05-28
Anticipated expiration: 2040-06-04
Also published as: JP2021189131A

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．

特開２０１２－０６３３１３号公報特開２０１９－２０３３８号公報

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

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

また、非特許文献１及び非特許文献２ではＧＰＳデータに対して適用することは考慮されていない。

ここで、特許文献２の技術によれば、時間的追跡をしなければ異常が判定できない観測値であっても、ロバストに状態を推定することができる。もっとも、特許文献２の技術を用いた場合であっても、複数の観測が同時に得られる際、逐次的に処理した場合に、処理順により結果が異なってしまうという問題が生じ得る。

本開示は、上記事情を鑑みて成されたものであり、複数の観測値が同時に観測された場合であっても、ロバストに状態を推定することができる状態推定装置、及び状態推定方法、状態推定プログラムを提供することを目的とする。

上記目的を達成するために、本開示に係る状態推定装置は、センサの観測値に基づいて推定対象の状態を推定する状態推定装置であって、前記センサによる観測において複数の観測値の各々が同時に得られた場合に、前記センサにより同時に観測された複数の観測値の各々を入力として、任意の第１観測、及び前記第１観測より後に統合する第２観測を識別する受信部と、観測前の状態を示す状態変数を保存する保存部と、前記推定対象の状態を表す仮説について、観測時点における、前記仮説が表す状態を予測する状態予測部と、前記第１観測について、前記状態変数、前記第１観測の第１観測値、及び前記状態予測部による予測結果に基づいて、当該第１観測値が正常値であると仮定して更新された状態を表す仮説と、当該第１観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部と、前記仮説生成部によって生成された前記仮説の各々を統合することにより前記推定対象の状態を推定する仮説統合部と、を含み、前記仮説生成部は、前記第２観測について、前記状態変数、前記第２観測の第２観測値、及び前記仮説統合部による前記第１観測についての統合結果に基づいて、当該第２観測値が正常値であると仮定して更新された状態を表す仮説と、当該第２観測値が異常値であると仮定して更新された状態を表す仮説とを生成する。

本開示に係る状態推定方法は、センサの観測値に基づいて推定対象の状態を推定する状態推定装置における状態推定方法であって、前記センサによる観測において複数の観測値の各々が同時に得られた場合に、前記センサにより同時に観測された複数の観測値の各々を入力として、任意の第１観測、及び前記第１観測より後に統合する第２観測を識別し、観測前の状態を示す状態変数を保存し、前記推定対象の状態を表す仮説について、観測時点における、前記仮説が表す状態を予測し、前記第１観測について、前記状態変数、前記第１観測の第１観測値、及び前記予測結果に基づいて、当該第１観測値が正常値であると仮定して更新された状態を表す仮説と、当該第１観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、前記第１観測について、生成された前記仮説の各々を統合し、前記第２観測について、前記状態変数、前記第２観測の第２観測値、及び前記第１観測についての統合結果に基づいて、当該第２観測値が正常値であると仮定して更新された状態を表す仮説と、当該第２観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、前記第２観測について、生成された前記仮説の各々を統合することにより前記推定対象の状態を推定する、処理をコンピュータに実行させる。

本開示に係る状態推定プログラムは、入力部が、センサの観測値に基づいて推定対象の状態を推定する状態推定装置における状態推定プログラムであって、前記センサによる観測において複数の観測値の各々が同時に得られた場合に、前記センサにより同時に観測された複数の観測値の各々を入力として、任意の第１観測、及び前記第１観測より後に統合する第２観測を識別し、観測前の状態を示す状態変数を保存し、前記推定対象の状態を表す仮説について、観測時点における、前記仮説が表す状態を予測し、前記第１観測について、前記状態変数、前記第１観測の第１観測値、及び前記予測結果に基づいて、当該第１観測値が正常値であると仮定して更新された状態を表す仮説と、当該第１観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、前記第１観測について、生成された前記仮説の各々を統合し、前記第２観測について、前記状態変数、前記第２観測の第２観測値、及び前記仮説統合部による前記第１観測についての統合結果に基づいて、当該第２観測値が正常値であると仮定して更新された状態を表す仮説と、当該第２観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、前記第２観測について、生成された前記仮説の各々を統合することにより前記推定対象の状態を推定する、処理をコンピュータに実行させる。

本開示の状態推定装置、及び状態推定方法、状態推定プログラムによれば、複数の観測値が同時に観測された場合であっても、ロバストに状態を推定することができる。

本開示の実施形態に係る概略構成を示すブロック図である。本開示の実施形態に係る仮説の生成及び統合の流れを示す図である。観測値が正常値である場合の観測分布と観測値が異常値である場合の観測分布とを示す図である。状態推定装置のハードウェア構成を示すブロック図である。本開示の実施形態に係る状態推定装置の状態推定処理ルーチンの内容を示すフローチャートである。本開示の実施形態に係るＣＡＮデータによる状態更新処理ルーチンの内容を示すフローチャートである。本開示の実施形態に係るＧＰＳデータによる状態更新統合処理ルーチンの内容を示すフローチャートである。

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

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

本開示の技術は、前提として特許文献２に関連した技術を用いている。すなわち複数のセンサ間でフレーム同期をしないアプローチをとっている。ＧＰＳセンサやＣＡＮ経由で得られる車速計等の車載センサのセンシング結果である観測値を受信したタイミングごとに、受信した観測値に基づいて、車両の位置を示す状態量の仮説を生成し、車両の位置を推定する。

ここで、特許文献２における前提として異常値の判定に関する問題がある。観測値がマルチパス（建物などに反射した電波）等によって異常値を含むことは時間的追跡をしなければ判定できない。また、１台のセンサの観測値を使って逐次車両の位置を推定するため、異常値を判定することができない、という問題がある。この問題を解決するために、特許文献２では、観測値が正常値である場合の仮説と、観測値が異常値である場合の仮説とをそれぞれ生成する。本開示の手法においても同様の手法を採用する。

ここで本開示の手法では、受信したタイミングにおいて、同時に２以上の観測値が得られた場合を考慮して、インフォメーションフィルタを用いたアプローチを採用する。本開示の手法を用いることで、推定結果が処理順に依らず、複数の仮説を保持しないため、高速に計算が可能である。

異常値が混在するセンサデータのうち、特に時間的に追跡しないと異常が判定できないデータに対して、正常値の場合及び異常値の場合の双方の仮説を考慮することにより、ロバストな異常値の判定を実現できる。ここで、観測は正常値と異常値との確率分布の重ね合わせとして表現される。それぞれの仮説をインフォメーションフィルタにより更新し、統合することで、複数の観測が同時に得られる場合でも適切に状態を推定することが可能となる。また、当該推定において異常値の影響を除去することが可能になる。これは特に、マルチパスが発生するＧＰＳによる測位において有効である。

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

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

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

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

ＧＰＳセンサ２００は、各ＧＰＳ衛星から得られる観測値であるＧＰＳデータを受信する。ＧＰＳデータには、疑似距離及びドップラー周波数が含まれている。そして、ＧＰＳセンサ２００は、受信したＧＰＳデータを状態推定装置３００に出力する。ここで、ＧＰＳセンサ２００では複数の観測が同時にされ得る。例えば、同時に２つの観測が得られたとするならば、それぞれの観測値であるＧＰＳデータを状態推定装置３００に出力する。以下の説明においては、ＧＰＳセンサ２００による観測において２つの観測が同時に得られた場合を前提として説明する。

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

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

はＧＰＳ受信機の時刻のずれ量を表す。

はＧＰＳ受信機の時刻のずれの変量を表す。

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

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

と分散Ｖとを推定する。

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

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

ここで、ｙ_ｓａｔはＧＰＳデータ、ｙ_ＣＡＮはＣＡＮデータにおける観測値である。本実施形態においては、同時に観測された２つのｙ_ｓａｔが得られている。観測値受信部３１０は、観測値の各々を入力として、観測の変数をそれぞれｋ＝１、ｋ＝２（以下、観測ｋと表す）として識別する。また、ｙ_ｓａｔについてそれぞれ観測値ｙ_ｓａｔ１、ｙ_ｓａｔ２（以下、単にｙ_１、ｙ_２と表記する場合もある）とする。ｋ＝１が、本開示の任意の第１観測、ｋ＝２が本開示の第１観測より後に統合する第２観測の一例である。なお、ｋを３以上とする場合であっても同様に適用可能である。

そして、観測値受信部３１０は、各データを受信するたびに、受信したデータを状態予測部３２０へ送る。また、観測値受信部３１０がＧＰＳデータ（ｙ_ｓａｔ）を受信した場合は、保存部３２２へＧＰＳデータ（ｙ_ｓａｔ）を受信したことを示す通知を送る。

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

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

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

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

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

また、

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

ここで仮説の生成及び統合の流れについて説明する。図２は、本開示の仮説の生成及び統合の流れを示す図である。図２に示す例では、観測前の状態をｋ＝０、各観測をｋ＝１、ｋ＝２として表記する。状態ｘ_０は状態変数として保持しておき各仮説の生成において参照する。観測ｋ＝１について、正常な仮説の状態ｘ_ｇ，１、異常の仮説が表す状態ｘ_ｃ，１を生成し、状態ｘ_１に統合する。観測ｋ＝２について、正常な仮説が表す状態ｘ_ｇ，２、異常な仮説の状態ｘ_ｃ，２を生成し、状態ｘ_２に統合する。以下、ｇが正常の仮説、ｃが異常の仮説を表す。

保存部３２２は、観測値受信部３１０から通知を受け付けると、観測前の状態を示す状態変数を保存する。観測前の状態は以下の状態変数ｘ_０で表される。ここでいう観測前の状態とは、当該観測時点に対する状態の更新及び統合を行っていないことを示す。このように観測前の状態を保存して、同時に観測の各々の状態の更新において、一意の変数として利用する。

仮説生成部３３０は、ＧＰＳデータが入力された場合には、観測ｋ＝１及びｋ＝２の順に、それぞれについて仮説の各々を生成する。ＧＰＳデータによる状態の更新では、ＧＰＳ衛星から受信した信号がマルチパス等の影響により、異常な値になっている可能性がある。そこで、仮説生成部３３０では、以下のように同時に得られた観測の各々について、ＧＰＳデータによる観測値が正常値の場合の仮説と、異常値の場合の仮説との２つの仮説をそれぞれ生成し、正常及び異常のそれぞれの可能性を考慮しながら状態を追跡する。状態変数ｘ_０、観測値（ｙ_ｋ）、及び予測結果（ｘ_１）又は統合結果（ｘ_２）に基づいて、正常の状態（ｘ_ｇ，ｋ）を表す仮説と、異常の状態（ｘ_ｃ，ｋ）を表す仮説とを生成する。

観測ｋ＝１については、状態変数ｘ_０、観測値ｙ_１、及び状態予測部３２０による予測結果に基づいて、正常の状態（ｘ_ｇ，１）を表す仮説と、異常の状態（ｘ_ｃ，１）を表す仮説とを生成する。正常の状態（ｘ_ｇ，１）は、観測値ｙ_１が正常値であると仮定して更新された状態である。異常の状態（ｘ_ｃ，１）は、観測値ｙ_１が異常値であると仮定して更新された状態である。また、観測ｋ＝２については、状態変数ｘ_０、観測値ｙ_２、及び後述する仮説統合部３５０による観測ｋ＝１についての統合結果に基づいて、正常の状態（ｘ_ｇ，２）を表す仮説と、異常の状態（ｘ_ｃ，１）を表す仮説とを生成する。正常の状態（ｘ_ｇ，２）は、観測値ｙ_２が正常値であると仮定して更新された状態である。異常の状態（ｘ_ｃ，２）は、観測値ｙ_２が異常値であると仮定して更新された状態である。

具体的には、仮説生成部３３０は、インフォメーションフィルタを用いて、状態の更新を行う。インフォメーションフィルタでは、以下のように予測結果又は統合結果の変数を変換して更新処理を行う。

インフォメーションフィルタΩ_ｋ及びξ_ｋにおいて用いる予測結果又は統合結果は、以下の仮説の状態における確率分布の平均及び分散である。

以下は、観測値が正常値であると仮定して更新された状態の仮説の導出である。

ｘ_ｇ｜ｋは観測値が正常値の場合の仮説の状態における確率分布の平均であり、Ｖ_ｇ｜ｋは観測値が正常値の場合の仮説の状態における分散である。また、Ｒは観測誤差分散行列である。ここで、ｈ（）は以下のＧＰＳ観測モデルとし、Ｈは以下のＧＰＳ観測モデルのヤコビアンとする。

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

以下は、観測値が異常値であると仮定して更新された状態の仮説の導出である。

ｘ_ｃ｜ｋは観測値が異常値の場合の仮説の状態における確率分布の平均であり、Ｖ_ｃ｜ｋは観測値が異常値の場合の仮説の状態における分散である。

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

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

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

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

重み算出部３４０は、仮説生成部３３０によって生成された複数の仮説の各々について重みを算出する。仮説のうち、観測値が正常値であると仮定して更新された状態を表す仮説については、観測値が正常値である場合の観測分布に基づいて、仮説の重みを算出する。観測値が異常値であると仮定して更新された状態を表す仮説については、観測値が異常値である場合の観測分布に基づいて仮説の重みを算出する。具体的には、以下のように、観測ｋについて、観測値が正常値である場合の観測分布として正規分布を用い、観測値が異常値である場合の観測分布としてコーシー分布を用いて、それぞれの仮説の重みを算出する。

ｐ_ｇが、観測値が正常値であると仮定して更新された状態を表す仮説の重み、ｐ_ｃが、観測値が異常値であると仮定して更新された状態を表す仮説の重みである。ｅ_ｋは、ＧＰＳデータとＧＰＳ衛星の観測モデルとの差である。ｗ_ｅｒｒは、異常値である観測値の重み係数を表す定数である。

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

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

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

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

仮説統合部３５０は、仮説生成部３３０によって生成された仮説の各々を統合することにより推定対象の状態を推定する。統合においては、以下の導出のように、観測ｋについて、重み算出部３４０で算出した仮説の各々の重みを用いて、仮説を統合する。

観測ｋ＝１については、状態（ｘ_ｇ，１）の仮説、及び状態（ｘ_ｃ，１）の仮説を統合した状態（ｘ_１）の仮説を生成する。観測ｋ＝２については、状態（ｘ_ｇ，２）の仮説、及び状態（ｘ_ｃ，２）の仮説を統合した状態（ｘ_２）の仮説を生成する。

状態推定装置３００では、仮説生成部３３０、重み算出部３４０、及び仮説統合部３５０において、観測ｋ＝１及びｋ＝２を順に処理することにより、推定対象の状態を推定する。仮説統合部３５０は、観測ｋ＝１についての統合結果（ｘ_１）を状態生成部３３０に送る。当該統合結果（ｘ_１）は、次に処理する観測ｋ＝２の仮説の生成に用いる。また、ＣＡＮデータを受信した場合の観測が行われた場合には、仮説の数は増減しないため、仮説統合部３５０は、仮説の統合を行わない。また、次の観測で用いるために、観測ｋ＝２についての統合結果（ｘ_２）を状態予測部３２０に送る。当該統合結果（ｘ_２）は、現時点での観測の推定結果として、次の観測における状態の予測に用いる。

そして、出力部３６０は、仮説統合部３５０の統合結果により推定した状態ｘに含まれる自車両の位置（緯度・経度）を出力する。

図４は、状態推定装置３００のハードウェア構成を示すブロック図である。

図４に示すように、状態推定装置３００は、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）１１、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）１２、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）１３、ストレージ１４、入力部１５、表示部１６及び通信インタフェース（Ｉ／Ｆ）１７を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

ＣＰＵ１１は、中央演算処理ユニットであり、各種プログラムを実行したり、各部を制御したりする。すなわち、ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４からプログラムを読み出し、ＲＡＭ１３を作業領域としてプログラムを実行する。ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。本実施形態では、ＲＯＭ１２又はストレージ１４には、状態推定プログラムが格納されている。

ＲＯＭ１２は、各種プログラム及び各種データを格納する。ＲＡＭ１３は、作業領域として一時的にプログラム又はデータを記憶する。ストレージ１４は、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）又はＳＳＤ（ＳｏｌｉｄＳｔａｔｅＤｒｉｖｅ）等の記憶装置により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

入力部１５は、マウス等のポインティングデバイス、及びキーボードを含み、各種の入力を行うために使用される。

表示部１６は、例えば、液晶ディスプレイであり、各種の情報を表示する。表示部１６は、タッチパネル方式を採用して、入力部１５として機能してもよい。

通信インタフェース１７は、端末等の他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

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

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

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

ステップＳ１０４において、受信したＣＡＮデータの観測値ｙ_ＣＡＮと、上記ステップＳ１０２による予測結果とに基づいて、仮説の各々の状態を更新する状態更新処理を行う。当該状態更新処理において状態を推定し、推定した状態に含まれる自車両の位置を出力して、ステップＳ１００に戻る。

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

ステップＳ１０８において、観測値受信部３１０は、同時に観測された観測値の各々を入力として、観測をそれぞれｋ＝１、ｋ＝２として識別する。

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

ステップＳ１１２において、観測の各々について、状態変数ｘ_０と、ＧＰＳデータの観測値ｙ_ｓａｔ（ｙ_１，ｙ_２）と、予測結果とに基づいて、仮説の各々の状態を更新し、統合する状態更新統合処理を行う。状態更新統合処理により得られた最終的な統合結果を、推定した状態に含まれる自車両の位置として出力して、ステップＳ１００に戻る。

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

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

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

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

ステップＳ２０４において、仮説統合部３５０は、上記ステップＳ２００で更新された仮説について、仮説が表す状態と、仮説の重みとに基づいて、状態を推定する。

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

ステップＳ３００において、保存部３２２は、状態変数ｘ_０を保存する。

ステップＳ３０２において、仮説生成部３３０は、観測ｋ＝１と設定する。

ステップＳ３０４において、仮説生成部３３０は、観測ｋについて、状態変数ｘ_０、観測値（ｙ_ｋ）、及び予測結果（ｘ_１）又は統合結果（ｘ_２）に基づいて、正常の状態（ｘ_ｇ，ｋ）を表す仮説と、異常の状態（ｘ_ｃ，ｋ）を表す仮説とを生成する。

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

ステップＳ３０８において、仮説統合部３５０は、観測ｋについて、ステップＳ３０４によって生成された仮説の各々を統合する。観測ｋ＝１については、状態（ｘ_ｇ，１）の仮説、及び状態（ｘ_ｃ，１）の仮説を統合した状態（ｘ_１）の仮説を生成する。観測ｋ＝２については、状態（ｘ_ｇ，２）の仮説、及び状態（ｘ_ｃ，２）の仮説を統合した状態（ｘ_２）の仮説を生成する。

ステップＳ３１０において、仮説統合部３５０は、全ての観測ｋについて処理を終了したか否かを判定する。処理を終了した場合には状態更新統合処理ルーチンを終了し、処理を終了していない場合にはステップＳ３１２においてｋ＝ｋ＋１とカウントアップしてステップＳ３０４に戻って処理を繰り返す。

以下は、本実施形態の実験結果の一例を示した表である。表１は平均誤差［ｍ］の測位精度、表２はＲＳＭＥ［ｍ］による測位精度、表３は計算速度を示す。

比較に用いた参考文献は以下の通りである。
［参考文献１］D-UKF : Gaoge Hu et. al.，"A derivative UKF for tightly coupled INS/GPS integrated navigation"，ISA Transactions, 56, pp.135-144, 2015.
［参考文献２］BA : Y. Kojima, T. Iwase, J. Meguro, and K. Takeyama. ”Development of a Novel Integrated Positioning System ”PRECISE” for ADAS Applications.” 19th ITS World Congress, 2012.
［参考文献３］GMR-MHT : S. Taguchi. T. Yoshimura, “Robust Bayesian Filtering for Positioning using GPS & INS in Multipath Environments.” IEEE/ION PLANS, 2018.

以上説明したように、本開示の実施形態に係る状態推定システムによれば、複数の観測値が同時に観測された場合であっても、ロバストに状態を推定することができる。

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

なお、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した状態推定処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－ＰｒｏｇｒａｍｍａｂｌｅＧａｔｅＡｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（ＰｒｏｇｒａｍｍａｂｌｅＬｏｇｉｃＤｅｖｉｃｅ）、及びＡＳＩＣ（ＡｐｐｌｉｃａｔｉｏｎＳｐｅｃｉｆｉｃＩｎｔｅｇｒａｔｅｄＣｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、状態推定処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

また、上記実施形態では、状態推定処理のプログラムがＲＯＭまたはストレージに予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（ＣｏｍｐａｃｔＤｉｓｋＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（ＤｉｇｉｔａｌＶｅｒｓａｔｉｌｅＤｉｓｋＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、及びＵＳＢ（ＵｎｉｖｅｒｓａｌＳｅｒｉａｌＢｕｓ）メモリ等の非一時的（ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ）記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

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

Claims

センサの観測値に基づいて推定対象の状態を推定する状態推定装置であって、
前記センサによる観測において複数の観測値の各々が同時に得られた場合に、
前記センサにより同時に観測された複数の観測値の各々を入力として、任意の第１観測、及び前記第１観測より後に統合する第２観測を識別する受信部と、
観測前の状態を示す状態変数を保存する保存部と、
前記推定対象の状態を表す仮説について、観測時点における、前記仮説が表す状態を予測する状態予測部と、
前記第１観測について、前記状態変数、前記第１観測の第１観測値、及び前記状態予測部による予測結果に基づいて、当該第１観測値が正常値であると仮定して更新された状態を表す仮説と、当該第１観測値が異常値であると仮定して更新された状態を表す仮説とを生成する仮説生成部と、
前記仮説生成部によって生成された前記仮説の各々における状態を、仮説の各々の重みを用いて統合することにより前記推定対象の状態を推定する仮説統合部と、
を含み、
前記仮説生成部は、前記第２観測について、前記状態変数、前記第２観測の第２観測値、及び前記仮説統合部による前記第１観測についての統合結果に基づいて、当該第２観測値が正常値であると仮定して更新された状態を表す仮説と、当該第２観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、
前記状態及び前記状態変数が確率分布の平均と分散を示す、
状態推定装置。
前記仮説生成部は、前記予測結果又は前記統合結果を所定の変数に変換して、前記仮説の各々を生成する請求項１に記載の状態推定装置。
前記仮説生成部によって生成された複数の仮説のうち、前記観測値が正常値であると仮定して更新された状態を表す仮説については、前記観測値が正常値である場合の観測分布に基づいて当該仮説の重みを算出し、前記観測値が異常値であると仮定して更新された状態を表す仮説については、前記観測値が異常値である場合の観測分布に基づいて当該仮説の重みを算出する重み算出部を更に含む、請求項１又は請求項２に記載の状態推定装置。
センサの観測値に基づいて推定対象の状態を推定する状態推定装置における状態推定方法であって、
前記センサによる観測において複数の観測値の各々が同時に得られた場合に、
前記センサにより同時に観測された複数の観測値の各々を入力として、任意の第１観測、及び前記第１観測より後に統合する第２観測を識別し、
観測前の状態を示す状態変数を保存し、
前記推定対象の状態を表す仮説について、観測時点における、前記仮説が表す状態を予測し、
前記第１観測について、前記状態変数、前記第１観測の第１観測値、及び予測結果に基づいて、当該第１観測値が正常値であると仮定して更新された状態を表す仮説と、当該第１観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、
前記第１観測について、生成された前記仮説の各々を統合し、
前記第２観測について、前記状態変数、前記第２観測の第２観測値、及び前記第１観測についての統合結果に基づいて、当該第２観測値が正常値であると仮定して更新された状態を表す仮説と、当該第２観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、前記状態及び前記状態変数が確率分布の平均と分散を示しており、
前記第２観測について、生成された前記仮説の各々における状態を、仮説の各々の重みを用いて統合することにより前記推定対象の状態を推定する、
処理をコンピュータに実行させる状態推定方法。
センサの観測値に基づいて推定対象の状態を推定する状態推定装置における状態推定プログラムであって、
前記センサによる観測において複数の観測値の各々が同時に得られた場合に、
前記センサにより同時に観測された複数の観測値の各々を入力として、任意の第１観測、及び前記第１観測より後に統合する第２観測を識別し、
観測前の状態を示す状態変数を保存し、
前記推定対象の状態を表す仮説について、観測時点における、前記仮説が表す状態を予測し、
前記第１観測について、前記状態変数、前記第１観測の第１観測値、及び予測結果に基づいて、当該第１観測値が正常値であると仮定して更新された状態を表す仮説と、当該第１観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、
前記第１観測について、生成された前記仮説の各々を統合し、
前記第２観測について、前記状態変数、前記第２観測の第２観測値、及び前記第１観測についての統合結果に基づいて、当該第２観測値が正常値であると仮定して更新された状態を表す仮説と、当該第２観測値が異常値であると仮定して更新された状態を表す仮説とを生成し、前記状態及び前記状態変数が確率分布の平均と分散を示しており、
前記第２観測について、生成された前記仮説の各々における状態を、仮説の各々の重みを用いて統合することにより前記推定対象の状態を推定する、
処理をコンピュータに実行させる状態推定プログラム。