(57)【要約】【課題】内部モデルに誤差が生じている場合に安定した位置推定結果を出力できるようになり、外部モデルに長時間の誤差が生じている場合でも、位置推定結果の異常を設定した誤差の範囲内で検知して、ユーザーに対して報知する。【解決手段】内部モデルに異常があることを判断するモデル異常判断手段と、内部モデルに異常がある場合にその誤差を計算する内部モデル誤差算出手段と、内部モデルおよび外部モデルの異常の状態から、位置推定装置が出力する位置が破たんする可能性を判断し、システム状態の異常として出力するシステム異常報知手段と、を有し、モデル異常判断手段で異常、および、観測異常判断手段で異常と判断した場合に、システム異常報知手段により、出力位置が破たんする可能性を報知する位置推定装置。【選択図】図１
2019082328.tif 000002
【特許請求の範囲】
【請求項１】
物体の運動パラメータから物体の位置を更新する内部モデル演算手段と、 
物体の位置を計測する位置計測手段から物体の位置そのものを計算する外部モデル演算手段と、
内部モデルが算出する誤差分散値を基に前記外部モデルが算出する位置の外れ値を判断する観測異常判断手段の結果を用いて、確率フィルタにより前記内部モデルの出力位置を修正する確率フィルタ計算手段と、を有する位置推定装置であって、
前記内部モデルに異常があることを判断するモデル異常判断手段と、
前記内部モデルに異常がある場合にその誤差を計算する内部モデル誤差算出手段と、
前記内部モデルおよび前記外部モデルの異常の状態から、前記位置推定装置が出力する位置が破たんする可能性を判断し、システム状態の異常として出力するシステム異常報知手段と、を有し、
前記モデル異常判断手段で異常、および、前記観測異常判断手段で異常と判断した場合に、前記システム異常報知手段により、出力位置が破たんする可能性を報知する位置推定装置。
【請求項２】
請求項１の位置推定装置において、
前記観測異常判断手段は、マハラノビス距離による外れ値判定に用いる誤差分散に、前記内部モデル誤差算出手段により算出された前記内部モデルのモデル化誤差の誤差分散を加味して、外れ値判定をする位置推定装置。
【請求項３】
請求項１の位置推定装置において、
前記モデル異常判断手段は、前記外部モデルが出力する位置を端点として、前記内部モデルによる位置の更新によって算出できる前記外部モデルとの差分の系列を、時間的に順方向と逆方向で同じ系列となるかを判断する位置推定装置。
【請求項４】
請求項１の位置推定装置において、
前記内部モデルのモデル化誤差を前記外部モデルが算出する位置と位置推定装置の推定位置との差分から計算するこ位置推定装置。
【請求項５】
請求項１の位置推定装置において、
前記システム異常報知手段は、前記外部モデルの異常か、前記内部モデルの異常かを判断し、前記外部モデルの異常の場合は前記位置推定装置以外のシステム異常、前記内部モデルの異常の場合は、前記位置推定装置のシステム異常が発生している可能性があることを、システム状態として出力する位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置推定装置に関する。
【背景技術】
【０００２】
位置推定装置は、全地球航法衛星システム（GNSS）や慣性計測装置（IMU）、車輪速度計等を用いて、車両の位置を推定する装置のことである。GNSS等のように直接位置を算出する機器から出力される位置データを、所望の座標系における位置に変換する際に用いる数式を外部モデルと呼ぶ。また、移動体の位置を、速度、姿勢、加速度、角速度などの関連する運動パラメータから順次推定する数式を内部モデルと呼ぶ。
【０００３】
従来では、内部モデルを用いて算出した位置を、カルマンフィルタなどに代表される確率フィルタを用いて、外部モデルにて算出された位置へ徐々に修正する手法を数多くみることができる。この確率フィルタを用いた位置修正方法は、内部モデル、外部モデルから推定される誤差を考慮して、位置を修正するため、確率的に最も存在する可能性の高い位置を算出できることが長所である。しかし、外部モデルによって出力された位置が実際の位置と大きく異なる、所謂外れ値が存在する場合には、フィルタからの出力結果が外れ値に影響されて大きな誤差を持つようになる。このような外れ値を除去するために、χ2検定や、マハラノビス距離による外れ値検出、除去が行われる。このような外れ値除去手法では、内部モデルで算出した位置と、その分散値から、外部モデルが算出した位置の存在確率を算出し、確率の低い位置を外れ値として検出する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１４−２２８４９５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
従来の外れ値除去手法は内部モデルでの見積もり誤差を信用して判断する仕組みになっている。そのため、内部モデルにて見積もられた誤差が過小であった場合や、数式として表わすことができていない現象がある場合には、内部モデルの出力する誤差が小さく、外部モデルが算出する位置の多くを全て外れ値として除去してしまうという問題があった。このような内部モデルの誤差をモデル化誤差と呼ぶことにする。
【０００６】
このようなモデル化誤差が存在する場合、従来は文献１に開示されているような、確率フィルタにおいて、常に外部モデルの算出位置をフィルタ内に取り込むという手法があった。しかし、この手法は、外部モデルにて、算出されるべき位置がしばらくの間大きな誤差を含んでいた場合に、推定する位置が大きくずれていくことになる。その際に、推定位置がずれている可能性をユーザーに報知することができれば、ユーザーにて対処可能であるが、従来の手法では、推定位置がずれている可能性ことを認識できず、ユーザーに対して的確に知らせるこということもできないという問題があった。そのため、位置推定装置内でのエラーが発生しても、ユーザーがエラーかどうかの判断をすることができなかった。
【課題を解決するための手段】
【０００７】
上記課題を解決するための本発明の特徴は、例えば以下の通りである。
【０００８】
物体の運動パラメータから物体の位置を更新する内部モデル演算手段と、 物体の位置を計測する位置計測手段から物体の位置そのものを計算する外部モデル演算手段と、内部モデルが算出する誤差分散値を基に外部モデルが算出する位置の外れ値を判断する観測異常判断手段の結果を用いて、確率フィルタにより内部モデルの出力位置を修正する確率フィルタ計算手段と、を有する位置推定装置であって、内部モデルに異常があることを判断するモデル異常判断手段と、内部モデルに異常がある場合にその誤差を計算する内部モデル誤差算出手段と、内部モデルおよび外部モデルの異常の状態から、位置推定装置が出力する位置が破たんする可能性を判断し、システム状態の異常として出力するシステム異常報知手段と、を有し、モデル異常判断手段で異常、および、観測異常判断手段で異常と判断した場合に、システム異常報知手段により、出力位置が破たんする可能性を報知する位置推定装置。
【発明の効果】
【０００９】
本発明により、内部モデルに誤差が生じている場合に安定した位置推定結果を出力できるようになり、外部モデルに長時間の誤差が生じている場合でも、位置推定結果の異常を設定した誤差の範囲内で検知して、ユーザーに対して報知することが可能となる。上記した以外の課題、構成及び効果は以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【００１０】
【図１】位置推定装置の概略構成を示す図である。
【図２】位置推定装置の処理フローを示した図である。
【図３】一時記憶領域のデータ構成を示した図である。
【図４】観測異常判断手段の処理フローを示した図である。
【図５】確率フィルタ計算手段の処理フローを示した図である。
【図６】モデル異常判断手段の処理フローを示した図である。
【図７】時間順方向の誤差系列のイメージを示した図である。
【図８】時間逆方向の誤差系列のイメージを示した図である。
【図９】誤差系列データテーブルのデータ構成を示した図である。
【図１０】内部モデル誤差算出手段の処理フローを示した図である。
【図１１】システム異常報知手段の処理フローを示した図である。
【発明を実施するための形態】
【００１１】
以下、位置推定装置の異常状態を通知するための処理について説明する。
【実施例１】
【００１２】
本発明は、自己の位置を推定する装置に関するもので、車両をはじめとする移動体の移動軌跡や走行制御に用いるものである。本発明では、確率フィルタを用いた位置推定装置において、外部モデルに異常がないサンプルを抜き出し、内部モデルの異常を判断することで、モデル化誤差を検知し、モデル化誤差に由来する内部モデルの誤差分散を車両状態毎に学習する。この誤差分散値を用いて、外れ値判定をすることで、外部モデルの外れ値が適切に判断できるようになる。
【００１３】
従来は、外部モデルにて、算出されるべき位置がしばらくの間大きな誤差を含んでいた場合に、推定する位置が大きくずれていくことになる。その際に、推定位置がずれている可能性をユーザーに報知することができれば、ユーザーにて対処可能であるが、従来の手法では、推定位置がずれている可能性ことを認識できず、ユーザーに対して的確に知らせるこということもできないという問題があった。そこで、確率フィルタを用いた位置推定装置において、外部モデルに異常がないサンプルを抜き出し、内部モデルの異常を判断することで、モデル化誤差を検知し、モデル化誤差に由来する内部モデルの誤差分散を車両状態毎に学習する。この誤差分散値を用いて、外れ値判定をすることで、外部モデルの外れ値が適切に判断できるようになる。
【００１４】
本発明の特徴として、例えば下記が挙げられる。
【００１５】
観測異常判断手段は、マハラノビス距離による外れ値判定に用いる誤差分散に、内部モデル誤差算出手段により算出された内部モデルのモデル化誤差の誤差分散を加味して、外れ値判定をする位置推定装置。
【００１６】
モデル異常判断手段は、外部モデルが出力する位置を端点として、内部モデルによる位置の更新によって算出できる外部モデルとの差分の系列を、時間的に順方向と逆方向で同じ系列となるかを判断する位置推定装置。
【００１７】
内部モデルのモデル化誤差を外部モデルが算出する位置と位置推定装置の推定位置との差分から計算する位置推定装置。
【００１８】
システム異常報知手段は、外部モデルの異常か、内部モデルの異常かを判断し、外部モデルの異常の場合は位置推定装置以外のシステム異常、内部モデルの異常の場合は、位置推定装置のシステム異常が発生している可能性があることを、システム状態として出力する位置推定装置。
【００１９】
図１に本発明の代表的な構成例を示す。実施例１では、例として、外部モデルには位置計測手段１０１で得られた位置を用い、姿勢計測手段１０２と速度計測手段１０３と加速度計測手段１０４および角速度計測手段１０５により、内部モデルを計算するような位置推定装置１０６を考える。また、実施例１における位置推定装置１０６は、地上を走行する車両に搭載されていることを前提としている。
【００２０】
位置推定装置１０６には、位置計測手段１０１が持つ座標系で出力した位置を、位置推定装置１０６が出力すべき座標系へ変換する外部モデル演算手段１０７と、姿勢計測手段１０２と速度計測手段１０３と加速度計測手段１０４および角速度計測手段１０５のそれぞれから得られる車両の運動パラメータから位置の更新を算出する内部モデル演算手段１０８と、内部モデルから得られた位置と外部モデルから得られた位置から、位置計測手段が出力する位置における外れ値の有無を判断する観測異常判断手段１０９と、カルマンフィルタ等を用いて、最も存在確率の高い点を求める位置として算出する確率フィルタ計算手段１１０と、内部モデルにおいてモデル化誤差の有無を判断するモデル異常判断手段１１１と、内部モデルの異常を逐次判断するために必要なデータが保存されている誤差系列データテーブル１１２と、内部モデルのモデル化誤差を学習する内部モデル誤差算出手段１１３と、外部モデルの異常状態および内部モデルの異常状態から、位置推定結果が信用できなくなるかどうかを判断し、信用できなくなる可能性がる場合に、その旨を報知するシステム異常報知手段１１４と、位置推定装置１０６内における各算出手段からの出力値を一時的に記憶する一時記憶領域１１５が備わっている。
【００２１】
上述の構成をもつ位置推定装置１０６における処理フローを図２に示す。
【００２２】
まず、ステップ２０１にて、ある一定周期でデータを出力する計測手段から位置・速度・姿勢などの運動パラメータを取得する。計測手段とその運動パラメータは、上述の、位置計測手段１０１から位置および位置計測フラグ、姿勢計測手段１０２から姿勢および姿勢計測フラグ、速度計測手段１０３から速度および速度計測フラグ、加速度計測手段１０４から加速度および加速度計測フラグ、角速度計測手段１０５から角速度および角速度計測フラグである。各計測手段から出力される計測フラグは、そのサンプル回にて計測手段から情報が出力されているかどうかを示すフラグであり、「１」なら出力有、「０」なら出力無である。各計測手段は時間同期がとれているものとする。ただし、計測手段から、各運動パラメータおよび、位置の情報を取得すると、ステップ２０２へ移行する。ステップ２０２では、外部モデル演算手段１０７において、外部モデルにより、位置計測手段１０１から取得した位置を、位置推定装置１０６が出力する位置へ座標変換し、一時記憶領域１１５へ記録する。また、取得フラグおよび各計測手段が出力もしくは事前に設定された誤差分散値は、一時記憶領域１１５に記憶される。図３に一時記憶領域１１５のデータ構成を示す。
もし、位置計測手段１０１からの出力がない場合は、外部モデル出力の取得フラグに０を設定し、その他の処理はスキップする。座標変換は以下の（式１）から求める。
【００２３】
【数１】
2019082328.tif 000003
【００２４】
【数２】
2019082328.tif 000004
【００２５】
【数３】
2019082328.tif 000005
【００２６】
ここで、（式１００）は、座標変換後の外部モデル出力値であり、（式１０１）は位置計測手段１０１からの出力、（式１）のlは位置計測手段１０１と位置推定装置１０６が出力する位置までを表わすベクトルである。（式１）のRiは位置計測手段１０１から出力される第i番目の位置を位置推定装置１０６が出力する座標系に変換する行列である。また、Rは車両座標で表わされた位置計測手段１０１と位置推定装置１０６が出力する位置までを表わすベクトルを位置推定装置１０６が出力する座標系に変換する行列であり、姿勢計測手段１０２もしくは内部モデル演算手段１０８にて、車両の姿勢が取得できる場合は、その値を用いることができる。また、設計などにより規定された値が存在するなら、その値を用いてもよい。
【００２７】
例えば、位置計測手段１０１としてGNSSを用いる場合を考える。GNSSが得られる位置はGNSSアンテナの位置であり、車両に備え付けられた位置推定装置１０６と、同じ車両上かつ位置推定装置１０６とは異なった位置に備えたけられたGNSSアンテナに対して、以下のように位置を変換することができる。
【００２８】
GNSSから得られる座標系を絶対座標（東をｘ軸、北をｙ軸、地平面垂直方向をｚ軸とした座標系）と呼び、車両に固定された座標系を車両座標系（車体軸の進行方向をx軸、後車輪軸と平行に車体左方向にｙ軸、xy平面に対して、垂直車体上方向に）と呼ぶことにする。GNSSアンテナと位置推定装置の位置との差分lは車両座標系で表わされる。
【００２９】
今、車両の姿勢θがロール角、ピッチ角、ヨー角それぞれ、（式２００）であったとする。GNSSから得られた絶対座標で表わされたGNSSアンテナ位置を（式２０１）とし、車両座標系で表わされたGNSSアンテナと位置推定装置の出力すべき位置までを位置推定装置が出力する座標系で表わしたベクトルが（式２０２）であったとすると、位置推定装置１０６の位置を絶対座標で表わすと、以下の（式２）となる。
【００３０】
【数４】
2019082328.tif 000006
【００３１】
【数５】
2019082328.tif 000007
【００３２】
【数６】
2019082328.tif 000008
【００３３】
【数７】
2019082328.tif 000009
【００３４】
ただし、（式２）のRθは車両座標系から絶対座標系へ変換する行列は、以下の（式３）のように表わされる。
【００３５】
【数８】
2019082328.tif 000010
【００３６】
次にステップ２０３にて、位置推定装置が出力する位置を、姿勢や速度等の情報を用いて、更新する内部モデルの計算をする。実施例１での内部モデルは、姿勢計測手段１０２、速度計測手段１０３、加速度計測手段１０４および角速度計測手段１０５から位置の更新を計算する。位置の更新は、例えば以下の（式４）のように計算できる。
【００３７】
姿勢計測手段１０２から得られた車両の姿勢θにより、車両座標系から絶対座標系への変換行列Rθが決まった場合、
【００３８】
【数９】
2019082328.tif 000011
【００３９】
ただし、（式４）のWは予測誤差分散値と呼び、各計測手段からの誤差の標準偏差から求まる共分散行列である。
【００４０】
また、加速度や角速度がなくとも、上記の位置更新は計算可能であり、その場合は、以下の（式５）のように計算できる。
【００４１】
【数１０】
2019082328.tif 000012
【００４２】
また、内部モデル演算手段では、車両状態を算出する。
【００４３】
車両状態は、内部モデルに使用する各計測手段からの出力値により自動的に決定する。各計測手段からの出力値を変数に持つ関数から、車両状態（式６００）である確率（式６０１）は、以下の（式６）のように表わされる。
【００４４】
【数１１】
2019082328.tif 000013
【００４５】
【数１２】
2019082328.tif 000014
【００４６】
【数１３】
2019082328.tif 000015
【００４７】
【数１４】
2019082328.tif 000016
【００４８】
関数（式６０２）は、事前に決められているものとする。また、入力値と出力値から機械学習を用いて、逐次学習してもよい。
【００４９】
上述のステップ２０２およびステップ２０３の処理の順序は、逆転しても後の処理には影響しないため、逆転してもよい。
【００５０】
次にステップ２０４にて、観測異常判断手段１０９にて、外部モデル演算手段１０７からの出力における異常の有無を判断する。観測値の異常は、χ２乗検定等により実施する。
【００５１】
図４にχ2乗検定のフローを示す。
【００５２】
まず、ステップ４０１にて、一時記憶領域１１５の外部モデル出力の取得フラグをチェックし、外部モデルからの出力が存在するかを判断する。外部モデルからの出力がある場合は、ステップ４０２に移行し、無い場合は観測異常判断手段１０９の処理を終了する。
【００５３】
次に、ステップ４０２にて、内部モデルが出力する位置、分散値および車両状態を一時記憶領域１１５より取得する。
【００５４】
次に、ステップ４０３にて、一時記憶領域１１５に記憶された内部モデルが計算した予測誤差分散値に、内部モデルのモデル化誤差分散値を足し合わせる。内部モデルのモデル化誤差分散値の算出は後述するが、初期値としては事前に実測などにより得られた規定値が入力されているものとする。
【００５５】
次に、ステップ４０４にて、対象となる外部モデル出力値のマハラノビス距離を計算する。マハラノビス距離dは以下の（式７）により求めることができる。
【００５６】
【数１５】
2019082328.tif 000017
【００５７】
ただし、外部モデル出力値を（式７００）とし、ステップ３０２にて取得した内部モデルの出力する位置を（式７０１）、内部モデルの出力する誤差の分散値をΣと表わしている。
【００５８】
【数１６】
2019082328.tif 000018
【００５９】
【数１７】
2019082328.tif 000019
【００６０】
次に、ステップ４０５にて、χ2乗分布から、外部モデル出力値の発生確率から求めた閾値とマハラノビス距離を比較する。マハラノビス距離dの2乗は、χ2乗分布に従うため、χ2乗分布から任意の発生確率pに対する値を求めて、閾値を決定する。閾値は常時計算してもよいし、事前に固定した値でもよい。この閾値よりも、マハラノビス距離が閾値より小さい場合は、ステップ４０６へ進み、観測値使用可否フラグを１に設定する。マハラノビス距離が閾値より大きい場合は、ステップ４０７へ進み、観測値使用可否フラグを０に設定する。
【００６１】
次に、ステップ４０８にて、一時記憶領域１１５に設定されている取得フラグの値に、観測値使用可否フラグの値をかけ合わせて、値を更新し、観測異常判断手段１０９の処理を終了する。
【００６２】
ステップ２０４の観測異常判断手段１０９が終われば、ステップ２０５に進み、確率フィルタ計算手段１１０において、位置推定装置１０６から出力する推定位置および、推定した位置の誤差分散値を計算する。確率フィルタの代表的な方法としてカルマンフィルタや、パーティクルフィルタなどを挙げることができる。実施例１では、外部モデル演算手段１０７および内部モデル演算手段１０８において計算したそれぞれのモデルの誤差分散から、カルマンフィルタにより推定位置および誤差分散値を求める。
【００６３】
図５に確率フィルタ計算手段１１０の処理フローを示す。
【００６４】
ステップ５０１にて、一時記憶領域１１５に保存された、外部モデルおよび内部モデル演算手段の出力値から、位置、および分散値を取得する。また、外部モデルにおいては、取得フラグを、内部モデルにおいては車両状態を同じく取得する。
【００６５】
次にステップ５０２にて、外部モデルの取得フラグが１かどうかを判断する。取得フラグが１の場合は、ステップ５０３に移行する。取得フラグが０の場合はステップ５０４へ移行する。ステップ５０３では、外部モデル出力値が存在し、かつ外れ値でもないため、外部モデルの誤差分散値を設定し、ステップ５０５へ移行する。ステップ５０４では、外部モデル出力値が存在しない、もしくは外れ値であったため、外部モデルの誤差分散値に予め決められた極大値を設定する。また、外部モデル演算手段１０７が出力した位置を内部モデル演算手段１０８が出力した値と同じ値に設定する。
【００６６】
次にステップ５０５にて、カルマンフィルタにおけるイノベーションvを計算する。イノベーションvは外部モデル演算手段１０７が出力した位置Xおよび内部モデル演算手段１０８が出力した更新位置xから以下の（式８）のように求まる。
【００６７】
【数１８】
2019082328.tif 000020
【００６８】
次にステップ５０６に移行し、カルマンゲインKを計算する。カルマンゲインKは、以下の（式９）のように求めることができる。
【００６９】
【数１９】
2019082328.tif 000021
【００７０】
次にステップ５０７にて、推定位置を計算する。推定位置は、以下の（式１０）のように計算できる。
【００７１】
【数２０】
2019082328.tif 000022
【００７２】
次にステップ５０８にて、推定誤差分散を計算する。推定誤差分散は以下の（式１１）のように計算できる。
【００７３】
【数２１】
2019082328.tif 000023
【００７４】
最後にステップ５０９にて、計算した推定位置、推定誤差分散、イノベーション値を一時記憶領域１１５に記録し、推定位置を位置推定装置１０６から出力し、確率フィルタ計算手段１１０の処理を終了する。
【００７５】
確率フィルタ計算手段１１０の処理が終了すれば、ステップ２０７に移行し、モデル異常判断手段１１１において、内部モデルにおける異常の有無を判断する。この異常判断は、外部モデルの各サンプル時における内部モデルとの差分の自己相関を算出し、これまでの自己相関との信号の差異を時系列信号と考え、独立成分分析などを用い、独立性がある場合にはこれまでの信号と異なると言えるため、異常が存在すると判断する。
【００７６】
具体的な処理フローを図６に示す。
【００７７】
まず、ステップ６０１にて、一時記憶領域１１５から外部モデルからの出力値および内部モデルからの出力値を取得する。次にステップ６０２にて、内部モデルによる位置の1サンプル毎の移動量を積算した移動量を計算する。この計算は、前回に外部モデルからの値を取得したサンプルからの積算であり、以下の（式１２）のように表わされる。
【００７８】
【数２２】
2019082328.tif 000024
【００７９】
ここで、dxはサンプル時における移動量であり、（式１２）の左辺は前回に外部モデルからの値を取得したサンプルからのt回分の積算である。計算した回転量および移動量にて、誤差系列データテーブル１１２を更新する。
【００８０】
次にステップ６０３に移行し、外部モデルの取得フラグが１かをチェックする。外部モデルの出力値が使用可能、つまり、外部モデル取得フラグが１の場合は、ステップ６０４へ移行する。外部モデルの出力値が使用不可の場合、つまり、外部モデル取得フラグが０の場合は、ステップ６０８へ移行する。
【００８１】
ステップ６０４では、時間順方向と逆方向それぞれに対して、誤差系列を計算する。誤差系列とは、外部モデルの出力する値と、内部モデルによって積算した移動量による値の差分のことである。内部モデルによって積算した移動量の始点を時間的に現在の時間に近い方を採った場合、時間逆方向であり、遠い方を採った場合、時間順方向と定義する。図７、図８に時間順方向および時間逆方向の誤差系列の例を示す。
【００８２】
時間順方向の誤差系列は、始点となる外部モデルの出力値７０１から内部モデル出力値の積算によって得られたベクトル７０２を足し合わせた位置と、次の外部モデルの出力位置７０３との差分７０４によって求めた値の系列である。この時間順方向の誤差系列は以下の（式１３）のように計算できる。
【００８３】
【数２３】
2019082328.tif 000025
【００８４】
ここで、Rは始点における回転を表わす行列であり、姿勢計測手段１０２もしくは内部モデル演算手段１０８にて、車両の姿勢が取得できる場合は、その値を用いる。
【００８５】
また、同様に時間逆方向の誤差系列は、始点となる外部モデルの出力値８０１から内部モデル出力値を時間に対して逆に積算することによって得られたベクトル８０２を引いた位置と、一つ前の外部モデルの出力位置８０３との差分８０４によって求めた値の系列である。この時間逆方向の誤差系列は以下の（式１４）のように計算できる。
【００８６】
【数２４】
2019082328.tif 000026
【００８７】
この計算によって、得られた誤差系列を誤差系列データテーブルへ設定する。誤差系列データテーブルの例を図９に示す。誤差系列データは最大ｍ個のデータを時間順に格納できるリングバッファ式のテーブルである。図９では説明のために全ての時刻サンプルを表記しているが、実際に格納されているデータは、外部モデル取得フラグが１の時のみである。
【００８８】
次にステップ６０５にて、時間順方向、逆方向それぞれにおいて作成したｍ個の誤差系列から、自己共分散行列を計算し、固有ベクトルを求め、時間順方向、時間逆方向の固有ベクトルが成す角を計算する。
【００８９】
誤差系列の自己相関散行列は以下の（式１５）のようにして求める。
【００９０】
【数２５】
2019082328.tif 000027
【００９１】
上式で求めた時間順方向、逆方向それぞれの自己相関行列の固有ベクトルがbf、brと表わされるとすると、二つの自己相関行列の固有ベクトルが成す角は、以下の（式１６）のように計算できる。
【００９２】
【数２６】
2019082328.tif 000028
【００９３】
次にステップ６０６において、時間順方向、逆方向の誤差系列データが同一のものであるかを判断する。モデル化誤差が存在する場合には、経時的に誤差が姿勢・速度に蓄積するため、時間順方向、逆方向の誤差系列データに差異が生じる。そこで、ステップ６０５にて計算した二つの自己相関行列の固有ベクトルが成す角（式１６）の左辺が閾値よりも大きいかを比較する。固有ベクトルの成す角（式１６）の左辺が閾値よりも大きい場合は、時間順方向、逆方向の誤差系列データに差異が生じていると考えることができ、内部モデルに異常があるとしてステップ６０７に移行する。また、固有ベクトルの成す角（式１６）の左辺が閾値よりも小さい場合は、時間順方向、逆方向の誤差系列データに差異がないと考えることができ、内部モデルに異常はないので、ステップ６０８に移行する。
【００９４】
ステップ６０７では、内部モデルにおける異常の有無を示すモデル異常フラグを１に設定し、出力して、処理を終了する。
【００９５】
ステップ６０８では、モデル異常フラグを０に設定し、出力して処理を終了する。
【００９６】
ステップ２０６においてモデル異常フラグを設定し終えれば、次にステップ２０７において、モデル異常フラグによって処理を変更する。モデル異常フラグが１に設定されていれば、内部モデル誤差を算出するためにステップ２０８へ移行する。モデル異常フラグが０に設定されていれば、システム異常を報知するかどうかを判断するためにステップ２０９へ移行する。
【００９７】
ステップ２０８では、内部モデル誤差算出手段１１３において、内部モデルのモデル化誤差を算出する。図１０に内部モデルのモデル化誤差を算出する処理フローを示す。
【００９８】
まず、ステップ１００１において、一時記憶領域１１５から、計算に必要な情報を取得する。計算に必要な情報は、外部モデル取得フラグと、内部モデル演算手段１０８にて計算される推定位置、現在の車両状態、速度、加速度、姿勢、角速度、確率フィルタ計算手段１１０において計算されるイノベーション、および内部モデル誤差算出段で更新をかける全ての車両状態である。
【００９９】
次にステップ１００２にて、外部モデル取得フラグをチェックする。外部モデル取得フラグが１の場合は、ステップ１００３へ進む。外部モデル取得フラグが０の場合は、モデル化誤差の算出ができないため、ステップ１００７へ進み、モデル化誤差算出不能フラグを1とする。モデル化誤差算出不能フラグは初期値として０が設定されており、毎起動ごとに初期化されているものとする。
【０１００】
ステップ１００３にて、ステップ１００１で取得した現在の車両状態における、モデル化誤差の平均値を更新する。モデル化誤差の平均値は、確率フィルタ計算手段１１０で計算したイノベーションvの平均値と等しいので、ステップ１００１で取得した現在の車両状態の平均値E[v]から、以下の（式１７）のように計算すればよい。
【０１０１】
【数２７】
2019082328.tif 000029
【０１０２】
ただし、Nは各車両状態に保存されているサンプル数である。
【０１０３】
次にステップ１００４にて、ステップ１００１で取得した現在の車両状態におけるモデル化誤差の分散値E[v2]を更新する。確率フィルタ計算手段１１０で計算したイノベーションの値から、以下の（式１８）のように逐次求めればよい。
【０１０４】
【数２８】
2019082328.tif 000030
【０１０５】
次にステップ１００５では、ステップ１００３およびステップ１００４にて計算したモデル化誤差の平均値および分散値を一時記憶領域１１５へ上書き更新する。
【０１０６】
最後に、ステップ１００６にて、車両状態の遷移確率（式１９）を更新する。ある時刻における車両状態（式１９０）から次のサンプル時における車両状態（式１９１）への各車両状態への遷移確率（式１９）を以下の（式１９）のように計算する。
【０１０７】
【数２９】
2019082328.tif 000031
【０１０８】
【数３０】
2019082328.tif 000032
【０１０９】
【数３１】
2019082328.tif 000033
【０１１０】
ただし、遷移確率（式１９）は以下の（式２０）のような行列で表わされる。
【０１１１】
【数３２】
2019082328.tif 000034
【０１１２】
車両状態の遷移確率は、上式をそのまま用いた場合は、値が安定しない場合も多いので、確率勾配法などを用いて、徐々に収束させるような手法をとってもよい。
【０１１３】
このとき、ある状態からある状態までの遷移確率が両方向とも、閾値よりも高い場合は、車両状態に安定性が無いため、一つの車両状態へと融合する。このとき、各車両状態に設定されたモデル化誤差平均値とモデル化誤差分散値をそれぞれのサンプル数を用いて、更新する。
【０１１４】
ステップ１００６もしくはステップ１００７が終了すれば、内部モデル誤差算出手段１１３の処理を終了する。
【０１１５】
次に、ステップ２０９にて、モデル化誤差算出不能フラグをチェックする。モデル化誤差算出不能フラグが0であった場合は、モデル化誤差の設定が完了しているので、処理を終了する。モデル化誤差算出不能フラグが１の場合は、システムの異常を判断するため、システム異常報知手段１１４を起動する。
【０１１６】
システム異常報知手段１１４が起動せず、内部モデル誤差算出手段１１３の処理が終了した場合は、位置推定装置１０６の処理を終了し、次の起動時における処理に備える。
【０１１７】
一方で、位置推定装置１０６の処理フロー中のステップ２０７において、モデル異常が認められない場合、つまり、モデル異常フラグが「０」の場合、もしくは、モデル化誤差の算出ができなかった場合は、ステップ２１０に移行する。
【０１１８】
ステップ２１０では、システム異常報知手段１１４において、ある一定の時間内にシステムに悪影響が出るような推定位置誤差が生じる可能性を判断し、システムに悪影響が出る場合は、その旨をシステム状態として出力する。
【０１１９】
図１１にシステム異常報知手段１１４の処理フローを示す。
【０１２０】
まず、ステップ１１０１にて、一時記憶領域１１５から現在の車両状態、内部モデルが算出した誤差分散値、現在の車両状ｘ態の確率から最も高い車両状態に対応するモデル化誤差分散値を取得する。
【０１２１】
次にステップ１１０２において、ステップ１１０１にて取得した、内部モデルの誤差分散値にモデル化誤差分散値を足し合わせた分散値から計算できる見積もり誤差dが閾値を超えるまでの最小時間を計算する。見積もり誤差は以下の（式２１）のようにして計算することができる。
【０１２２】
【数３３】
2019082328.tif 000035
【０１２３】
上式の右辺は楕円形の方程式を表わしており、その長軸が時間単位に伸びる量が誤差分散値Σにより、決定する。よって、見積もり誤差の閾値を時間単位に長軸が伸びる量で割れば見積もり誤差が閾値を超えるまでの最小時間を求めることができる。
【０１２４】
また、この閾値までの時間は、設計時に決定し、固定値を与える場合は、このステップは不要である。
【０１２５】
次にステップ１１０３にて、見積もり誤差が閾値を超えるまでの時間が、閾値以下であるかを判断する。閾値以下の場合は、システム異常とはいえないため、報知する情報は無く、システム異常報知手段１１４の処理を終了する。
【０１２６】
見積もり誤差が閾値を超えるまでの時間が、閾値より大きい場合は、ステップ１１０４に移行し、モデル化誤差算出不能フラグが「１」であるかを判断する。これは、内部モデル誤差算出手段１１３の処理ステップ１００２にて、外部モデルを取得しておらず、内部モデルに異常があるにも関わらず、その誤差が算出できなかった場合に「１」となる。
【０１２７】
モデル化誤差が算出不能であった場合は、ステップ１１０５に移行する。この場合は、位置推定装置１０６のシステム自体が異常値を出力する可能性があるため、位置推定装置１０６のシステム異常として、位置推定装置１０６の外部へシステム状態を出力する。
【０１２８】
モデル化誤差が算出可能であった場合は、位置推定装置１０６は正常な動作をしているため、外部のシステム（位置計測手段１０１）の異常である可能性がある。そのため、位置推定装置１０６以外のシステム異常として、外部へシステム状態を出力する。
【０１２９】
外部へのシステム状態の出力が終了すれば、システム異常報知手段１１４の処理を終了する。システム異常報知手段１１４の処理が終了した場合は、位置推定装置１０６の処理を終了し、次の起動時における処理に備える。
【０１３０】
以上の一連の動作により、システム異常報知手段１１４から位置推定装置１０６の異常状態が出力できるようになり、ユーザーは位置推定装置１０６の異常か、外部システムの異常かを判断できるようになる。
【０１３１】
モデル異常判断手段１１１において用いる、異常値の判断手法は様々であり、実施例１で用いたのとは別の手法を用いることも可能である。例えば、この独立性判断の他に、内部モデルと外部モデルとの差分がある閾値以上であった場合に異常と判断するような処理でも、異常値を判断することが可能である。ただし、閾値の設定には十分な経験による設定が必要となる。
【符号の説明】
【０１３２】
１０１  位置計測手段、１０２  姿勢計測手段、１０３  速度計測手段、１０４加速度計測手段、１０５角速度計測手段、１０６  位置推定装置、１０７  外部モデル演算手段、１０８  内部モデル演算手段、１０９  観測異常判断手段、１１０  確率フィルタ計算手段、１１１  モデル異常判断手段、１１２誤差系列データテーブル、１１３  内部モデル誤差算出手段、１１４  システム異常報知手段、１１５一時記憶領域  
【図１】
2019082328.tif 000036
【図２】
2019082328.tif 000037
【図３】
2019082328.tif 000038
【図４】
2019082328.tif 000039
【図５】
2019082328.tif 000040
【図６】
2019082328.tif 000041
【図７】
2019082328.tif 000042
【図８】
2019082328.tif 000043
【図９】
2019082328.tif 000044
【図１０】
2019082328.tif 000045
【図１１】
2019082328.tif 000046