マルチセンサ判定装置及びプログラム
【課題】所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定する。
【解決手段】状態推定部２０で、マルチセンサ１２で観測された観測値Ｄ（各観測値ｄ）から推定値ｘを推定し、事前分布算出部２６で、観測値Ｄから推定される推定値ｘの事前確率分布ｐ（ｘ）を算出し、事後分布算出部２８で、観測プール２４に記憶された観測値Ｄを用いて、観測値Ｄの尤度ｐ（Ｄ｜ｘ）、及び推定値の事前確率分布ｐ（ｘ）に基づいて、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。観測値分布算出部３０で、推定値の事後確率分布ｐ（ｘ｜Ｄ）と各観測値の尤度ｐ（ｄ｜ｘ）とに基づいて、各観測値の確率分布ｐ（ｄ）を算出し、外れ値判定部３２で、新たに観測された観測値Ｄの各観測値ｄと、各観測値の確率分布ｐ（ｄ）の各々とを観測値の次元毎に比較して、外れ値を判定し、外れ値ではないと判定された観測値を観測プール２４へ記憶する。
【解決手段】状態推定部２０で、マルチセンサ１２で観測された観測値Ｄ（各観測値ｄ）から推定値ｘを推定し、事前分布算出部２６で、観測値Ｄから推定される推定値ｘの事前確率分布ｐ（ｘ）を算出し、事後分布算出部２８で、観測プール２４に記憶された観測値Ｄを用いて、観測値Ｄの尤度ｐ（Ｄ｜ｘ）、及び推定値の事前確率分布ｐ（ｘ）に基づいて、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。観測値分布算出部３０で、推定値の事後確率分布ｐ（ｘ｜Ｄ）と各観測値の尤度ｐ（ｄ｜ｘ）とに基づいて、各観測値の確率分布ｐ（ｄ）を算出し、外れ値判定部３２で、新たに観測された観測値Ｄの各観測値ｄと、各観測値の確率分布ｐ（ｄ）の各々とを観測値の次元毎に比較して、外れ値を判定し、外れ値ではないと判定された観測値を観測プール２４へ記憶する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マルチセンサ判定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、ＤＯＰ（Dilution Of Precision：位置精度劣化指数）とＵＥＲＥ（User Equivalent Range Error：使用者等価距離誤差）との積を用いて測位精度を算出するＧＰＳ受信機が提案されている（例えば、特許文献１参照）。
【０００３】
また、複数のセンサの検出値をカルマンフィルタを用いて統合して、測位解を推定するＧＰＳレシーバにおいて、カルマンフィルタの推定値の算出過程で算出される誤差共分散行列の対角要素を用いて、ＧＰＳ測位解の精度を判定するＧＰＳレシーバが提案されている（例えば、特許文献２〜４参照）。
【０００４】
また、衛星組の機能的残差ＡＰＲ、ＤＯＰ、及び衛星数の各々に重み付けを行って算出した評価値に基づいて、測位された現在位置を評価するなど、観測値の情報を用いて推定精度を評価する手法が提案されている（例えば、特許文献５及び非特許文献１参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−５１０４７号公報
【特許文献２】特開２００１−３３７１５０号公報
【特許文献３】特開２００８−２０３６５号公報
【特許文献４】特開２００８−２６２８２号公報
【特許文献５】特開２００８−２８１５５３号公報
【非特許文献】
【０００６】
【非特許文献１】Position and velocity reliability testing in degraded GPS signal environments
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献１〜４の技術では、ＤＯＰやカルマンフィルタの共分散行列を手がかりに観測値から推定された推定値の信頼度を算出しているが、ＤＯＰやカルマンフィルタの共分散行列は、観測値の値とは独立であるため、実際に観測値に含まれる外れ値（推定される状態との残差が所定値以上となる観測値、異常値）を検出及び排除することができない、という問題がある。
【０００８】
また、特許文献５及び非特許文献１では、観測値を用いて推定値の信頼度を算出しているが、観測値のうち外れ値が大半を占める場合には、外れ値の判定に失敗する、という問題がある。この問題を図８を参照して説明する。例えば、同図（ａ）に示すように、複数の観測値のうち１つが外れ値であった場合には、同図（ｂ）に示すように、各観測値から推定される推定値の分布に基づいて、容易に外れ値を判定することができる。しかし、同図（ｃ）に示すように、複数の観測値の大半が外れ値の場合には、同図（ｄ）に示すように、正常な観測値による推定値の分布の方が例外的な分布となってしまい、適切に外れ値を判定することができない。
【０００９】
本発明は、上記の問題点を解決するためになされたもので、複数のセンサで構成されたマルチセンサで観測された次元の異なる複数の観測値から状態を推定する場合において、所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定することができるマルチセンサ判定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明のマルチセンサ判定装置は、各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサと、前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段と、前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段と、を含んで構成されている。
【００１１】
本発明のマルチセンサ判定装置によれば、確率分布算出手段が、各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサの各センサにより過去に観測され、かつ外れ値を含まない各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する。そして、判定手段が、確率分布算出手段により算出された推定値の確率分布に基づいて算出された観測値各々の確率分布と、マルチセンサで観測された新たな観測値各々とを次元毎に比較して、新たな観測値各々が外れ値か否かを判定する。
【００１２】
このように、外れ値ではないと判定された観測値のみで算出された推定値の確率分布に基づいて、新たに観測された観測値が外れ値か否かを判定するため、所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定することができる。また、次元の異なる観測値から推定された推定値の事後確率分布から、各観測値の確率分布を算出して、外れ値の判定を行うため、次元の異なる複数の観測値を比較して矛盾を判定することができ、精度の高い判定を行うことができる。
【００１３】
また、前記判定手段により外れ値ではないと判定された観測値を記憶する観測値記憶手段を含み、前記確率分布算出手段は、前記観測値記憶手段に記憶された観測値を用いて、前記推定値の確率分布を算出することができる。
【００１４】
また、本発明のマルチセンサ判定装置は、前記確率分布算出手段により算出された推定値の確率分布を記憶する確率分布記憶手段と、前記確率分布記憶手段に記憶された推定値の確率分布を、前記判定手段により外れ値ではないと判定された観測値を用いて更新する更新手段と、を含んで構成することができ、前記判定手段は、前記更新手段により前記推定値の確率分布が更新された場合には、該更新された推定値の確率分布を用いて、前記新たな観測値各々が外れ値か否かを判定することができる。これにより、外れ値ではないと判定された観測値自体を記憶しておく場合と比較して、計算量及びメモリ量を削減することができる。
【００１５】
また、前記確率分布算出手段により算出された推定値の確率分布、または前記複数のセンサ各々の誤差分布に基づいて算出される前記推定値の事前確率分布に基づいて、前記推定値の信頼度を算出する信頼度算出手段を含んで構成することができる。このように、次元の異なる複数の観測値から算出した推定値の確率分布に基づいて推定値の信頼度を算出するため、精度良く信頼度を算出することができる。
【００１６】
また、前記マルチセンサを、ＧＰＳ衛星から送信された信号を受信するＧＰＳ装置、前記マルチセンサが搭載される移動体の速度を検出する速度センサ、前記移動体のヨーレイトを検出するヨーレイトセンサ、及び地図情報を取得するセンサの少なくとも１つを含んで構成し、前記確率分布算出手段は、前記移動体の位置または速度の確率分布を算出するようにすることができる。これにより、マルチパスの影響により、外れ値が連続して生じるような場合でも、外れ値を精度良く判定することができる。
【００１７】
また、本発明のマルチセンサ判定プログラムは、コンピュータを、各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサで観測された観測値を取得する取得手段、前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段、及び前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段として機能させるためのプログラムである。
【００１８】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００１９】
以上説明したように、本発明のマルチセンサ判定装置及びプログラムによれば、外れ値ではないと判定された観測値のみで算出された推定値の確率分布に基づいて、新たに観測された観測値が外れ値か否かを判定するため、所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定することができ、また、次元の異なる観測値から推定された推定値の確率分布から、各観測値の確率分布を算出して、外れ値の判定を行うため、次元の異なる複数の観測値を比較して矛盾を判定することができ、精度の高い判定を行うことができる、という効果が得られる。
【図面の簡単な説明】
【００２０】
【図１】本実施の形態の原理を説明するためのイメージ図である。
【図２】第１の実施の形態に係るマルチセンサ判定装置の機能的構成を示すブロック図である。
【図３】外れ値の判定を説明するための図である。
【図４】信頼度の算出を説明するための図である。
【図５】第１の実施の形態に係るマルチセンサ判定装置におけるマルチセンサ判定処理ルーチンの内容を示すフローチャートである。
【図６】第２の実施の形態に係るマルチセンサ判定装置の機能的構成を示すブロック図である。
【図７】第２の実施の形態に係るマルチセンサ判定装置におけるマルチセンサ判定処理ルーチンの内容を示すフローチャートである。
【図８】従来技術の問題点を説明するためのイメージ図である。
【発明を実施するための形態】
【００２１】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２２】
まず、本実施の形態の原理について説明する。
【００２３】
図１に示すように、時刻ｔ−１までに観測された観測値のうち、予め外れ値ではないと判定済みの観測値を用いて、推定値の確率分布を算出する。この推定値の確率分布と、時刻ｔで新たに観測された観測値とを比較することにより、新たな観測値が外れ値か否かを判定する。そして、外れ値であると判定された場合には、その観測値を廃棄し、外れ値ではないと判定された場合には、次時刻において、推定値の確率分布の算出に用いられる。
【００２４】
このように、外れ値ではないと判定された観測値のみを推定値の確率分布の算出に用いることで、連続して外れ値が発生し、所定時間内に観測された観測値の大半を外れ値が占める場合でも、常に正常な過去の観測値のみに基づいて、外れ値の判定に用いる推定値の確率分布を適切に算出することができ、正確な外れ値判定を行うことができる。
【００２５】
以下で、各実施の形態について説明する。なお、各実施の形態では、車両に搭載され、複数のセンサを備えたマルチセンサの観測値から自車両の位置を推定する場合において、マルチセンサの外れ値の判定、及び推定される状態の推定値の信頼度を算出するマルチセンサ判定装置に、本発明を適用した場合を例に説明する。
【００２６】
図２に示すように、第１の実施の形態におけるマルチセンサ判定装置１０は、異なる次元の観測値を各々観測する複数のセンサで構成されたマルチセンサ１２と、外れ値の判定及び推定値の信頼度の算出の処理を実行するコンピュータ１６とを備えている。
【００２７】
マルチセンサ１２は、複数のセンサとして、ＧＰＳ衛星から送信された信号を受信するＧＰＳ装置、自車両の速度を検出する速度センサ、自車両のヨーレイトを検出するヨーレイトセンサ、及び地図情報を取得する地図情報取得部を含んで構成されている。これらの各センサから、擬似距離、ＩＮＳ速度、ドップラー周波数、位相、地図情報等が観測値として観測される。
【００２８】
コンピュータ１６は、ＣＰＵ、後述するマルチセンサ判定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。
【００２９】
このコンピュータ１６を以下で説明するマルチセンサ判定処理ルーチンに従って機能ブロックで表すと、図２に示すように、マルチセンサ１２で観測された観測値から自車両の位置（状態）を推定する状態推定部２０と、初期条件に基づいて観測値が外れ値か否かを判定する初期プールフィルタ２２と、初期プールフィルタ２２及び後述する外れ値判定部３２を通過した観測値が記憶される観測プール２４と、観測値から推定される推定値の事前確率分布を算出する事前分布算出部２６と、推定値の事前確率分布及び観測プールに記憶された観測値に基づいて、推定値の事後確率分布を算出する事後分布算出部２８と、推定値の事後確率分布に基づいて、各観測値の確率分布を算出する観測値分布算出部３０と、各観測値の確率分布と新たに観測された観測値とを比較して、外れ値か否かを判定する外れ値判定部３２と、事後確率分布に基づいて、推定値の信頼度を算出する信頼度算出部３４と、を含んだ構成で表すことができる。
【００３０】
なお、事後分布算出部２８が本発明の確率分布算出手段の一例であり、観測値分布算出部３０及び外れ値判定部３２が本発明の判定手段の一例である。
【００３１】
状態推定部２０は、マルチセンサ１２で観測された観測値Ｄを取得し、観測値Ｄに基づいて状態を示す推定値ｘを推定する。ここでは、観測値Ｄは、擬似距離ρ（衛星位置ｘｓａｔ）、ＩＮＳ速度ｖ、地図情報における自車両から最も近い道路上の位置ｘｎｅａｒｍａｐ等の各観測値を含んでいる。また、推定値ｘは自車両の位置である。観測値Ｄに基づく推定値ｘの推定は、従来既知の手法を用いることができる。なお、以下では、各観測値をまとめて表現する場合には、観測値Ｄと表記し、観測値Ｄに含まれる個々の観測値を表現する場合には、観測値ｄと表記する。
【００３２】
初期プールフィルタ２２は、観測プール２４に観測値が記憶されていない場合に、初期条件に従って観測値を観測プールへ記憶するか否かを判定する。例えば、ＧＰＳ装置により観測された観測値の場合には、ＧＰＳ衛星からの信号のＳ／Ｎや衛星仰角に基づいて、受信状況が良好か否かにより判定したり、ＤＯＰにより衛星の配置が良好か否かを判定したりすることができる。また、電波を受信した衛星数や、観測値や残差の時系列のばらつき等により判定するようにしてもよい。初期プールフィルタ２２における初期条件は、厳しい条件を設定しておくことが好ましい。
【００３３】
観測プール２４には、初期プールフィルタ２２を通過した観測値が記憶される。また、後述する外れ値判定部３２で外れ値ではないと判定された観測値が記憶される。
【００３４】
事前分布算出部２６は、マルチセンサ１２を構成する各センサの誤差分布など、予め知識として把握できる情報に基づいて、各観測値ｄの事前確率分布を算出し、この各観測値ｄの事前確率分布に基づいて、観測値Ｄから推定される推定値ｘの事前確率分布ｐ（ｘ）を算出する。観測値ｄの事前確率分布としては、例えば、擬似距離ρについて、下記（１）式に示すような擬似距離精度σＧＰＳを用いることができる。
【００３５】
【数１】
【００３６】
なお、推定値の事前確率分布ｐ（ｘ）として、後述する事後分布算出部２８において、一時刻前に算出された推定値の事後確率分布を用いてもよい。
【００３７】
事後分布算出部２８は、事前分布算出部２６で算出された推定値の事前確率分布ｐ（ｘ）、及び観測プールに記憶された観測値Ｄを用いて算出される、推定値ｘに対する観測値Ｄの確からしさを示す尤度ｐ（Ｄ｜ｘ）に基づいて、下記（２）式に従って、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。
【００３８】
【数２】
【００３９】
なお、Ｚは正規化定数である。
【００４０】
より具体的には、まず、観測値の尤度ｐ（Ｄ｜ｘ）を、観測値ｄ毎に算出し、これをｐ（ｄ｜ｘ）とする。例えば、擬似距離の尤度ｐ（ρ｜ｘ）、ＩＮＳ速度ｖから得られる移動ベクトルの尤度ｐ（ｘｔ｜ｘｔ−１）、及び自車両と道路とがどれくらい離れているかの指標ｌの尤度ｐ（ｌ｜ｘ）は、下記（３）〜（５）式により算出することができる。
【００４１】
【数３】
【００４２】
ここで、Ｃｂはクロックバイアスである。また、Ｎ（μ，σ２）は、平均μ、標準偏差σの正規分布を表す。（３）〜（５）式を用いた場合には、（２）式は、下記（６）式のように表すことができる。また、各式のｘは、各観測値ｄが観測された際に、状態推定部２０で観測値ｄを含む観測値Ｄに基づいて推定された推定値ｘである。
【００４３】
【数４】
【００４４】
観測値分布算出部３０は、事後分布算出部２８で算出された推定値の事後確率分布ｐ（ｘ｜Ｄ）を現在の推定値の分布として用い、この現在の推定値の分布と各観測値の尤度とに基づいて、各観測値の確率分布ｐ（ｄ）を算出する。例えば、擬似距離ρの場合には、下記（７）式により、擬似距離ρの確率分布ｐ（ρ）を算出することができる。
【００４５】
【数５】
【００４６】
外れ値判定部３２は、マルチセンサ１２で観測された各観測値ｄと、観測値分布算出部３０で算出された各観測値の確率分布ｐ（ｄ）とを次元毎に比較して、観測された各観測値ｄが外れ値か否かを判定する。例えば、図３に示すように、観測された擬似距離ρについては、擬似距離の確率分布ｐ（ρ）と比較して、観測された擬似距離の内側確率を外れ値である確率とし、外れ値である確率が予め定めた所定値以上か否かにより、観測された擬似距離が外れ値か否かを判定することができる。また、外れ値判定部３２は、外れ値であると判定した観測値ｄを破棄し、外れ値ではないと判定した観測値ｄを観測プール２４に記憶する。
【００４７】
信頼度算出部３４は、事後分布算出部２８で算出された推定値の事後確率分布ｐ（ｘ｜Ｄ）に基づいて、状態推定部２０で推定される推定値ｘの信頼度を算出する。例えば、図４に示すように、推定値の事後確率分布ｐ（ｘ｜Ｄ）の平均を中心に幅ａの区間を設定し、この区間に入る確率を信頼度として算出することができる。区間幅ａは、要求される精度に応じて定めることができる。
【００４８】
次に、図５を参照して、第１の実施の形態のマルチセンサ判定装置１０において実行されるマルチセンサ判定処理ルーチンについて説明する。
【００４９】
ステップ１００で、マルチセンサ１２で観測された観測値Ｄ（各観測値ｄ）を取得し、観測値Ｄに基づいて状態を示す推定値ｘを推定する。
【００５０】
次に、ステップ１０２で、観測プール２４に過去に観測された正常な観測値が記憶されているか否かを判定する。観測プール２４に観測値が記憶されていない場合には、ステップ１０４へ移行して、初期プールフィルタ２２を通過した観測値ｄを観測プール２４へ記憶し、初期プールフィルタ２２を通過しなかった観測値ｄを廃棄する。一方、観測プールに観測値が記憶されている場合には、ステップ１０６へ移行する。
【００５１】
ステップ１０６では、マルチセンサ１２を構成する各センサの誤差分布など、予め知識として把握できる情報に基づいて、観測値Ｄから推定される推定値ｘの事前確率分布ｐ（ｘ）を算出する。なお、一時刻前に算出された推定値ｘの事後確率分布ｐ（ｘ｜Ｄ）を用いてもよい。
【００５２】
次に、ステップ１０８で、観測プール２４に記憶された観測値Ｄを用いて、推定値ｘに対する観測値Ｄの確からしさを示す尤度ｐ（Ｄ｜ｘ）を算出する。次に、ステップ１１０で、上記ステップ１０６で算出された推定値の事前確率分布ｐ（ｘ）、及び上記ステップ１０８で算出された尤度ｐ（Ｄ｜ｘ）に基づいて、（２）式に従って、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。
【００５３】
次に、ステップ１１２で、上記ステップ１１０で算出した推定値の事後確率分布ｐ（ｘ｜Ｄ）を現在の推定値の分布として用い、この現在の推定値の分布と各観測値の尤度ｐ（ｄ｜ｘ）とに基づいて、各観測値の確率分布ｐ（ｄ）を算出する。
【００５４】
次に、ステップ１１４で、上記ステップ１００で取得した観測値Ｄの各観測値ｄと、上記ステップ１１２で算出した各観測値の確率分布ｐ（ｄ）の各々とを観測値の次元毎に比較する。次に、ステップ１１６で、上記ステップ１１４の比較結果に基づいて、上記ステップ１００で取得された観測値Ｄの各観測値ｄが外れ値か否かを判定する。観測値ｄが外れ値の場合には、ステップ１１８へ移行して、その観測値ｄを破棄し、観測値ｄが外れ値ではない場合には、ステップ１２０へ移行して、その観測値ｄを観測プール２４に記憶する。
【００５５】
次に、ステップ１２２で、上記ステップ１１０で算出された推定値の事後確率分布ｐ（ｘ｜Ｄ）に基づいて、上記ステップ１００で推定された推定値ｘの信頼度を算出して出力し、ステップ１００に戻る。
【００５６】
以上説明したように、第１の実施の形態のマルチセンサ判定装置によれば、外れ値ではないと判定された観測値のみを観測プールに記憶し、観測プールに記憶された観測値に基づいて、外れ値の判定に用いる推定値の確率分布を算出するため、連続して外れ値が発生し、所定時間内に観測された観測値の大半を外れ値が占める場合でも、適切に外れ値を判定することができる。
【００５７】
また、次元の異なる観測値から推定された推定値の事後確率分布から、各観測値の確率分布を算出して、外れ値の判定を行うため、次元の異なる複数の観測値を比較して矛盾を判定することができ、精度の高い判定を行うことができる。
【００５８】
また、上記のように次元の異なる複数の観測値を統合して算出した推定値の事後確率分布から推定値の信頼度を算出するため、精度良く信頼度を算出することができる。
【００５９】
次に、第２の実施の形態について説明する。第２の実施の形態では、外れ値ではないと判定された観測値を記憶しておくのではなく、そこから算出された推定値の事後確率分布を記憶しておく点が、第１の実施の形態と異なる。なお、第２の実施の形態のマルチセンサ判定装置において、第１の実施の形態のマルチセンサ判定装置１０の構成と同様の構成については、同一符号を付して詳細な説明を省略する。
【００６０】
図６に示すように、第２の実施の形態におけるマルチセンサ判定装置２１０は、マルチセンサ１２と、コンピュータ２１６とを備えている。このコンピュータ２１６を以下で説明するマルチセンサ判定処理ルーチンに従って機能ブロックで表すと、図６に示すように、状態推定部２０と、初期プールフィルタ２２と、事前分布算出部２６と、事後分布算出部２２８と、算出された推定値の事後確率分布を記憶する事後分布記憶部３６と、観測値分布算出部２３０と、外れ値判定部３２と、外れ値ではないと判定された観測値を用いて推定値の事後確率分布を更新する事後分布更新部３８と、信頼度算出部３４と、を含んだ構成で表すことができる。
【００６１】
事後分布算出部２２８は、初期プールフィルタ２２を通過した観測値に基づいて、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する点が、観測プール２４に記憶された観測値に基づいて、推定値の事後確率分布を算出する第１の実施の形態における事後分布算出部２８と異なるだけである。
【００６２】
観測値分布算出部２３０は、事後分布記憶部３６に一旦記憶された推定値の事後確率分布に基づいて、観測値の確率分布ｐ（ｄ）を算出する点が、第１の実施の形態における観測値分布算出部３０と異なるだけである。
【００６３】
事後分布更新部３８は、外れ値判定部３２で外れ値ではないと判定された観測値を用いて、事後分布記憶部３６に記憶された推定値の事後確率分布ｐ（ｘ｜Ｄ）を更新する。
【００６４】
次に、図７を参照して、第２の実施の形態のマルチセンサ判定装置２１０において実行されるマルチセンサ判定処理ルーチンについて説明する。なお、第１の実施の形態のマルチセンサ判定処理ルーチンと同一の処理については、同一符号を付して詳細な説明を省略する。
【００６５】
ステップ１００で、マルチセンサ１２で観測された観測値Ｄ（各観測値ｄ）を取得し、観測値Ｄに基づいて状態を示す推定値ｘを推定する。
【００６６】
次に、ステップ２０２で、事後分布記憶部３６に推定値の事後確率分布が記憶されているか否かを判定する。推定値の事後確率分布が記憶されていない場合には、ステップ２０４へ移行し、記憶されている場合には、ステップ１１２へ移行する。
【００６７】
ステップ２０４では、上記ステップ１００で取得された観測値Ｄに初期プールフィルタ２２を適用し、次に、ステップ１０６で、推定値ｘの事前確率分布ｐ（ｘ）を算出する。次に、ステップ２０８で、上記ステップ２０４で初期プールフィルタ２２を通過した観測値Ｄを用いて、観測値Ｄの尤度ｐ（Ｄ｜ｘ）を算出し、次に、ステップ１１０で、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。次に、ステップ２０９で、上記ステップ１１０で算出した推定値の事後確率分布ｐ（ｘ｜Ｄ）を事後分布記憶部３６に記憶して、ステップ１００へ戻る。
【００６８】
上記ステップ２０２で肯定判定されて、ステップ１１２へ移行した場合には、ステップ１１２〜１１６を実行して、上記ステップ１００で取得された観測値Ｄの各観測値ｄが外れ値か否かを判定し、観測値ｄが外れ値の場合には、ステップ１１８へ移行して、その観測値ｄを破棄し、観測値ｄが外れ値ではない場合には、ステップ２２０へ移行して、その観測値ｄを用いて、事後分布記憶部３６に記憶された推定値の事後確率分布ｐ（ｘ｜Ｄ）を更新する。
【００６９】
次に、ステップ１２２で、推定値ｘの信頼度を算出して出力し、ステップ１００に戻る。
【００７０】
以上説明したように、第２の実施の形態のマルチセンサ判定装置によれば、第１の実施の形態と同様の効果を奏すると共に、外れ値ではないと判定された観測値自体ではなく、そこから算出された分布を記憶しておくため、計算量及びメモリ量を削減することができる。
【００７１】
なお、上記第１及び第２の実施の形態では、（７）式に従って観測値の確率分布ｐ（ｄ）を算出する場合について説明したが、下記（８）式及び（９）式に従って算出してもよい。
【００７２】
【数６】
【００７３】
ここで、Ｈは観測方程式のデザイン行列、Ｑｙは観測値ｙの共分散行列、ｒｙは観測値ｙの残差、ａ及びｂはパラメータである。
【００７４】
また、上記第１及び第２の実施の形態では、推定値の事後確率分布に基づいて推定値の信頼度を算出する場合について説明したが、推定値の事前確率分布に基づいて信頼度を算出するようにしてもよい。例えば、下記（１０）に示す観測値ｙの分散行列、及び下記（１１）式に示す観測方程式から、下記（１２）式に示す推定値の事前確率分布を算出し、この推定値の事前確率分布Ｑｘに基づいて、推定値の信頼度を算出することができる。
【００７５】
【数７】
【符号の説明】
【００７６】
１０、２１０ マルチセンサ判定装置
１２ マルチセンサ
１６、２１６ コンピュータ
２０ 状態推定部
２２ 初期プールフィルタ
２４ 観測プール
２６ 事前分布算出部
２８、２２８ 事後分布算出部
３０、２３０ 観測値分布算出部
３２ 外れ値判定部
３４ 信頼度算出部
３６ 事後分布記憶部
３８ 事後分布更新部
【技術分野】
【０００１】
本発明は、マルチセンサ判定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、ＤＯＰ（Dilution Of Precision：位置精度劣化指数）とＵＥＲＥ（User Equivalent Range Error：使用者等価距離誤差）との積を用いて測位精度を算出するＧＰＳ受信機が提案されている（例えば、特許文献１参照）。
【０００３】
また、複数のセンサの検出値をカルマンフィルタを用いて統合して、測位解を推定するＧＰＳレシーバにおいて、カルマンフィルタの推定値の算出過程で算出される誤差共分散行列の対角要素を用いて、ＧＰＳ測位解の精度を判定するＧＰＳレシーバが提案されている（例えば、特許文献２〜４参照）。
【０００４】
また、衛星組の機能的残差ＡＰＲ、ＤＯＰ、及び衛星数の各々に重み付けを行って算出した評価値に基づいて、測位された現在位置を評価するなど、観測値の情報を用いて推定精度を評価する手法が提案されている（例えば、特許文献５及び非特許文献１参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−５１０４７号公報
【特許文献２】特開２００１−３３７１５０号公報
【特許文献３】特開２００８−２０３６５号公報
【特許文献４】特開２００８−２６２８２号公報
【特許文献５】特開２００８−２８１５５３号公報
【非特許文献】
【０００６】
【非特許文献１】Position and velocity reliability testing in degraded GPS signal environments
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献１〜４の技術では、ＤＯＰやカルマンフィルタの共分散行列を手がかりに観測値から推定された推定値の信頼度を算出しているが、ＤＯＰやカルマンフィルタの共分散行列は、観測値の値とは独立であるため、実際に観測値に含まれる外れ値（推定される状態との残差が所定値以上となる観測値、異常値）を検出及び排除することができない、という問題がある。
【０００８】
また、特許文献５及び非特許文献１では、観測値を用いて推定値の信頼度を算出しているが、観測値のうち外れ値が大半を占める場合には、外れ値の判定に失敗する、という問題がある。この問題を図８を参照して説明する。例えば、同図（ａ）に示すように、複数の観測値のうち１つが外れ値であった場合には、同図（ｂ）に示すように、各観測値から推定される推定値の分布に基づいて、容易に外れ値を判定することができる。しかし、同図（ｃ）に示すように、複数の観測値の大半が外れ値の場合には、同図（ｄ）に示すように、正常な観測値による推定値の分布の方が例外的な分布となってしまい、適切に外れ値を判定することができない。
【０００９】
本発明は、上記の問題点を解決するためになされたもので、複数のセンサで構成されたマルチセンサで観測された次元の異なる複数の観測値から状態を推定する場合において、所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定することができるマルチセンサ判定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、本発明のマルチセンサ判定装置は、各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサと、前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段と、前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段と、を含んで構成されている。
【００１１】
本発明のマルチセンサ判定装置によれば、確率分布算出手段が、各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサの各センサにより過去に観測され、かつ外れ値を含まない各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する。そして、判定手段が、確率分布算出手段により算出された推定値の確率分布に基づいて算出された観測値各々の確率分布と、マルチセンサで観測された新たな観測値各々とを次元毎に比較して、新たな観測値各々が外れ値か否かを判定する。
【００１２】
このように、外れ値ではないと判定された観測値のみで算出された推定値の確率分布に基づいて、新たに観測された観測値が外れ値か否かを判定するため、所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定することができる。また、次元の異なる観測値から推定された推定値の事後確率分布から、各観測値の確率分布を算出して、外れ値の判定を行うため、次元の異なる複数の観測値を比較して矛盾を判定することができ、精度の高い判定を行うことができる。
【００１３】
また、前記判定手段により外れ値ではないと判定された観測値を記憶する観測値記憶手段を含み、前記確率分布算出手段は、前記観測値記憶手段に記憶された観測値を用いて、前記推定値の確率分布を算出することができる。
【００１４】
また、本発明のマルチセンサ判定装置は、前記確率分布算出手段により算出された推定値の確率分布を記憶する確率分布記憶手段と、前記確率分布記憶手段に記憶された推定値の確率分布を、前記判定手段により外れ値ではないと判定された観測値を用いて更新する更新手段と、を含んで構成することができ、前記判定手段は、前記更新手段により前記推定値の確率分布が更新された場合には、該更新された推定値の確率分布を用いて、前記新たな観測値各々が外れ値か否かを判定することができる。これにより、外れ値ではないと判定された観測値自体を記憶しておく場合と比較して、計算量及びメモリ量を削減することができる。
【００１５】
また、前記確率分布算出手段により算出された推定値の確率分布、または前記複数のセンサ各々の誤差分布に基づいて算出される前記推定値の事前確率分布に基づいて、前記推定値の信頼度を算出する信頼度算出手段を含んで構成することができる。このように、次元の異なる複数の観測値から算出した推定値の確率分布に基づいて推定値の信頼度を算出するため、精度良く信頼度を算出することができる。
【００１６】
また、前記マルチセンサを、ＧＰＳ衛星から送信された信号を受信するＧＰＳ装置、前記マルチセンサが搭載される移動体の速度を検出する速度センサ、前記移動体のヨーレイトを検出するヨーレイトセンサ、及び地図情報を取得するセンサの少なくとも１つを含んで構成し、前記確率分布算出手段は、前記移動体の位置または速度の確率分布を算出するようにすることができる。これにより、マルチパスの影響により、外れ値が連続して生じるような場合でも、外れ値を精度良く判定することができる。
【００１７】
また、本発明のマルチセンサ判定プログラムは、コンピュータを、各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサで観測された観測値を取得する取得手段、前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段、及び前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段として機能させるためのプログラムである。
【００１８】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００１９】
以上説明したように、本発明のマルチセンサ判定装置及びプログラムによれば、外れ値ではないと判定された観測値のみで算出された推定値の確率分布に基づいて、新たに観測された観測値が外れ値か否かを判定するため、所定時間内に観測された観測値の大半を外れ値が占める場合であっても、適切に外れ値を判定することができ、また、次元の異なる観測値から推定された推定値の確率分布から、各観測値の確率分布を算出して、外れ値の判定を行うため、次元の異なる複数の観測値を比較して矛盾を判定することができ、精度の高い判定を行うことができる、という効果が得られる。
【図面の簡単な説明】
【００２０】
【図１】本実施の形態の原理を説明するためのイメージ図である。
【図２】第１の実施の形態に係るマルチセンサ判定装置の機能的構成を示すブロック図である。
【図３】外れ値の判定を説明するための図である。
【図４】信頼度の算出を説明するための図である。
【図５】第１の実施の形態に係るマルチセンサ判定装置におけるマルチセンサ判定処理ルーチンの内容を示すフローチャートである。
【図６】第２の実施の形態に係るマルチセンサ判定装置の機能的構成を示すブロック図である。
【図７】第２の実施の形態に係るマルチセンサ判定装置におけるマルチセンサ判定処理ルーチンの内容を示すフローチャートである。
【図８】従来技術の問題点を説明するためのイメージ図である。
【発明を実施するための形態】
【００２１】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２２】
まず、本実施の形態の原理について説明する。
【００２３】
図１に示すように、時刻ｔ−１までに観測された観測値のうち、予め外れ値ではないと判定済みの観測値を用いて、推定値の確率分布を算出する。この推定値の確率分布と、時刻ｔで新たに観測された観測値とを比較することにより、新たな観測値が外れ値か否かを判定する。そして、外れ値であると判定された場合には、その観測値を廃棄し、外れ値ではないと判定された場合には、次時刻において、推定値の確率分布の算出に用いられる。
【００２４】
このように、外れ値ではないと判定された観測値のみを推定値の確率分布の算出に用いることで、連続して外れ値が発生し、所定時間内に観測された観測値の大半を外れ値が占める場合でも、常に正常な過去の観測値のみに基づいて、外れ値の判定に用いる推定値の確率分布を適切に算出することができ、正確な外れ値判定を行うことができる。
【００２５】
以下で、各実施の形態について説明する。なお、各実施の形態では、車両に搭載され、複数のセンサを備えたマルチセンサの観測値から自車両の位置を推定する場合において、マルチセンサの外れ値の判定、及び推定される状態の推定値の信頼度を算出するマルチセンサ判定装置に、本発明を適用した場合を例に説明する。
【００２６】
図２に示すように、第１の実施の形態におけるマルチセンサ判定装置１０は、異なる次元の観測値を各々観測する複数のセンサで構成されたマルチセンサ１２と、外れ値の判定及び推定値の信頼度の算出の処理を実行するコンピュータ１６とを備えている。
【００２７】
マルチセンサ１２は、複数のセンサとして、ＧＰＳ衛星から送信された信号を受信するＧＰＳ装置、自車両の速度を検出する速度センサ、自車両のヨーレイトを検出するヨーレイトセンサ、及び地図情報を取得する地図情報取得部を含んで構成されている。これらの各センサから、擬似距離、ＩＮＳ速度、ドップラー周波数、位相、地図情報等が観測値として観測される。
【００２８】
コンピュータ１６は、ＣＰＵ、後述するマルチセンサ判定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。
【００２９】
このコンピュータ１６を以下で説明するマルチセンサ判定処理ルーチンに従って機能ブロックで表すと、図２に示すように、マルチセンサ１２で観測された観測値から自車両の位置（状態）を推定する状態推定部２０と、初期条件に基づいて観測値が外れ値か否かを判定する初期プールフィルタ２２と、初期プールフィルタ２２及び後述する外れ値判定部３２を通過した観測値が記憶される観測プール２４と、観測値から推定される推定値の事前確率分布を算出する事前分布算出部２６と、推定値の事前確率分布及び観測プールに記憶された観測値に基づいて、推定値の事後確率分布を算出する事後分布算出部２８と、推定値の事後確率分布に基づいて、各観測値の確率分布を算出する観測値分布算出部３０と、各観測値の確率分布と新たに観測された観測値とを比較して、外れ値か否かを判定する外れ値判定部３２と、事後確率分布に基づいて、推定値の信頼度を算出する信頼度算出部３４と、を含んだ構成で表すことができる。
【００３０】
なお、事後分布算出部２８が本発明の確率分布算出手段の一例であり、観測値分布算出部３０及び外れ値判定部３２が本発明の判定手段の一例である。
【００３１】
状態推定部２０は、マルチセンサ１２で観測された観測値Ｄを取得し、観測値Ｄに基づいて状態を示す推定値ｘを推定する。ここでは、観測値Ｄは、擬似距離ρ（衛星位置ｘｓａｔ）、ＩＮＳ速度ｖ、地図情報における自車両から最も近い道路上の位置ｘｎｅａｒｍａｐ等の各観測値を含んでいる。また、推定値ｘは自車両の位置である。観測値Ｄに基づく推定値ｘの推定は、従来既知の手法を用いることができる。なお、以下では、各観測値をまとめて表現する場合には、観測値Ｄと表記し、観測値Ｄに含まれる個々の観測値を表現する場合には、観測値ｄと表記する。
【００３２】
初期プールフィルタ２２は、観測プール２４に観測値が記憶されていない場合に、初期条件に従って観測値を観測プールへ記憶するか否かを判定する。例えば、ＧＰＳ装置により観測された観測値の場合には、ＧＰＳ衛星からの信号のＳ／Ｎや衛星仰角に基づいて、受信状況が良好か否かにより判定したり、ＤＯＰにより衛星の配置が良好か否かを判定したりすることができる。また、電波を受信した衛星数や、観測値や残差の時系列のばらつき等により判定するようにしてもよい。初期プールフィルタ２２における初期条件は、厳しい条件を設定しておくことが好ましい。
【００３３】
観測プール２４には、初期プールフィルタ２２を通過した観測値が記憶される。また、後述する外れ値判定部３２で外れ値ではないと判定された観測値が記憶される。
【００３４】
事前分布算出部２６は、マルチセンサ１２を構成する各センサの誤差分布など、予め知識として把握できる情報に基づいて、各観測値ｄの事前確率分布を算出し、この各観測値ｄの事前確率分布に基づいて、観測値Ｄから推定される推定値ｘの事前確率分布ｐ（ｘ）を算出する。観測値ｄの事前確率分布としては、例えば、擬似距離ρについて、下記（１）式に示すような擬似距離精度σＧＰＳを用いることができる。
【００３５】
【数１】
【００３６】
なお、推定値の事前確率分布ｐ（ｘ）として、後述する事後分布算出部２８において、一時刻前に算出された推定値の事後確率分布を用いてもよい。
【００３７】
事後分布算出部２８は、事前分布算出部２６で算出された推定値の事前確率分布ｐ（ｘ）、及び観測プールに記憶された観測値Ｄを用いて算出される、推定値ｘに対する観測値Ｄの確からしさを示す尤度ｐ（Ｄ｜ｘ）に基づいて、下記（２）式に従って、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。
【００３８】
【数２】
【００３９】
なお、Ｚは正規化定数である。
【００４０】
より具体的には、まず、観測値の尤度ｐ（Ｄ｜ｘ）を、観測値ｄ毎に算出し、これをｐ（ｄ｜ｘ）とする。例えば、擬似距離の尤度ｐ（ρ｜ｘ）、ＩＮＳ速度ｖから得られる移動ベクトルの尤度ｐ（ｘｔ｜ｘｔ−１）、及び自車両と道路とがどれくらい離れているかの指標ｌの尤度ｐ（ｌ｜ｘ）は、下記（３）〜（５）式により算出することができる。
【００４１】
【数３】
【００４２】
ここで、Ｃｂはクロックバイアスである。また、Ｎ（μ，σ２）は、平均μ、標準偏差σの正規分布を表す。（３）〜（５）式を用いた場合には、（２）式は、下記（６）式のように表すことができる。また、各式のｘは、各観測値ｄが観測された際に、状態推定部２０で観測値ｄを含む観測値Ｄに基づいて推定された推定値ｘである。
【００４３】
【数４】
【００４４】
観測値分布算出部３０は、事後分布算出部２８で算出された推定値の事後確率分布ｐ（ｘ｜Ｄ）を現在の推定値の分布として用い、この現在の推定値の分布と各観測値の尤度とに基づいて、各観測値の確率分布ｐ（ｄ）を算出する。例えば、擬似距離ρの場合には、下記（７）式により、擬似距離ρの確率分布ｐ（ρ）を算出することができる。
【００４５】
【数５】
【００４６】
外れ値判定部３２は、マルチセンサ１２で観測された各観測値ｄと、観測値分布算出部３０で算出された各観測値の確率分布ｐ（ｄ）とを次元毎に比較して、観測された各観測値ｄが外れ値か否かを判定する。例えば、図３に示すように、観測された擬似距離ρについては、擬似距離の確率分布ｐ（ρ）と比較して、観測された擬似距離の内側確率を外れ値である確率とし、外れ値である確率が予め定めた所定値以上か否かにより、観測された擬似距離が外れ値か否かを判定することができる。また、外れ値判定部３２は、外れ値であると判定した観測値ｄを破棄し、外れ値ではないと判定した観測値ｄを観測プール２４に記憶する。
【００４７】
信頼度算出部３４は、事後分布算出部２８で算出された推定値の事後確率分布ｐ（ｘ｜Ｄ）に基づいて、状態推定部２０で推定される推定値ｘの信頼度を算出する。例えば、図４に示すように、推定値の事後確率分布ｐ（ｘ｜Ｄ）の平均を中心に幅ａの区間を設定し、この区間に入る確率を信頼度として算出することができる。区間幅ａは、要求される精度に応じて定めることができる。
【００４８】
次に、図５を参照して、第１の実施の形態のマルチセンサ判定装置１０において実行されるマルチセンサ判定処理ルーチンについて説明する。
【００４９】
ステップ１００で、マルチセンサ１２で観測された観測値Ｄ（各観測値ｄ）を取得し、観測値Ｄに基づいて状態を示す推定値ｘを推定する。
【００５０】
次に、ステップ１０２で、観測プール２４に過去に観測された正常な観測値が記憶されているか否かを判定する。観測プール２４に観測値が記憶されていない場合には、ステップ１０４へ移行して、初期プールフィルタ２２を通過した観測値ｄを観測プール２４へ記憶し、初期プールフィルタ２２を通過しなかった観測値ｄを廃棄する。一方、観測プールに観測値が記憶されている場合には、ステップ１０６へ移行する。
【００５１】
ステップ１０６では、マルチセンサ１２を構成する各センサの誤差分布など、予め知識として把握できる情報に基づいて、観測値Ｄから推定される推定値ｘの事前確率分布ｐ（ｘ）を算出する。なお、一時刻前に算出された推定値ｘの事後確率分布ｐ（ｘ｜Ｄ）を用いてもよい。
【００５２】
次に、ステップ１０８で、観測プール２４に記憶された観測値Ｄを用いて、推定値ｘに対する観測値Ｄの確からしさを示す尤度ｐ（Ｄ｜ｘ）を算出する。次に、ステップ１１０で、上記ステップ１０６で算出された推定値の事前確率分布ｐ（ｘ）、及び上記ステップ１０８で算出された尤度ｐ（Ｄ｜ｘ）に基づいて、（２）式に従って、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。
【００５３】
次に、ステップ１１２で、上記ステップ１１０で算出した推定値の事後確率分布ｐ（ｘ｜Ｄ）を現在の推定値の分布として用い、この現在の推定値の分布と各観測値の尤度ｐ（ｄ｜ｘ）とに基づいて、各観測値の確率分布ｐ（ｄ）を算出する。
【００５４】
次に、ステップ１１４で、上記ステップ１００で取得した観測値Ｄの各観測値ｄと、上記ステップ１１２で算出した各観測値の確率分布ｐ（ｄ）の各々とを観測値の次元毎に比較する。次に、ステップ１１６で、上記ステップ１１４の比較結果に基づいて、上記ステップ１００で取得された観測値Ｄの各観測値ｄが外れ値か否かを判定する。観測値ｄが外れ値の場合には、ステップ１１８へ移行して、その観測値ｄを破棄し、観測値ｄが外れ値ではない場合には、ステップ１２０へ移行して、その観測値ｄを観測プール２４に記憶する。
【００５５】
次に、ステップ１２２で、上記ステップ１１０で算出された推定値の事後確率分布ｐ（ｘ｜Ｄ）に基づいて、上記ステップ１００で推定された推定値ｘの信頼度を算出して出力し、ステップ１００に戻る。
【００５６】
以上説明したように、第１の実施の形態のマルチセンサ判定装置によれば、外れ値ではないと判定された観測値のみを観測プールに記憶し、観測プールに記憶された観測値に基づいて、外れ値の判定に用いる推定値の確率分布を算出するため、連続して外れ値が発生し、所定時間内に観測された観測値の大半を外れ値が占める場合でも、適切に外れ値を判定することができる。
【００５７】
また、次元の異なる観測値から推定された推定値の事後確率分布から、各観測値の確率分布を算出して、外れ値の判定を行うため、次元の異なる複数の観測値を比較して矛盾を判定することができ、精度の高い判定を行うことができる。
【００５８】
また、上記のように次元の異なる複数の観測値を統合して算出した推定値の事後確率分布から推定値の信頼度を算出するため、精度良く信頼度を算出することができる。
【００５９】
次に、第２の実施の形態について説明する。第２の実施の形態では、外れ値ではないと判定された観測値を記憶しておくのではなく、そこから算出された推定値の事後確率分布を記憶しておく点が、第１の実施の形態と異なる。なお、第２の実施の形態のマルチセンサ判定装置において、第１の実施の形態のマルチセンサ判定装置１０の構成と同様の構成については、同一符号を付して詳細な説明を省略する。
【００６０】
図６に示すように、第２の実施の形態におけるマルチセンサ判定装置２１０は、マルチセンサ１２と、コンピュータ２１６とを備えている。このコンピュータ２１６を以下で説明するマルチセンサ判定処理ルーチンに従って機能ブロックで表すと、図６に示すように、状態推定部２０と、初期プールフィルタ２２と、事前分布算出部２６と、事後分布算出部２２８と、算出された推定値の事後確率分布を記憶する事後分布記憶部３６と、観測値分布算出部２３０と、外れ値判定部３２と、外れ値ではないと判定された観測値を用いて推定値の事後確率分布を更新する事後分布更新部３８と、信頼度算出部３４と、を含んだ構成で表すことができる。
【００６１】
事後分布算出部２２８は、初期プールフィルタ２２を通過した観測値に基づいて、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する点が、観測プール２４に記憶された観測値に基づいて、推定値の事後確率分布を算出する第１の実施の形態における事後分布算出部２８と異なるだけである。
【００６２】
観測値分布算出部２３０は、事後分布記憶部３６に一旦記憶された推定値の事後確率分布に基づいて、観測値の確率分布ｐ（ｄ）を算出する点が、第１の実施の形態における観測値分布算出部３０と異なるだけである。
【００６３】
事後分布更新部３８は、外れ値判定部３２で外れ値ではないと判定された観測値を用いて、事後分布記憶部３６に記憶された推定値の事後確率分布ｐ（ｘ｜Ｄ）を更新する。
【００６４】
次に、図７を参照して、第２の実施の形態のマルチセンサ判定装置２１０において実行されるマルチセンサ判定処理ルーチンについて説明する。なお、第１の実施の形態のマルチセンサ判定処理ルーチンと同一の処理については、同一符号を付して詳細な説明を省略する。
【００６５】
ステップ１００で、マルチセンサ１２で観測された観測値Ｄ（各観測値ｄ）を取得し、観測値Ｄに基づいて状態を示す推定値ｘを推定する。
【００６６】
次に、ステップ２０２で、事後分布記憶部３６に推定値の事後確率分布が記憶されているか否かを判定する。推定値の事後確率分布が記憶されていない場合には、ステップ２０４へ移行し、記憶されている場合には、ステップ１１２へ移行する。
【００６７】
ステップ２０４では、上記ステップ１００で取得された観測値Ｄに初期プールフィルタ２２を適用し、次に、ステップ１０６で、推定値ｘの事前確率分布ｐ（ｘ）を算出する。次に、ステップ２０８で、上記ステップ２０４で初期プールフィルタ２２を通過した観測値Ｄを用いて、観測値Ｄの尤度ｐ（Ｄ｜ｘ）を算出し、次に、ステップ１１０で、推定値の事後確率分布ｐ（ｘ｜Ｄ）を算出する。次に、ステップ２０９で、上記ステップ１１０で算出した推定値の事後確率分布ｐ（ｘ｜Ｄ）を事後分布記憶部３６に記憶して、ステップ１００へ戻る。
【００６８】
上記ステップ２０２で肯定判定されて、ステップ１１２へ移行した場合には、ステップ１１２〜１１６を実行して、上記ステップ１００で取得された観測値Ｄの各観測値ｄが外れ値か否かを判定し、観測値ｄが外れ値の場合には、ステップ１１８へ移行して、その観測値ｄを破棄し、観測値ｄが外れ値ではない場合には、ステップ２２０へ移行して、その観測値ｄを用いて、事後分布記憶部３６に記憶された推定値の事後確率分布ｐ（ｘ｜Ｄ）を更新する。
【００６９】
次に、ステップ１２２で、推定値ｘの信頼度を算出して出力し、ステップ１００に戻る。
【００７０】
以上説明したように、第２の実施の形態のマルチセンサ判定装置によれば、第１の実施の形態と同様の効果を奏すると共に、外れ値ではないと判定された観測値自体ではなく、そこから算出された分布を記憶しておくため、計算量及びメモリ量を削減することができる。
【００７１】
なお、上記第１及び第２の実施の形態では、（７）式に従って観測値の確率分布ｐ（ｄ）を算出する場合について説明したが、下記（８）式及び（９）式に従って算出してもよい。
【００７２】
【数６】
【００７３】
ここで、Ｈは観測方程式のデザイン行列、Ｑｙは観測値ｙの共分散行列、ｒｙは観測値ｙの残差、ａ及びｂはパラメータである。
【００７４】
また、上記第１及び第２の実施の形態では、推定値の事後確率分布に基づいて推定値の信頼度を算出する場合について説明したが、推定値の事前確率分布に基づいて信頼度を算出するようにしてもよい。例えば、下記（１０）に示す観測値ｙの分散行列、及び下記（１１）式に示す観測方程式から、下記（１２）式に示す推定値の事前確率分布を算出し、この推定値の事前確率分布Ｑｘに基づいて、推定値の信頼度を算出することができる。
【００７５】
【数７】
【符号の説明】
【００７６】
１０、２１０ マルチセンサ判定装置
１２ マルチセンサ
１６、２１６ コンピュータ
２０ 状態推定部
２２ 初期プールフィルタ
２４ 観測プール
２６ 事前分布算出部
２８、２２８ 事後分布算出部
３０、２３０ 観測値分布算出部
３２ 外れ値判定部
３４ 信頼度算出部
３６ 事後分布記憶部
３８ 事後分布更新部
【特許請求の範囲】
【請求項１】
各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサと、
前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段と、
前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段と、
を含むマルチセンサ判定装置。
【請求項２】
前記判定手段により外れ値ではないと判定された観測値を記憶する観測値記憶手段を含み、
前記確率分布算出手段は、前記観測値記憶手段に記憶された観測値を用いて、前記推定値の確率分布を算出する
請求項１記載のマルチセンサ判定装置。
【請求項３】
前記確率分布算出手段により算出された推定値の確率分布を記憶する確率分布記憶手段と、
前記確率分布記憶手段に記憶された推定値の確率分布を、前記判定手段により外れ値ではないと判定された観測値を用いて更新する更新手段と、を含み、
前記判定手段は、前記更新手段により前記推定値の確率分布が更新された場合には、該更新された推定値の確率分布を用いて、前記新たな観測値各々が外れ値か否かを判定する
請求項１記載のマルチセンサ判定装置。
【請求項４】
前記確率分布算出手段により算出された推定値の確率分布、または前記複数のセンサ各々の誤差分布に基づいて算出される前記推定値の事前確率分布に基づいて、前記推定値の信頼度を算出する信頼度算出手段を含む請求項１〜請求項３のいずれか１項記載のマルチセンサ判定装置。
【請求項５】
前記マルチセンサを、ＧＰＳ衛星から送信された信号を受信するＧＰＳ装置、前記マルチセンサが搭載される移動体の速度を検出する速度センサ、前記移動体のヨーレイトを検出するヨーレイトセンサ、及び地図情報を取得するセンサの少なくとも１つを含んで構成し、
前記確率分布算出手段は、前記移動体の位置または速度の確率分布を算出する
請求項１〜請求項４のいずれか１項記載のマルチセンサ判定装置。
【請求項６】
コンピュータを、
各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサで観測された観測値を取得する取得手段、
前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段、及び
前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段
として機能させるためのマルチセンサ判定プログラム。
【請求項７】
コンピュータを、請求項１〜請求項４のいずれか１項記載のマルチセンサ判定装置を構成する各手段として機能させるためのマルチセンサ判定プログラム。
【請求項１】
各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサと、
前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段と、
前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段と、
を含むマルチセンサ判定装置。
【請求項２】
前記判定手段により外れ値ではないと判定された観測値を記憶する観測値記憶手段を含み、
前記確率分布算出手段は、前記観測値記憶手段に記憶された観測値を用いて、前記推定値の確率分布を算出する
請求項１記載のマルチセンサ判定装置。
【請求項３】
前記確率分布算出手段により算出された推定値の確率分布を記憶する確率分布記憶手段と、
前記確率分布記憶手段に記憶された推定値の確率分布を、前記判定手段により外れ値ではないと判定された観測値を用いて更新する更新手段と、を含み、
前記判定手段は、前記更新手段により前記推定値の確率分布が更新された場合には、該更新された推定値の確率分布を用いて、前記新たな観測値各々が外れ値か否かを判定する
請求項１記載のマルチセンサ判定装置。
【請求項４】
前記確率分布算出手段により算出された推定値の確率分布、または前記複数のセンサ各々の誤差分布に基づいて算出される前記推定値の事前確率分布に基づいて、前記推定値の信頼度を算出する信頼度算出手段を含む請求項１〜請求項３のいずれか１項記載のマルチセンサ判定装置。
【請求項５】
前記マルチセンサを、ＧＰＳ衛星から送信された信号を受信するＧＰＳ装置、前記マルチセンサが搭載される移動体の速度を検出する速度センサ、前記移動体のヨーレイトを検出するヨーレイトセンサ、及び地図情報を取得するセンサの少なくとも１つを含んで構成し、
前記確率分布算出手段は、前記移動体の位置または速度の確率分布を算出する
請求項１〜請求項４のいずれか１項記載のマルチセンサ判定装置。
【請求項６】
コンピュータを、
各々次元の異なる観測値を観測する複数のセンサで構成されたマルチセンサで観測された観測値を取得する取得手段、
前記マルチセンサの各センサにより過去に観測され、かつ外れ値を含まない前記各々次元の異なる観測値から推定される状態を示す推定値の確率分布を算出する確率分布算出手段、及び
前記確率分布算出手段により算出された推定値の確率分布に基づいて算出された前記観測値各々の確率分布と、前記マルチセンサで観測された新たな観測値各々とを次元毎に比較して、前記新たな観測値各々が外れ値か否かを判定する判定手段
として機能させるためのマルチセンサ判定プログラム。
【請求項７】
コンピュータを、請求項１〜請求項４のいずれか１項記載のマルチセンサ判定装置を構成する各手段として機能させるためのマルチセンサ判定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１２−２３３８００（Ｐ２０１２−２３３８００Ａ）
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願番号】特願２０１１−１０２９１０（Ｐ２０１１−１０２９１０）
【出願日】平成２３年５月２日（２０１１．５．２）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願日】平成２３年５月２日（２０１１．５．２）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]