センサ誤差推定装置及び観測装置及び方法及びプログラム
【課題】計測誤差を推定し、推定した計測誤差で計測量を補正する観測装置において、誤差推定の進捗度を正しく見積もる。
【解決手段】誤差推定部１３０は、距離及び距離変化率観測部１１２が観測した擬似距離などに基づいて、車速パルス発生器１２１・ジャイロスコープ１２２などが計測した速度・角速度などの計測誤差を推定し、推定した計測誤差の推定誤差を算出する。進捗度算出部１５１は、誤差推定部１３０が算出した推定誤差に基づいて、誤差推定の進捗度を算出する。
【解決手段】誤差推定部１３０は、距離及び距離変化率観測部１１２が観測した擬似距離などに基づいて、車速パルス発生器１２１・ジャイロスコープ１２２などが計測した速度・角速度などの計測誤差を推定し、推定した計測誤差の推定誤差を算出する。進捗度算出部１５１は、誤差推定部１３０が算出した推定誤差に基づいて、誤差推定の進捗度を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、センサの計測誤差を推定する装置、及び推定した計測誤差による補正をして観測する装置、及びその装置が用いる方法、及びコンピュータを用いてその装置を実現するプログラムに関する。
【背景技術】
【０００２】
計測に用いるセンサなどは誤差を有しているので、正確な計測を行うには、センサなどが有する計測誤差を推定し、補正する必要がある。
例えば、測位装置においては、速度・角速度などを計測し、デッドレコニング（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ：自律航法測位）計算により、位置を算出する方法と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：全地球測位システム）衛星からのＧＰＳ信号に基づいて、ＧＰＳ衛星までの擬似距離を観測し、位置を算出する方法とがある。また、カルマンフィルタなどを用いて２つの方法を結合することにより、センサなどが有する計測誤差を推定する技術がある。
【特許文献１】特開２００６−１８９３２０号公報
【特許文献２】特開２００５−２４１５１７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
計測誤差の推定結果がどれほど信頼できるかにより、観測結果に対する信頼性も変わってくる。特に、カルマンフィルタを用いた方法では、観測開始直後は推定結果の信頼性が低く、徐々に信頼性が高くなる。しかし、観測開始直後の信頼性が低い期間（センサの初期化期間）がどの程度あるかは、必ずしも明らかではない。
この発明は、例えば、上記のような課題を解決するためになされたものであり、誤差推定がどこまで進んでいるかを計算により見積もることを目的とする。
【課題を解決するための手段】
【０００４】
この発明にかかるセンサ誤差推定装置は、
情報を処理する処理装置と、
物理量を計測するセンサと、
上記処理装置を用いて、観測量を示す情報を観測情報として入力し、上記処理装置を用いて、入力した観測情報に基づいて、上記センサの計測誤差を推定し、上記処理装置を用いて、推定した計測誤差の推定誤差を算出し、上記処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力する誤差推定部と、
上記処理装置を用いて、上記誤差推定部が出力した推定誤差情報を入力し、上記処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出し、上記処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力する進捗度算出部と、
を有することを特徴とする。
【発明の効果】
【０００５】
この発明にかかるセンサ誤差推定装置によれば、誤差推定部が算出した推定誤差に基づいて、進捗度算出部が誤差推定の進捗度を算出するので、誤差推定の進捗度を計算により見積もることができるという効果を奏する。
【発明を実施するための最良の形態】
【０００６】
実施の形態１．
実施の形態１を、図１〜図１０を用いて説明する。
【０００７】
図１は、この実施の形態における測位システム８００の全体構成の一例を示すシステム構成図である。
測位装置１００は、例えば車両等に搭載し、車両等の現在位置を算出する。測位装置１００は、観測装置及びセンサ誤差推定装置の一例である。
測位装置１００は、ＧＰＳ衛星２００が発信したＧＰＳ信号を受信する。ＧＰＳ衛星２００は、発信装置の一例である。ＧＰＳ信号は、発信装置が発信した電波の一例である。
測位装置１００は、また、搭載された車両等の速度・加速度・角速度などを計測するセンサを有している。
測位装置１００は、ＧＰＳ／センサ複合測位を行う。
すなわち、測位装置１００は、センサが計測した速度・加速度・角速度などに基づいて、デッドレコニングにより車両等の位置を算出する。
また、測位装置１００は、受信したＧＰＳ信号に基づいて、センサ等の誤差を推定し、センサが計測した速度などを補正することにより、正確な位置を算出する。
【０００８】
図２は、この実施の形態における測位装置１００の外観の一例を示す図である。
測位装置１００は、システムユニット９１０、ＣＲＴ（Ｃａｔｈｏｄｅ・Ｒａｙ・Ｔｕｂｅ）やＬＣＤ（液晶）の表示画面を有する表示装置９０１、キーボード９０２（Ｋｅｙ・Ｂｏａｒｄ：Ｋ／Ｂ）、マウス９０３、ＦＤＤ９０４（Ｆｌｅｘｉｂｌｅ・Ｄｉｓｋ・Ｄｒｉｖｅ）、コンパクトディスク装置９０５（ＣＤＤ）、プリンタ装置９０６、スキャナ装置９０７などのハードウェア資源を備え、これらはケーブルや信号線で接続されている。
システムユニット９１０は、コンピュータであり、ファクシミリ機９３２、電話器９３１とケーブルで接続され、また、ローカルエリアネットワーク９４２（ＬＡＮ）、ゲートウェイ９４１を介してインターネット９４０に接続されている。
【０００９】
図３は、この実施の形態における測位装置１００のハードウェア資源の一例を示す図である。
図３において、測位装置１００は、プログラムを実行するＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信装置９１５、表示装置９０１、キーボード９０２、マウス９０３、ＦＤＤ９０４、ＣＤＤ９０５、プリンタ装置９０６、スキャナ装置９０７、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。磁気ディスク装置９２０の代わりに、光ディスク装置、メモリカード読み書き装置などの記憶装置でもよい。
ＲＡＭ９１４は、揮発性メモリの一例である。ＲＯＭ９１３、ＦＤＤ９０４、ＣＤＤ９０５、磁気ディスク装置９２０の記憶媒体は、不揮発性メモリの一例である。これらは、記憶装置あるいは記憶部の一例である。
通信装置９１５、キーボード９０２、スキャナ装置９０７、ＦＤＤ９０４などは、入力部、入力装置の一例である。
また、通信装置９１５、表示装置９０１、プリンタ装置９０６などは、出力部、出力装置の一例である。
【００１０】
通信装置９１５は、ファクシミリ機９３２、電話器９３１、ＬＡＮ９４２等に接続されている。通信装置９１５は、ＬＡＮ９４２に限らず、インターネット９４０、ＩＳＤＮ等のＷＡＮ（ワイドエリアネットワーク）などに接続されていても構わない。インターネット９４０或いはＩＳＤＮ等のＷＡＮに接続されている場合、ゲートウェイ９４１は不用となる。
磁気ディスク装置９２０には、オペレーティングシステム９２１（ＯＳ）、ウィンドウシステム９２２、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３のプログラムは、ＣＰＵ９１１、オペレーティングシステム９２１、ウィンドウシステム９２２により実行される。
【００１１】
上記プログラム群９２３には、以下に述べる実施の形態の説明において「〜部」、「〜手段」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
ファイル群９２４には、以下に述べる実施の形態の説明において、「〜の判定結果」、「〜の計算結果」、「〜の処理結果」として説明する情報やデータや信号値や変数値やパラメータが、「〜ファイル」や「〜データベース」の各項目として記憶されている。「〜ファイル」や「〜データベース」は、ディスクやメモリなどの記録媒体に記憶される。ディスクやメモリになどの記憶媒体に記憶された情報やデータや信号値や変数値やパラメータは、読み書き回路を介してＣＰＵ９１１によりメインメモリやキャッシュメモリに読み出され、抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示などのＣＰＵの動作に用いられる。抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示のＣＰＵの動作の間、情報やデータや信号値や変数値やパラメータは、メインメモリやキャッシュメモリやバッファメモリに一時的に記憶される。
また、以下に述べる実施の形態の説明において説明するフローチャートの矢印の部分は主としてデータや信号の入出力を示し、データや信号値は、ＲＡＭ９１４のメモリ、ＦＤＤ９０４のフレキシブルディスク、ＣＤＤ９０５のコンパクトディスク、磁気ディスク装置９２０の磁気ディスク、その他光ディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）等の記録媒体に記録される。また、データや信号は、バス９１２や信号線やケーブルその他の伝送媒体によりオンライン伝送される。
【００１２】
また、以下に述べる実施の形態の説明において「〜部」、「〜手段」として説明するものは、「〜回路」、「〜装置」、「〜機器」であってもよく、また、「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」、「〜手段」として説明するものは、ＲＯＭ９１３に記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、素子・デバイス・基板・配線などのハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。ファームウェアとソフトウェアは、プログラムとして、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等の記録媒体に記憶される。プログラムはＣＰＵ９１１により読み出され、ＣＰＵ９１１により実行される。すなわち、プログラムは、以下に述べる「〜部」、「〜手段」としてコンピュータを機能させるものである。あるいは、以下に述べる「〜部」、「〜手段」の手順や方法をコンピュータに実行させるものである。
【００１３】
図４は、この実施の形態における測位装置１００の機能ブロックの一例を示すブロック構成図である。
測位装置１００は、ＧＰＳ受信機１１１、距離及び距離変化率観測部１１２、車速パルス発生器１２１、ジャイロスコープ１２２、誤差推定部１３０、センサ補正部１４１、位置算出部１４２、進捗度算出部１５１、信頼性判定部１５２を有する。
【００１４】
ＧＰＳ受信機１１１は、ＧＰＳ衛星２００が発信したＧＰＳ信号を受信する。ＧＰＳ受信機１１１は、受信装置の一例である。
ＧＰＳ受信機１１１は、受信できるＧＰＳ信号が複数ある場合には、複数のＧＰＳ衛星２００がそれぞれ発信した複数のＧＰＳ信号を、それぞれ受信する。
【００１５】
距離及び距離変化率観測部１１２は、ＧＰＳ受信機１１１が受信したＧＰＳ信号に基づいて、ＧＰＳ衛星までの距離（以下「擬似距離」という。）と、擬似距離の変化率とを算出する。
距離及び距離変化率観測部１１２は、ＧＰＳ受信機１１１が受信したＧＰＳ信号が複数ある場合には、ＧＰＳ受信機１１１が受信した複数のＧＰＳ信号それぞれについて、それぞれのＧＰＳ衛星２００までの擬似距離及び擬似距離の変化率を算出する。ただし、信頼できる擬似距離及び擬似距離の変化率を算出できないＧＰＳ信号については棄却し、擬似距離や擬似距離の変化率を算出しない。
信頼できる擬似距離や擬似距離の変化率が算出できない場合としては、例えば、受信したＧＰＳ信号がマルチパスである場合や、ＧＰＳ衛星２００の仰角が低い場合などがある。
距離及び距離変化率観測部１１２は、これらの条件を判定して、擬似距離や擬似距離の変化率を算出するか否かを決定する。あるいは、距離及び距離変化率観測部１１２は、擬似距離や擬似距離の変化率を算出したのち、算出した擬似距離や擬似距離の変化率が信頼できるものであるか否かを判断して、棄却してもよい。
距離及び距離変化率観測部１１２は、算出した擬似距離や擬似距離の変化率を示す情報を距離及び距離変化率情報として出力する。
【００１６】
車速パルス発生器１２１は、測位装置１００を搭載した車両等の速度を計測する。車速パルス発生器１２１は、速度センサの一例である。
車速パルス発生器１２１は、車速パルスを出力する。車速パルスとは、計測した速度に比例する周波数を有するパルス信号である。車速パルスは、速度情報の一例である。
【００１７】
ジャイロスコープ１２２は、測位装置１００を相対した車両等の角速度を計測する。ジャイロスコープ１２２は、角速度センサの一例である。
ジャイロスコープ１２２は、計測した角速度に比例する電圧（あるいは、その電圧をアナログデジタル変換した数値を示す情報）を出力する。ジャイロスコープ１２２が出力する電圧（あるいは数値）は、角速度情報の一例である。
【００１８】
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報を入力する。
距離及び距離変化率情報は、観測情報の一例である。
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、車速パルス発生器１２１が出力した車速パルス（あるいは、車速パルスをＣＰＵ９１１が処理可能な情報に変換した情報）を入力する。
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、ジャイロスコープ１２２が出力した電圧（あるいは、電圧をＣＰＵ９１１が処理可能な情報に変換した情報）を入力する。
車速パルスなどの速度情報・角速度情報は、計測情報の一例である。
【００１９】
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、入力した観測情報と計測情報とに基づいて、センサの計測誤差を推定する。
計測誤差とは、センサの計測に伴う誤差である。測位装置１００は、誤差推定部１３０が推定した計測誤差を補正することにより、正確な位置を算出する。
【００２０】
例えば、車速パルス発生器１２１の場合、車速パルス発生器１２１が出力した車速パルスの周波数に、あらかじめ定められた定数（スケールファクタ）を乗じることにより、車速パルス発生器１２１が計測した車両等の速度を知ることができる。しかし、車速パルス発生器１２１のスケールファクタは、周囲の温度や製造時の精度などにより、公称値と異なる値となる場合があるので、正確なスケールファクタを知らなければ、車両等の正確な速度を求めることができない。このようなスケールファクタのずれを、スケールファクタ誤差という。スケールファクタ誤差は、計測誤差の一例である。
【００２１】
ジャイロスコープ１２２の場合も同様に、ジャイロスコープ１２２が出力した電圧に、あらかじめ定められた定数（スケールファクタ）を乗じることにより、ジャイロスコープ１２２が計測した角速度を知ることができる。したがって、ジャイロスコープ１２２にもスケールファクタ誤差が存在する。
また、ジャイロスコープ１２２は、回転していない状態でも一定の電圧を出力する場合があるので、これも誤差の要因となる。このようなずれをバイアス誤差という。バイアス誤差も、計測誤差の一例である。
【００２２】
また、距離及び距離変化率観測部１１２では、通常、ＧＰＳ衛星２００までの距離及び距離変化率を算出するため、内部に時計を有している。この時計のクロック周波数がずれると、ＧＰＳ衛星２００までの距離や擬似距離の変化率を正確に算出できない。クロックの誤差には、バイアス誤差、ドリフト誤差などがある。バイアス誤差、ドリフト誤差も、計測誤差の一例である。
【００２３】
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差を示す情報を、計測誤差情報として出力する。
【００２４】
また、誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差の推定誤差を算出する。
推定誤差とは、推定した計測誤差と真の計測誤差との間の誤差がどのくらいあるかを示すものである。推定した計測誤差は、真の計測誤差を中心として正規分布すると考えられるので、推定誤差は、その分散（あるいは、２つ以上の推定誤差の間に相関がある場合は共分散）で表わす。
誤差推定部１３０は、算出した推定誤差を示す情報を、推定誤差情報として出力する。
【００２５】
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、車速パルス発生器１２１が出力した車速パルスなどの速度情報を入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、ジャイロスコープ１２２が出力した角速度情報を入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、入力した速度情報が示す速度を、入力した推定誤差情報が示す推定誤差で補正する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、入力した角速度情報が示す角速度を、入力した推定誤差情報が示す推定誤差で補正する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、補正した物理量を示す情報を、補正計測情報として出力する。
【００２６】
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、センサ補正部１４１が出力した補正計測情報を入力する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、入力した補正計測情報に基づいて、現在位置を算出する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、算出した現在位置を示す情報を、位置情報として出力する。
位置算出部１４２が出力した位置情報は、例えば、表示装置９０１などに表示して利用者等に知らせる。
【００２７】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出する。
【００２８】
測位装置１００が動作を開始した直後は、誤差推定部１３０は計測誤差を正しく推定できない。距離及び距離変化率情報などの観測情報や、速度情報・角速度情報などの計測情報が集まるにつれて、計測誤差の推定の精度が向上していく。
誤差推定の進捗度は、計測誤差の推定の精度がどのくらいであるかを示すものである。
【００２９】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、算出した進捗度を示す情報を、進捗度情報として出力する。
【００３０】
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力する。
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報に基づいて、誤差推定部１３０の初期化が終わったか否か、すなわち、誤差推定部１３０が推定した計測誤差が信頼できるか否かを判定する。信頼性判定部１５２は、初期化終了判定部の一例である。
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、判定した判定結果を示す情報を、判定情報として出力する。
【００３１】
信頼性判定部１５２が出力した判定情報は、例えば、表示装置９０１などに表示して、利用者等に知らせる。
あるいは、位置算出部１４２が算出した位置情報をマップマッチング（Ｍａｐ Ｍａｔｃｈｉｎｇ：地図照合）する際に利用してもよい。
【００３２】
デッドレコニングによる位置算出では、車両等の移動ベクトルを積分して、現在位置を求める。移動ベクトルは、移動速度と移動方向とから求める。移動速度は、速度センサが計測した値である。移動方向は、角速度センサが計測した角速度を積分することにより求める。
したがって、速度センサや角速度センサに計測誤差があると、たとえ微小な誤差でも積分によって誤差が蓄積し、正しい位置算出ができない。
そのため、ＧＰＳ信号による補正が必要となる。
【００３３】
ＧＰＳ測位は、絶対精度が高いが、受信状態が悪いと測位できない場合があり、連続性が劣っている。そこで、ＧＰＳ測位とデッドレコニング測位とを複合計算することで、互いの長所を生かした高精度な測位を実現する。
【００３４】
一方、センサの計測誤差が正しく推定された後であれば、ＧＰＳ信号が受信できなくても、ある程度正確な位置算出ができる。これにより、高層ビル街のようなＧＰＳ信号の受信状態が悪い状況や、地下のようなＧＰＳ信号が受信できない状況であっても、正確な位置算出ができる。
したがって、ＧＰＳ信号が受信できない状況などになったとき、センサの計測誤差が正しく推定されているか否かにより、位置算出部１４２が算出する現在位置の信頼性が大きく変わることになる。
【００３５】
このため、計測誤差が正しく推定されているか否かを正確に判定することが重要である。
【００３６】
次に、誤差推定部１３０について、詳しく説明する。
【００３７】
図５は、この実施の形態における誤差推定部１３０が計測誤差を推定するために用いるカルマンフィルタの動作を示す図である。
【００３８】
カルマンフィルタは、離散的に得られる観測値を元に、知りたい状態量を推定するものである。
カルマンフィルタには、大きく分けて、時間外挿過程と観測更新過程との２つの工程がある。
時間外挿過程は、１つ前の状態から、現在の状態を予測する工程である。
観測更新過程は、現在の状態について得られた観測値を元に、真の状態を推定する工程である。
カルマンフィルタは、観測値が得られるたびにこの２つの工程を繰り返し、徐々に推定の精度を上げていく。
【００３９】
時間外挿過程には、状態方程式外挿処理と、誤差共分散外挿処理とがある。
【００４０】
状態方程式外挿処理とは、１つ前の状態量ベクトル（推定）ｘｋ＋から、現在の状態量ベクトル（予測）ｘｋ＋１−を予測する処理である。
状態量の予測には、状態遷移行列Φを用いる。
状態遷移行列Φは、例えば、力学モデルから導き出したものである。
【００４１】
誤差共分散外挿処理とは、１つ前の誤差共分散行列（推定）Ｐｋ＋から、現在の誤差共分散行列（予測）Ｐｋ＋１−を予測する処理である。
誤差共分散の予測には、状態遷移行列Φと、プロセスノイズ行列Ｑとを用いる。
状態遷移行列Φは、状態方程式外挿処理で用いたものと同じものである。
プロセスノイズ行列Ｑは、モデルの時間発展に伴う誤差を示すものである。
【００４２】
観測更新過程には、カルマンゲイン算出処理と、状態方程式更新処理と、誤差共分散更新処理とがある。
【００４３】
カルマンゲイン算出処理とは、状態量ベクトル（予測）ｘｋ＋１−を、観測値ベクトルｚｋ＋１で補正して、状態量ベクトル（推定）ｘｋ＋１＋を得るための行列であるカルマンゲインＫを算出する処理である。
カルマンゲインの算出には、誤差共分散行列（予測）Ｐｋ＋１−と、観測行列Ｈと、観測ノイズ行列Ｒとを用いる。
誤差共分散行列（予測）は、誤差共分散外挿処理で予測したものである。
観測行列Ｈは、状態量ベクトルｘを観測値ベクトルｚに変換する行列である。すなわち、状態量ベクトルが示す状態を観測した結果として、観測値ベクトルが示す観測値が得られるという関係を数式化したものである。
観測ノイズ行列Ｒは、観測に伴う誤差を示すものである。観測ノイズ行列Ｒが示す観測に伴う誤差は、スケールファクタ誤差などの組織的な観測誤差とは異なり、観測に伴ってランダムに混入する誤差（ノイズ）のことである。
【００４４】
状態方程式更新処理とは、状態量ベクトル（予測）ｘｋ＋１−を、観測値ベクトルｚｋ＋１で補正し、状態量ベクトル（推定）ｘｋ＋１＋を推定する処理である。
状態量ベクトルの推定には、カルマンゲインＫと、観測行列Ｈとを用いる。
カルマンゲインＫは、カルマンゲイン算出処理で算出したものである。
観測行列Ｈは、カルマンゲイン算出処理で用いたものと同じものである。
【００４５】
誤差共分散更新処理とは、誤差共分散行列（予測）Ｐｋ＋１−を補正して、誤差共分散行列（推定）Ｐｋ＋１＋を推定する処理である。
誤差共分散行列の推定には、カルマンゲインＫと、観測行列Ｈとを用いる。
カルマンゲインＫは、カルマンゲイン算出処理で算出したものである。
観測行列Ｈは、カルマンゲイン算出処理で用いたものと同じものである。
【００４６】
カルマンフィルタは、以上の処理を繰り返すことにより、状態を推定する。
【００４７】
図６は、この実施の形態における誤差推定部１３０の内部ブロックの構成を示す詳細ブロック図である。
誤差推定部１３０は、遷移行列生成部１３１、観測値生成部１３２、推定誤差記憶部１３３、推定誤差予測部１３４、カルマンゲイン算出部１３５、推定誤差算出部１３６、計測誤差記憶部１３７、計測誤差予測部１３８、計測誤差推定部１３９を有する。
【００４８】
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、時刻ｋ＋１における速度情報と角速度情報とを入力する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、入力した速度情報と角速度情報に基づいて、時刻ｋから時刻ｋ＋１への状態遷移行列Φｋ，ｋ＋１を生成する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、生成した状態遷移行列Φｋ，ｋ＋１を示す情報を出力する。
【００４９】
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、時刻ｋ＋１における距離及び距離変化率情報を入力する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報に基づいて、時刻ｋ＋１における観測値ベクトルｚｋ＋１を生成する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、生成した観測値ベクトルｚｋ＋１を示す情報を出力する。
【００５０】
なお、距離及び距離変化率情報の数が少ない場合には、観測更新過程を処理することができない。そのため、観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報の数を、所定の数（例えば２）と比較する。観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報の数が所定の数以上である場合のみ、観測値ベクトルｚｋ＋１を生成する。
観測値ベクトルｚｋ＋１を生成しない場合、観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、観測値ベクトルｚｋ＋１を生成しないことを示す情報を出力する。
【００５１】
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、時刻ｋにおける誤差共分散行列（推定）Ｐｋ＋を示す情報を、推定誤差情報として記憶している。
【００５２】
推定誤差予測部１３４は、上述した誤差共分散外挿処理を行う。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、推定誤差記憶部１３３が記憶した推定誤差情報を入力する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報を入力する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、誤差共分散行列（推定）Ｐｋ＋、状態遷移行列Φｋ，ｋ＋１から、時刻ｋ＋１における誤差共分散行列（予測）Ｐｋ＋１−を算出する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を出力する。
【００５３】
カルマンゲイン算出部１３５は、上述したカルマンゲイン算出処理を行う。
ただし、観測値生成部１３２が観測値ベクトルｚｋ＋１を生成しなかった場合、カルマンゲイン算出部１３５は、カルマンゲイン算出処理をしない。
観測値生成部１３２が出力した情報に基づいて、観測値生成部１３２が観測値ベクトルｚｋ＋１を生成したと判断した場合、カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、誤差共分散行列（予測）Ｐｋ＋１−から、カルマンゲインＫを算出する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、算出したカルマンゲインＫを示す情報を出力する。
【００５４】
推定誤差算出部１３６は、上述した誤差共分散更新処理を行う。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
【００５５】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成した場合、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、カルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報を入力する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、誤差共分散行列（予測）Ｐｋ＋１−、カルマンゲインＫから、時刻ｋ＋１における誤差共分散行列（推定）Ｐｋ＋１＋を算出する。
【００５６】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成しなかった場合、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した誤差共分散行列（予測）Ｐｋ＋１−を、そのまま誤差共分散行列（推定）Ｐｋ＋１＋とする。
【００５７】
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（推定）Ｐｋ＋１＋を示す情報を出力する。
【００５８】
推定誤差記憶部１３３は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した誤差共分散行列（推定）Ｐｋ＋１＋を示す情報を入力する。
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、入力した情報を推定誤差情報として記憶する。
【００５９】
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、時刻ｋにおける状態量ベクトル（推定）ｘｋ＋を示す情報を、計測誤差情報として記憶している。
【００６０】
計測誤差予測部１３８は、上述した状態方程式外挿処理を行う。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、計測誤差記憶部１３７が記憶した計測誤差情報を入力する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報を入力する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、状態量ベクトル（推定）ｘｋ＋、状態遷移行列Φｋ，ｋ＋１から、時刻ｋ＋１における状態量ベクトル（予測）ｘｋ＋１−を算出する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（予測）ｘｋ＋１−を示す情報を出力する。
【００６１】
計測誤差推定部１３９は、上述した状態方程式更新処理を行う。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、計測誤差予測部１３８が出力した状態量ベクトルｘｋ＋１−を示す情報を入力する。
【００６２】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成した場合、計測誤差推定部１３９は、観測値生成部１３２が出力した観測値ベクトルｚｋ＋１を示す情報を入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、カルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報を入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、状態量ベクトルｘｋ＋１−、観測値ベクトルｚｋ＋１、カルマンゲインＫから、時刻ｋ＋１における状態量ベクトル（推定）ｘｋ＋１＋を算出する。
【００６３】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成しなかった場合、計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した状態量ベクトル（予測）ｘｋ＋１−を、そのまま状態量ベクトル（推定）ｘｋ＋１＋とする。
【００６４】
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（推定）ｘｋ＋１＋を示す情報を出力する。
【００６５】
計測誤差記憶部１３７は、ＣＰＵ９１１などの処理装置を用いて、計測誤差推定部１３９が出力した状態量ベクトル（推定）ｘｋ＋１＋を示す情報を入力する。
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、入力した情報を、計測誤差情報として記憶する。
【００６６】
次に、動作について説明する。
【００６７】
図７は、この実施の形態における測位装置１００が現在位置を算出する測位処理の流れの一例を示すフローチャート図である。
【００６８】
Ｓ１１〜Ｓ１３の処理と、Ｓ２１〜Ｓ２５の処理とは、それぞれ独立して並列に動作する。
【００６９】
Ｓ１１において、ＧＰＳ受信機１１１は、ＧＰＳ衛星２００が発信したＧＰＳ信号を受信する。ＧＰＳ受信機１１１は、受信できるＧＰＳ信号が複数ある場合には、複数のＧＰＳ衛星２００が発信した複数のＧＰＳ信号をそれぞれ受信する。
【００７０】
Ｓ１２において、距離及び距離変化率観測部１１２は、Ｓ１１でＧＰＳ受信機１１１が受信したＧＰＳ信号に基づいて、ＧＰＳ衛星２００までの擬似距離及び擬似距離の変化率を算出する。
距離及び距離変化率観測部１１２は、算出した擬似距離及び擬似距離の変化率を示す情報を、距離及び距離変化率情報として出力する。
距離及び距離変化率観測部１１２は、ＧＰＳ受信機１１１が受信したＧＰＳ信号のうち、信頼できる擬似距離及び擬似距離の変化率を算出できるＧＰＳ信号についてのみ、距離及び距離変化率情報を出力する。
【００７１】
Ｓ１３において、観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、Ｓ１２で距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報を入力する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報の数が所定の数（例えば、２）以上である場合、観測値ベクトルｚｋ＋１を生成する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、生成した観測値ベクトルｚｋ＋１を示す情報を出力する。
【００７２】
一方、Ｓ２１において、車速パルス発生器１２１は、速度を計測する。
車速パルス発生器１２１は、計測した速度を示す速度情報を出力する。
【００７３】
Ｓ２２において、ジャイロスコープ１２２は、角速度を計測する。
ジャイロスコープ１２２は、計測した角速度を示す角速度情報を出力する。
【００７４】
Ｓ２３において、遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２１で車速パルス発生器１２１が出力した速度情報と、Ｓ２２でジャイロスコープ１２２が出力した角速度情報とを入力する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、入力した速度情報と角速度情報とに基づいて、状態遷移行列Φｋ，ｋ＋１を生成する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、生成した状態遷移行列Φｋ，ｋ＋１を示す情報を出力する。
【００７５】
Ｓ２４において、推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、推定誤差記憶部１３３が記憶した時刻ｋにおける誤差共分散行列（推定）Ｐｋ＋を示す推定誤差情報と、Ｓ２３で遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報とを入力する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における誤差共分散行列（予測）Ｐｋ＋１−＝Φｋ，ｋ＋１Ｐｋ＋Φｋ，ｋ＋１Ｔ＋Ｑｋ，ｋ＋１を算出する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を出力する。
【００７６】
Ｓ２５において、計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、計測誤差記憶部１３７が記憶した時刻ｋにおける状態量ベクトルｘｋ＋を示す計測誤差情報と、Ｓ２３で遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報とを入力する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における状態量ベクトル（予測）ｘｋ＋１−＝Φｋ，ｋ＋１ｘｋ＋を算出する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（予測）ｘｋ＋１−を示す情報を出力する。
【００７７】
Ｓ３１において、観測更新可能であるか否かに基づいて、処理を分岐する。観測更新可能である場合とは、観測値生成部１３２が観測値ベクトルｚｋ＋１を生成した場合のことである。
観測更新可能な場合には、Ｓ３２へ進む。
観測更新できない場合には、Ｓ３５へ進む。
【００７８】
Ｓ３２において、カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２４で推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、カルマンゲインＫ＝Ｐｋ＋１−Ｈｋ＋１（Ｈｋ＋１ＴＰｋ＋１−Ｈｋ＋１＋Ｒｋ＋１）−１を算出する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、算出したカルマンゲインＫを示す情報を出力する。
【００７９】
Ｓ３３において、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２４で推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報と、Ｓ３２でカルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報とを入力する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における誤差共分散行列（推定）Ｐｋ＋１＋＝Ｐｋ＋１−−ＫＨｋ＋１ＴＰｋ＋１−を算出する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報を出力する。
推定誤差記憶部１３３は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報を入力する。
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、入力した推定誤差情報を記憶する。
【００８０】
Ｓ３４において、計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、Ｓ１３で観測値生成部１３２が出力した観測値ベクトルｚｋ＋１を示す情報と、Ｓ２５で計測誤差予測部１３８が出力した状態量ベクトル（予測）ｘｋ＋１−を示す情報と、Ｓ３２でカルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報とを入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における状態量ベクトル（推定）ｘｋ＋１＋＝ｘｋ＋１−＋Ｋ（ｚｋ＋１−Ｈｋ＋１Ｔｘｋ＋１−）を算出する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報を出力する。
計測誤差記憶部１３７は、ＣＰＵ９１１などの処理装置を用いて、計測誤差推定部１３９が出力した状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報を入力する。
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、入力した計測誤差情報を記憶する。
その後、Ｓ４１へ進む。
【００８１】
一方、Ｓ３５において、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２４で推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した情報をそのまま、誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報として出力する。
推定誤差記憶部１３３は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報を入力する。
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、入力した推定誤差情報を記憶する。
【００８２】
Ｓ３６において、計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２５で計測誤差予測部１３８が出力した状態量ベクトル（予測）ｘｋ＋１−を示す情報を入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した情報をそのまま、状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報として出力する。
計測誤差記憶部１３７は、ＣＰＵ９１１などの処理装置を用いて、計測誤差推定部１３９が出力した状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報を入力する。
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、入力した計測誤差情報を記憶する。
その後、Ｓ４１へ進む。
【００８３】
Ｓ４１において、センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２１で車速パルス発生器１２１が出力した速度情報と、Ｓ２２でジャイロスコープ１２２が出力した角速度情報と、Ｓ３４またはＳ３６で計測誤差推定部１３９が出力した計測誤差情報とを入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、入力した計測誤差情報に基づいて、入力した速度情報が示す速度と角速度情報が示す角速度とを補正する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、補正した速度を示す補正速度情報と、補正した角速度を示す補正角速度情報とを出力する。
【００８４】
Ｓ４２において、位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、Ｓ４１でセンサ補正部１４１が出力した補正速度情報と、補正角速度情報とを入力する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、入力した補正速度情報と補正角速度情報とに基づいて、デッドレコニング処理を行い、現在位置を算出する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、算出した現在位置を示す情報を出力する。
【００８５】
Ｓ５１において、進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、Ｓ３３またはＳ３５で推定誤差算出部１３６が出力した推定誤差情報を入力する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差推定の進捗度を示す進捗度情報を出力する。
【００８６】
Ｓ５２において、信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、Ｓ５１で進捗度算出部１５１が出力した進捗度情報を入力する。
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報に基づいて、初期化が終了したか否かを判定する。初期化が終了していればセンサ補正部１４１が出力した補正速度情報・補正角速度情報などの補正計測情報が信頼できるので、信頼性判定部１５２は、すなわち、センサ補正部１４１が出力した補正計測情報が信頼できるか否かを判定する。また、補正計測情報が信頼できれば位置算出部１４２が出力した位置情報が信頼できるので、信頼性判定部１５２は、すなわち、位置算出部１４２が出力した位置情報が信頼できるか否かを判定する。
【００８７】
なお、この例では、速度情報及び角速度情報を状態遷移行列Φの生成に使用し、距離及び距離変化率情報から観測値ベクトルｚを生成しているが、速度情報及び角速度情報を、観測値ベクトルの生成に使用してもよい。
しかし、ＧＰＳ信号の受信状態によっては、観測更新に必要な距離及び距離変化率情報が得られない場合がある。速度情報及び角速度情報を観測値ベクトルｚの生成に使用すると、観測更新に必要な距離及び距離変化率情報が得られず観測更新ができなかった場合、推定された計測誤差には、計測された速度情報や角速度情報が反映されないことになる。
この例のように、速度情報及び角速度情報を観測値ベクトルｚではなく、状態遷移行列Φの生成に使用すると、観測更新に必要な距離及び距離変化率情報が得られない場合でも、ある程度の確度をもって、計測誤差を推定できるので、好ましい。
【００８８】
また、この例では、ＧＰＳ信号から算出した擬似距離及び擬似距離の変化率に基づいて、観測値ベクトルｚを生成している（これを「密結合カルマンフィルタ方式」という）が、ＧＰＳ信号に基づいて位置や速度を算出し、算出した位置や速度に基づいて、観測値ベクトルｚを生成してもよい（これを「疎結合カルマンフィルタ方式」という）。
しかし、ＧＰＳ信号に基づいて位置や速度を算出するためには、３乃至４以上のＧＰＳ信号を受信する必要があるのに対し、観測更新に必要な距離及び距離変化率情報は２つ以上（２衛星以上）あればよいので、この例における方式のほうが、利用可能なＧＰＳ信号が少ない場合でも、観測更新が可能となり、好ましい。
【００８９】
また、この例では、速度情報及び角速度情報に基づくデッドレコニングによる位置算出を主とし、これをＧＰＳ信号に基づいて補正しているが、ＧＰＳ信号に基づく位置算出を主として、デッドレコニングに基づいて補正することとしてもよい。
【００９０】
次に、誤差推定の進捗度について説明する。
【００９１】
図８は、この実施の形態における進捗度算出部１５１及び信頼性判定部１５２を示す図である。
【００９２】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した推定誤差情報を入力する。
推定誤差情報は、推定誤差算出部１３６が算出した時刻ｋ＋１における誤差共分散行列Ｐｋ＋１＋を示す情報である。
誤差共分散行列は、推定した状態量（この場合は計測誤差）と、真の状態量との差（推定誤差）を要素とするベクトルｐの外積の期待値〈ｐｐＴ〉である。
誤差共分散行列の対角要素は、対応する状態量の推定誤差の分散を示す。誤差共分散行列の非対角要素は、対応する２つの状態量の推定誤差の共分散を示し、２つの状態量の推定誤差の間に相関がなければ０となる。
したがって、誤差共分散行列の対角要素の値から、対応する状態量の推定誤差がどの程度あるかを知ることができる。
【００９３】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出すべき所定の状態量に対応する誤差共分散行列Ｐｋ＋１＋の対角要素の値（推定誤差の分散）Ｐｓｅｎｓｏｒを取得する。
【００９４】
推定した状態量が完全に真の状態量と一致し、推定誤差が０になれば、推定誤差の分散は０になる。しかし、実際には、推定した状態量が完全に真の状態量と一致することはないので、誤差推定が進むと、推定誤差の分散は０に近づくが０にはならない。
【００９５】
そこで、この実施の形態では、推定誤差の分散Ｐｓｅｎｓｏｒを、推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔと比較することにより、進捗度算出部１５１が誤差推定の進捗度を算出する。
推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔは、あらかじめ定めたものを、図示していない推定誤差初期値記憶部が、ＲＯＭ９１３などの記憶装置を用いて記憶しておく。推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔは、例えば、車速パルス発生器１２１の仕様書に記載の精度・誤差などに基づいて算出する。推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔは、観測開始時（時刻ｋ＝０）に推定誤差記憶部１３３が記憶している誤差共分散行列Ｐ０＋の要素である。
【００９６】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定の進捗度を算出すべき所定の状態量に対応する推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔ（誤差共分散行列の要素の初期値）を示す情報を入力する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した情報が示す推定誤差の分散Ｐｓｅｎｓｏｒと、推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出する。
【００９７】
この実施の形態では、誤差推定の進捗度Ｌｃｏｎｖを、Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔと定義する。進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定の進捗度Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出する。
この式は、推定誤差の分散Ｐｓｅｎｓｏｒが初期値Ｐｓｅｎｓｏｒ１ｓｔから比較してどこまで小さくなっているかを示すものであり、Ｐｓｅｎｓｏｒが初期値のままであれば推定誤差の進捗度（収束度合い）Ｌｃｏｎｖ＝０となる。逆に最大限に収束してＰｓｅｎｓｏｒ＝０となれば、Ｌｃｏｎｖ＝１となる。
したがって、誤差推定の進捗度Ｌｃｏｎｖは、０〜１の間の値をとり、大きいほど誤差推定が進んでいることを示す。
【００９８】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差推定の進捗度Ｌｃｏｎｖを示す進捗度情報を出力する。信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力する。
【００９９】
上述したように、推定誤差の分散Ｐｓｅｎｓｏｒが完全に０になることはないので、誤差推定が進むと進捗度Ｌｃｏｎｖは１に近づくが、１にはならない。
そこで、信頼性判定部１５２は、進捗度Ｌｃｏｎｖを所定の閾値（例えば０．９５）と比較することにより、誤差推定の信頼性を判定する。
すなわち、信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖと所定の閾値とを比較して、進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、センサの初期化が終了したと判定する。
【０１００】
「センサの初期化」とは、すなわち、車速パルス発生器１２１、ジャイロスコープ１２２などの計測誤差が正しく推定できたことをいう。
観測開始時には、計測誤差がどれほどあるかわからない。その後、カルマンフィルタにより、速度情報や角速度情報などの計測情報と、受信したＧＰＳ信号に基づく距離及び距離変化率情報などの観測情報とから、それぞれの計測誤差が推定できる。
ひとたび正しい計測誤差が推定できれば、その後ＧＰＳ信号の受信状態が悪くなっても、計測誤差がわかっているので、速度情報や角速度情報などだけに基づいて位置を算出しても、大きく間違うことはなく、正確な位置が算出できる。
ＧＰＳ信号が受信できない状態が長く続くと、推定した計測誤差が真の計測誤差からすこしずつ離れていく場合もあるが、ＧＰＳ信号が受信できれば、すぐに真の計測誤差に追随することができるので、推定した計測誤差が真の計測誤差から大きく離れることはない。
したがって、計測開始後、最初に進捗度Ｌｃｏｎｖが所定の閾値を超えるまでの期間を、センサの初期化の期間と呼ぶ。
【０１０１】
次に、この実施の形態における測位装置１００を用いた実験結果について説明する。
【０１０２】
図９は、この実施の形態における測位装置１００による計測誤差推定の実験結果を示すグラフ図である。
図１０は、図９に示した実験において、進捗度算出部１５１が算出した誤差推定の進捗度を示すグラフ図である。
【０１０３】
図９及び図１０において、横軸は共に時刻を示し、一目盛は５０秒である。
図９において、縦軸は誤差推定部１３０が推定した計測誤差を示す。
一番上のグラフは、車速パルス発生器１２１のスケールファクタ誤差について推定した計測誤差である。
真ん中のグラフは、ジャイロスコープ１２２のスケールファクタ誤差について推定した計測誤差である。
一番下のグラフは、ジャイロスコープ１２２のバイアス誤差について推定した計測誤差である。
図１０において、縦軸は誤差推定の進捗度（収束度）を示す。
一番上のグラフは、車速パルス発生器１２１のスケールファクタ誤差についての誤差推定の進捗度である。
真ん中のグラフは、ジャイロスコープ１２２のスケールファクタ誤差についての誤差推定の進捗度である。
一番下のグラフは、ジャイロスコープ１２２のバイアス誤差についての誤差推定の進捗度である。
【０１０４】
図９において、車速パルス発生器１２１のスケールファクタ誤差及びジャイロスコープ１２２のスケールファクタ誤差は、時刻４．５４３付近で一定値に収束している。また、ジャイロスコープ１２２のバイアス誤差は、時刻４．５４４付近で一定値に収束している。
一方、図１０において、推定した計測誤差が一定値に収束するのとほぼ同じタイミングで、進捗度が１に近づいており、進捗度の定義が妥当であることがわかる。
【０１０５】
特に注目すべき点は、ジャイロスコープ１２２のバイアス誤差について推定した計測誤差は、階段状に変化しており、時刻４．５４３付近で、一定値に収束したかに見える。
しかし、この時点での進捗度は０．５程度であるから、センサの初期化が終了していないことがわかる。
【０１０６】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
情報を処理するＣＰＵ９１１などの処理装置と、物理量（速度・角速度）を計測するセンサ（車速パルス発生器１２１・ジャイロスコープ１２２）と、誤差推定部１３０と、進捗度算出部１５１とを有し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、観測量（擬似距離及び擬似距離の変化率）を示す情報（距離及び距離変化率情報）を観測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した観測情報（距離及び距離変化率情報）に基づいて、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）の計測誤差を推定し、
ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力し、
進捗度算出部１５１は、
ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した進捗度Ｌｃｏｎｖを示す情報を進捗度情報として出力することを特徴とする。
【０１０７】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、誤差推定部１３０が算出した推定誤差に基づいて、進捗度算出部１５１が誤差推定の進捗度Ｌｃｏｎｖを算出するので、誤差推定の進捗度を計算により見積もることができるという効果を奏する。
【０１０８】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
誤差推定部１３０が、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタを用いて計測誤差ｘｋ＋１＋を推定し、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタが算出する誤差共分散行列Ｐｋ＋１＋を示す情報を推定誤差情報として出力することを特徴とする。
【０１０９】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、カルマンフィルタを用いて計測誤差を推定する誤差推定部１３０が、計測誤差を推定するために算出する誤差共分散行列Ｐｋ＋１＋に基づいて、進捗度算出部１５１が誤差推定の進捗度Ｌｃｏｎｖを算出するので、進捗度算出のための計算量が少なく、ＣＰＵ９１１などの処理装置の資源を有効に活用できるという効果を奏する。
【０１１０】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
進捗度算出部１５１が、
入力した推定誤差情報が示す誤差共分散行列Ｐｋ＋１＋の要素のうち、少なくとも一つの要素について、ＣＰＵ９１１などの処理装置を用いて、上記要素の値（推定誤差の分散）Ｐｓｅｎｓｏｒと上記要素の初期値（推定誤差の分散の初期値）Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出することを特徴とする。
【０１１１】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、進捗度算出部１５１が、誤差共分散行列Ｐｋ＋１＋の要素のうち、少なくとも一つの要素について、上記要素の値と、上記要素の初期値とに基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出するので、誤差推定の進捗度を正しく見積もることができるという効果を奏する。
【０１１２】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
進捗度算出部１５１が、
ＣＰＵ９１１などの処理装置を用いて、上記要素の値（推定誤差の分散）Ｐｓｅｎｓｏｒと上記要素の初期値（推定誤差の分散の初期値）Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出することを特徴とする。
【０１１３】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、進捗度算出部１５１が、Ｌｃｏｎｖ＝１−Ｐｓｅｎｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出することにより誤差推定の進捗度を算出するので、簡単な計算で進捗度を算出でき、ＣＰＵ９１１などの処理装置の資源を有効に活用できるという効果を奏する。
【０１１４】
この実施の形態における測位装置１００（センサ誤差推定装置）は、更に、
初期化終了判定部（信頼性判定部１５２）を有し、
初期化終了判定部（信頼性判定部１５２）は、
ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖと所定の閾値とを比較して、進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、初期化が終了したと判定することを特徴とする。
【０１１５】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、進捗度算出部１５１が算出した誤差推定の進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、信頼性判定部１５２が初期化が終了したと判定するので、センサの初期化が終了したか否かを、簡単に判定できるという効果を奏する。
【０１１６】
この実施の形態における測位装置１００（センサ誤差推定装置）は、更に、
受信装置（ＧＰＳ受信機１１１）と、距離及び距離変化率観測部１１２とを有し、
受信装置（ＧＰＳ受信機１１１）は、
発信装置（ＧＰＳ衛星２００）が発信した電波（ＧＰＳ信号）を受信し、
距離及び距離変化率観測部１１２は、
受信装置（ＧＰＳ受信機１１１）が受信した電波（ＧＰＳ信号）に基づいて、発信装置（ＧＰＳ衛星２００）までの距離（擬似距離）及び距離変化率（擬似距離の変化率）を算出し、算出した距離（擬似距離）及び距離変化率（擬似距離の変化率）を示す情報を距離及び距離変化率情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報を、観測情報として入力することを特徴とする。
【０１１７】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、受信装置（ＧＰＳ受信機１１１）が受信した電波（ＧＰＳ信号）に基づいて距離及び距離変化率観測部１１２が算出した擬似距離及び擬似距離の変化率に基づいて、誤差推定部１３０が計測誤差を推定し、進捗度算出部１５１が進捗度を算出するので、推定したセンサの観測計測誤差が信頼できるか否かを判定できるという効果を奏する。
【０１１８】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
速度センサ（車速パルス発生器１２１）を有し、
速度センサ（車速パルス発生器１２１）は、
速度を計測し、計測した速度を示す情報を、速度情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、速度センサ（車速パルス発生器１２１）が出力した速度情報を、計測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、速度センサ（車速パルス発生器１２１）のスケールファクタ誤差を推定することを特徴とする。
【０１１９】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、速度センサ（車速パルス発生器１２１）が計測した速度について、誤差推定部１３０がスケールファクタ誤差を推定し、進捗度算出部１５１が進捗度を算出するので、推定したスケールファクタ誤差が信頼できるか否かを判定できるという効果を奏する。
【０１２０】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
角速度センサ（ジャイロスコープ１２２）を有し、
角速度センサ（ジャイロスコープ１２２）は、
角速度を計測し、計測した角速度を示す情報を、角速度情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、角速度センサ（ジャイロスコープ１２２）が出力した角速度情報を、計測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、角速度センサ（ジャイロスコープ１２２）のスケールファクタ誤差及びバイアス誤差の少なくともいずれかを推定することを特徴とする。
【０１２１】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、角速度センサ（ジャイロスコープ１２２）が計測した角速度について、誤差推定部１３０がスケールファクタ誤差及びバイアス誤差の少なくともいずれかを推定し、進捗度算出部１５１が進捗度を算出するので、推定したスケールファクタ誤差またはバイアス誤差が信頼できるか否かを判定できるという効果を奏する。
【０１２２】
この実施の形態における測位装置１００（観測装置）は、
情報を処理するＣＰＵ９１１などの処理装置と、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）と、誤差推定部１３０と、進捗度算出部１５１と、センサ補正部１４１と、信頼性判定部１５２とを有し、
センサ（車速パルス発生器１２１・ジャイロスコープ１２２）は、
物理量（速度・角速度）を計測し、計測した物理量（速度・角速度）を示す情報を、計測情報（速度情報・角速度情報）として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、観測量（擬似距離及び擬似距離の変化率）を示す情報を観測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）が出力した計測情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した観測情報（距離及び距離変化率情報）と入力した計測情報（速度情報・角速度情報）とに基づいて、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）の計測誤差を推定し、
ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差を示す情報を計測誤差情報として出力し、
ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力し、
進捗度算出部１５１は、
ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力し、
センサ補正部１４１は、
ＣＰＵ９１１などの処理装置を用いて、計測情報（速度情報・角速度情報）と、誤差推定部１３０が出力した計測誤差情報とを入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した計測情報（速度情報・角速度情報）が示す物理量（速度・角速度）を、入力した計測誤差情報が示す計測誤差で補正し、
ＣＰＵ９１１などの処理装置を用いて、補正した物理量（速度・角速度）を示す情報（補正速度情報・補正角速度情報）を補正計測情報として出力し、
信頼性判定部１５２は、
ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖに基づいて、センサ補正部１４１が出力した補正計測情報が信頼できるか否かを判定することを特徴とする。
【０１２３】
この実施の形態における測位装置１００（観測装置）によれば、進捗度算出部１５１が算出した進捗度Ｌｃｏｎｖに基づいて、信頼性判定部１５２が補正計測情報の信頼性を判定するので、補正計測情報が信頼できるか否かを正しく判定できるという効果を奏する。
【０１２４】
この実施の形態における測位装置１００（観測装置）は、
誤差推定部１３０と、進捗度算出部１５１と、信頼性判定部１５２とを有し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタを用いて計測誤差ｘｋ＋１＋を推定し、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタが算出する誤差共分散行列Ｐｋ＋１＋を示す情報を推定誤差情報として出力し、
進捗度算出部１５１は、
入力した推定誤差情報が示す誤差共分散行列Ｐｋ＋１＋の要素のうち、少なくとも一つの要素について、ＣＰＵ９１１などの処理装置を用いて、上記要素の値Ｐｓｅｎｓｏｒと上記要素の初期値Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出し、
信頼性判定部１５２は、
ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖと所定の閾値とを比較して、進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、センサ補正部１４１が出力した補正計測情報が信頼できると判定することを特徴とする。
【０１２５】
この実施の形態における測位装置１００（観測装置）によれば、信頼性判定部１５２が、進捗度と所定の閾値とを比較して、補正計測情報の信頼性を判定するので、簡単な計算で信頼性を判定でき、ＣＰＵ９１１などの処理装置の資源を有効に活用できるという効果を奏する。
【０１２６】
この実施の形態におえる測位装置１００（観測装置）は、更に、
受信装置（ＧＰＳ受信機１１１）と、距離及び距離変化率観測部１１２と、速度センサ（車速パルス発生器１２１）と、角速度センサ（ジャイロスコープ１２２）と、位置算出部１４２とを有し、
受信装置（ＧＰＳ受信機１１１）は、
発信装置（ＧＰＳ衛星２００）が発信した電波（ＧＰＳ信号）を受信し、
距離及び距離変化率観測部１１２は、
受信装置（ＧＰＳ受信機１１１）が受信した電波（ＧＰＳ信号）に基づいて、発信装置（ＧＰＳ衛星２００）までの距離（擬似距離）及び距離変化率（擬似距離の変化率）を算出し、算出した距離（擬似距離）及び距離変化率（擬似距離の変化率）を示す情報を距離及び距離変化率情報として出力し、
速度センサ（車速パルス発生器１２１）は、
速度を計測し、計測した速度を示す情報を、速度情報として出力し、
各速度センサ（ジャイロスコープ１２２）は、
角速度を計測し、計測した角速度を示す情報を、角速度情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報と、速度センサ（車速パルス発生器１２１）が出力した速度情報と、角速度センサ（ジャイロスコープ１２２）が出力した角速度情報とを、計測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、速度センサ（車速パルス発生器１２１）のスケールファクタ誤差と、角速度センサ（ジャイロスコープ１２２）のスケールファクタ誤差及びバイアス誤差とを推定し、
センサ補正部１４１は、
ＣＰＵ９１１などの処理装置を用いて、速度情報が示す速度と上記角速度情報が示す角速度とを、誤差推定部１３０が推定したスケールファクタ誤差及びバイアス誤差で補正し、
位置算出部１４２は、
ＣＰＵ９１１などの処理装置を用いて、センサ補正部１４１が出力した補正計測情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した補正計測情報に基づいて、現在位置を算出することを特徴とする。
【０１２７】
この実施の形態における測位装置１００（観測装置）によれば、センサ補正部１４１が補正した補正計測情報に基づいて、位置算出部１４２が位置を算出し、信頼性判定部１５２が信頼性を判定するので、位置算出部１４２が算出した現在位置が信頼できるか否かを判定できるという効果を奏する。
【０１２８】
この実施の形態における測位装置１００（センサ誤差推定装置）が計測誤差を推定する方法は、
情報を処理するＣＰＵ９１１などの処理装置と、物理量を計測するセンサ（車速パルス発生器１２１・ジャイロスコープ１２２）とを有するセンサ誤差推定装置（測位装置１００）が計測誤差を推定する方法において、
ＣＰＵ９１１などの処理装置が、観測量（擬似距離及び擬似距離の変化率）を示す情報（距離及び距離変化率情報）を観測情報として入力し、
ＣＰＵ９１１などの処理装置が、上記センサ（車速パルス発生器１２１・ジャイロスコープ１２２）が計測した物理量（速度・角速度）を示す情報（速度情報・角速度情報）を計測情報として入力し、
ＣＰＵ９１１などの処理装置が、入力した観測情報（距離及び距離変化率情報）と入力した計測情報とに基づいて、上記センサの計測誤差を推定し、
ＣＰＵ９１１などの処理装置が、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置が、算出した進捗度Ｌｃｏｎｖを示す情報を進捗度情報として出力することを特徴とする。
【０１２９】
この実施の形態における方法によれば、ＣＰＵ９１１などの処理装置が、センサの計測誤差を推定し、推定した計測誤差の推定誤差を算出し、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出するので、誤差推定の進捗度を計算により見積もることができるという効果を奏する。
【０１３０】
この実施の形態における測位装置１００（観測装置）が観測する方法は、
情報を処理するＣＰＵ９１１などの処理装置と、物理量を計測するセンサ（車速パルス発生器１２１・ジャイロスコープ１２２）とを有する観測装置（測位装置１００）が観測する方法において、
ＣＰＵ９１１などの処理装置が、観測量（擬似距離及び擬似距離の変化率）を示す情報（距離及び距離変化率情報）を観測情報として入力し、
ＣＰＵ９１１などの処理装置が、上記センサ（車速パルス発生器１２１・ジャイロスコープ１２２）が計測した物理量（速度・角速度）を示す情報（速度情報・角速度情報）を計測情報として入力し、
ＣＰＵ９１１などの処理装置が、入力した観測情報（距離及び距離変化率情報）と計測情報（速度情報・角速度情報）とに基づいて、上記センサ（車速パルス発生器１２１・ジャイロスコープ１２２）の計測誤差を推定し、
ＣＰＵ９１１などの処理装置が、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置が、入力した計測情報が示す物理量（速度・角速度）を、推定した計測誤差で補正し、
ＣＰＵ９１１などの処理装置が、補正した物理量（速度・角速度）を示す情報を補正計測情報として出力し、
ＣＰＵ９１１などの処理装置が、算出した誤差推定の進捗度Ｌｃｏｎｖに基づいて、出力した補正計測情報が信頼できるか否かを判定することを特徴とする。
【０１３１】
この実施の形態における方法によれば、ＣＰＵ９１１などの処理装置が、センサの計測誤差を推定し、推定した計測誤差の推定誤差を算出し、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、算出した誤差推定の進捗度Ｌｃｏｎｖに基づいて、補正計測情報の信頼性を判定するので、補正計測情報の信頼性を正しく判定できるという効果を奏する。
【０１３２】
この実施の形態における測位装置１００は、情報を処理するＣＰＵ９１１などの処理装置を有するコンピュータをこの実施の形態における測位装置１００として機能させるプログラムを、上記コンピュータが実行することにより、実現することができる。
【０１３３】
この実施の形態におけるプログラムは、
情報を処理するＣＰＵ９１１などの処理装置を有するコンピュータを、この実施の形態における測位装置１００（センサ誤差推定装置）として機能させることを特徴とする。
【０１３４】
この実施の形態におけるプログラムによれば、誤差推定の進捗度を、コンピュータによる計算により見積もることができるという効果を奏する。
【０１３５】
この実施の形態におけるプログラムは、
情報を処理するＣＰＵ９１１などの処理装置を有するコンピュータを、この実施の形態における測位装置１００（観測装置）として機能させることを特徴とする。
【０１３６】
この実施の形態におけるプログラムによれば、補正計測情報の信頼性を、コンピュータによる計算により判定できるという効果を奏する。
【０１３７】
この実施の形態における測位装置１００（ＧＰＳ／センサ複合演算測位装置）は、センサ誤差推定計算の収束状況を計算により見積もることを特徴とする。
これにより、ＧＰＳ／センサ複合演算測位装置において、ジャイロ、車速パルス、加速度センサなどのセンサの誤差推定（初期化）の進行度を計算できる。
また、初期化の進行状況を見積もることができるので、現在のセンサデータの信頼度を推定でき、測位結果の信頼度を推定できる。
【図面の簡単な説明】
【０１３８】
【図１】実施の形態１における測位システム８００の全体構成の一例を示すシステム構成図。
【図２】実施の形態１における測位装置１００の外観の一例を示す図。
【図３】実施の形態１における測位装置１００のハードウェア資源の一例を示す図。
【図４】実施の形態１における測位装置１００の機能ブロックの一例を示すブロック構成図。
【図５】実施の形態１における誤差推定部１３０が計測誤差を推定するために用いるカルマンフィルタの動作を示す図。
【図６】実施の形態１における誤差推定部１３０の内部ブロックの構成を示す詳細ブロック図。
【図７】実施の形態１における測位装置１００が現在位置を算出する測位処理の流れの一例を示すフローチャート図。
【図８】実施の形態１における進捗度算出部１５１及び信頼性判定部１５２を示す図。
【図９】実施の形態１における測位装置１００による計測誤差推定の実験結果を示すグラフ図。
【図１０】図９に示した実験において、進捗度算出部１５１が算出した誤差推定の進捗度を示すグラフ図。
【符号の説明】
【０１３９】
１００ 測位装置、１１１ ＧＰＳ受信機、１１２ 距離及び距離変化率観測部、１２１ 車速パルス発生器、１２２ ジャイロスコープ、１３０ 誤差推定部、１３１ 遷移行列生成部、１３２ 観測値生成部、１３３ 推定誤差記憶部、１３４ 推定誤差予測部、１３５ カルマンゲイン算出部、１３６ 推定誤差算出部、１３７ 計測誤差記憶部、１３８ 計測誤差予測部、１３９ 計測誤差推定部、１４１ センサ補正部、１４２ 位置算出部、１５１ 進捗度算出部、１５２ 信頼性判定部、２００ ＧＰＳ衛星、８００ 測位システム、９０１ 表示装置、９０２ キーボード、９０３ マウス、９０４ ＦＤＤ、９０５ ＣＤＤ、９０６ プリンタ装置、９０７ スキャナ装置、９１０ システムユニット、９１１ ＣＰＵ、９１２ バス、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信装置、９２０ 磁気ディスク装置、９２１ ＯＳ、９２２ ウィンドウシステム、９２３ プログラム群、９２４ ファイル群、９３１ 電話器、９３２ ファクシミリ機、９４０ インターネット、９４１ ゲートウェイ、９４２ ＬＡＮ。
【技術分野】
【０００１】
この発明は、センサの計測誤差を推定する装置、及び推定した計測誤差による補正をして観測する装置、及びその装置が用いる方法、及びコンピュータを用いてその装置を実現するプログラムに関する。
【背景技術】
【０００２】
計測に用いるセンサなどは誤差を有しているので、正確な計測を行うには、センサなどが有する計測誤差を推定し、補正する必要がある。
例えば、測位装置においては、速度・角速度などを計測し、デッドレコニング（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ：自律航法測位）計算により、位置を算出する方法と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：全地球測位システム）衛星からのＧＰＳ信号に基づいて、ＧＰＳ衛星までの擬似距離を観測し、位置を算出する方法とがある。また、カルマンフィルタなどを用いて２つの方法を結合することにより、センサなどが有する計測誤差を推定する技術がある。
【特許文献１】特開２００６−１８９３２０号公報
【特許文献２】特開２００５−２４１５１７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
計測誤差の推定結果がどれほど信頼できるかにより、観測結果に対する信頼性も変わってくる。特に、カルマンフィルタを用いた方法では、観測開始直後は推定結果の信頼性が低く、徐々に信頼性が高くなる。しかし、観測開始直後の信頼性が低い期間（センサの初期化期間）がどの程度あるかは、必ずしも明らかではない。
この発明は、例えば、上記のような課題を解決するためになされたものであり、誤差推定がどこまで進んでいるかを計算により見積もることを目的とする。
【課題を解決するための手段】
【０００４】
この発明にかかるセンサ誤差推定装置は、
情報を処理する処理装置と、
物理量を計測するセンサと、
上記処理装置を用いて、観測量を示す情報を観測情報として入力し、上記処理装置を用いて、入力した観測情報に基づいて、上記センサの計測誤差を推定し、上記処理装置を用いて、推定した計測誤差の推定誤差を算出し、上記処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力する誤差推定部と、
上記処理装置を用いて、上記誤差推定部が出力した推定誤差情報を入力し、上記処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出し、上記処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力する進捗度算出部と、
を有することを特徴とする。
【発明の効果】
【０００５】
この発明にかかるセンサ誤差推定装置によれば、誤差推定部が算出した推定誤差に基づいて、進捗度算出部が誤差推定の進捗度を算出するので、誤差推定の進捗度を計算により見積もることができるという効果を奏する。
【発明を実施するための最良の形態】
【０００６】
実施の形態１．
実施の形態１を、図１〜図１０を用いて説明する。
【０００７】
図１は、この実施の形態における測位システム８００の全体構成の一例を示すシステム構成図である。
測位装置１００は、例えば車両等に搭載し、車両等の現在位置を算出する。測位装置１００は、観測装置及びセンサ誤差推定装置の一例である。
測位装置１００は、ＧＰＳ衛星２００が発信したＧＰＳ信号を受信する。ＧＰＳ衛星２００は、発信装置の一例である。ＧＰＳ信号は、発信装置が発信した電波の一例である。
測位装置１００は、また、搭載された車両等の速度・加速度・角速度などを計測するセンサを有している。
測位装置１００は、ＧＰＳ／センサ複合測位を行う。
すなわち、測位装置１００は、センサが計測した速度・加速度・角速度などに基づいて、デッドレコニングにより車両等の位置を算出する。
また、測位装置１００は、受信したＧＰＳ信号に基づいて、センサ等の誤差を推定し、センサが計測した速度などを補正することにより、正確な位置を算出する。
【０００８】
図２は、この実施の形態における測位装置１００の外観の一例を示す図である。
測位装置１００は、システムユニット９１０、ＣＲＴ（Ｃａｔｈｏｄｅ・Ｒａｙ・Ｔｕｂｅ）やＬＣＤ（液晶）の表示画面を有する表示装置９０１、キーボード９０２（Ｋｅｙ・Ｂｏａｒｄ：Ｋ／Ｂ）、マウス９０３、ＦＤＤ９０４（Ｆｌｅｘｉｂｌｅ・Ｄｉｓｋ・Ｄｒｉｖｅ）、コンパクトディスク装置９０５（ＣＤＤ）、プリンタ装置９０６、スキャナ装置９０７などのハードウェア資源を備え、これらはケーブルや信号線で接続されている。
システムユニット９１０は、コンピュータであり、ファクシミリ機９３２、電話器９３１とケーブルで接続され、また、ローカルエリアネットワーク９４２（ＬＡＮ）、ゲートウェイ９４１を介してインターネット９４０に接続されている。
【０００９】
図３は、この実施の形態における測位装置１００のハードウェア資源の一例を示す図である。
図３において、測位装置１００は、プログラムを実行するＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信装置９１５、表示装置９０１、キーボード９０２、マウス９０３、ＦＤＤ９０４、ＣＤＤ９０５、プリンタ装置９０６、スキャナ装置９０７、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。磁気ディスク装置９２０の代わりに、光ディスク装置、メモリカード読み書き装置などの記憶装置でもよい。
ＲＡＭ９１４は、揮発性メモリの一例である。ＲＯＭ９１３、ＦＤＤ９０４、ＣＤＤ９０５、磁気ディスク装置９２０の記憶媒体は、不揮発性メモリの一例である。これらは、記憶装置あるいは記憶部の一例である。
通信装置９１５、キーボード９０２、スキャナ装置９０７、ＦＤＤ９０４などは、入力部、入力装置の一例である。
また、通信装置９１５、表示装置９０１、プリンタ装置９０６などは、出力部、出力装置の一例である。
【００１０】
通信装置９１５は、ファクシミリ機９３２、電話器９３１、ＬＡＮ９４２等に接続されている。通信装置９１５は、ＬＡＮ９４２に限らず、インターネット９４０、ＩＳＤＮ等のＷＡＮ（ワイドエリアネットワーク）などに接続されていても構わない。インターネット９４０或いはＩＳＤＮ等のＷＡＮに接続されている場合、ゲートウェイ９４１は不用となる。
磁気ディスク装置９２０には、オペレーティングシステム９２１（ＯＳ）、ウィンドウシステム９２２、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３のプログラムは、ＣＰＵ９１１、オペレーティングシステム９２１、ウィンドウシステム９２２により実行される。
【００１１】
上記プログラム群９２３には、以下に述べる実施の形態の説明において「〜部」、「〜手段」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
ファイル群９２４には、以下に述べる実施の形態の説明において、「〜の判定結果」、「〜の計算結果」、「〜の処理結果」として説明する情報やデータや信号値や変数値やパラメータが、「〜ファイル」や「〜データベース」の各項目として記憶されている。「〜ファイル」や「〜データベース」は、ディスクやメモリなどの記録媒体に記憶される。ディスクやメモリになどの記憶媒体に記憶された情報やデータや信号値や変数値やパラメータは、読み書き回路を介してＣＰＵ９１１によりメインメモリやキャッシュメモリに読み出され、抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示などのＣＰＵの動作に用いられる。抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示のＣＰＵの動作の間、情報やデータや信号値や変数値やパラメータは、メインメモリやキャッシュメモリやバッファメモリに一時的に記憶される。
また、以下に述べる実施の形態の説明において説明するフローチャートの矢印の部分は主としてデータや信号の入出力を示し、データや信号値は、ＲＡＭ９１４のメモリ、ＦＤＤ９０４のフレキシブルディスク、ＣＤＤ９０５のコンパクトディスク、磁気ディスク装置９２０の磁気ディスク、その他光ディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）等の記録媒体に記録される。また、データや信号は、バス９１２や信号線やケーブルその他の伝送媒体によりオンライン伝送される。
【００１２】
また、以下に述べる実施の形態の説明において「〜部」、「〜手段」として説明するものは、「〜回路」、「〜装置」、「〜機器」であってもよく、また、「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」、「〜手段」として説明するものは、ＲＯＭ９１３に記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、素子・デバイス・基板・配線などのハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。ファームウェアとソフトウェアは、プログラムとして、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等の記録媒体に記憶される。プログラムはＣＰＵ９１１により読み出され、ＣＰＵ９１１により実行される。すなわち、プログラムは、以下に述べる「〜部」、「〜手段」としてコンピュータを機能させるものである。あるいは、以下に述べる「〜部」、「〜手段」の手順や方法をコンピュータに実行させるものである。
【００１３】
図４は、この実施の形態における測位装置１００の機能ブロックの一例を示すブロック構成図である。
測位装置１００は、ＧＰＳ受信機１１１、距離及び距離変化率観測部１１２、車速パルス発生器１２１、ジャイロスコープ１２２、誤差推定部１３０、センサ補正部１４１、位置算出部１４２、進捗度算出部１５１、信頼性判定部１５２を有する。
【００１４】
ＧＰＳ受信機１１１は、ＧＰＳ衛星２００が発信したＧＰＳ信号を受信する。ＧＰＳ受信機１１１は、受信装置の一例である。
ＧＰＳ受信機１１１は、受信できるＧＰＳ信号が複数ある場合には、複数のＧＰＳ衛星２００がそれぞれ発信した複数のＧＰＳ信号を、それぞれ受信する。
【００１５】
距離及び距離変化率観測部１１２は、ＧＰＳ受信機１１１が受信したＧＰＳ信号に基づいて、ＧＰＳ衛星までの距離（以下「擬似距離」という。）と、擬似距離の変化率とを算出する。
距離及び距離変化率観測部１１２は、ＧＰＳ受信機１１１が受信したＧＰＳ信号が複数ある場合には、ＧＰＳ受信機１１１が受信した複数のＧＰＳ信号それぞれについて、それぞれのＧＰＳ衛星２００までの擬似距離及び擬似距離の変化率を算出する。ただし、信頼できる擬似距離及び擬似距離の変化率を算出できないＧＰＳ信号については棄却し、擬似距離や擬似距離の変化率を算出しない。
信頼できる擬似距離や擬似距離の変化率が算出できない場合としては、例えば、受信したＧＰＳ信号がマルチパスである場合や、ＧＰＳ衛星２００の仰角が低い場合などがある。
距離及び距離変化率観測部１１２は、これらの条件を判定して、擬似距離や擬似距離の変化率を算出するか否かを決定する。あるいは、距離及び距離変化率観測部１１２は、擬似距離や擬似距離の変化率を算出したのち、算出した擬似距離や擬似距離の変化率が信頼できるものであるか否かを判断して、棄却してもよい。
距離及び距離変化率観測部１１２は、算出した擬似距離や擬似距離の変化率を示す情報を距離及び距離変化率情報として出力する。
【００１６】
車速パルス発生器１２１は、測位装置１００を搭載した車両等の速度を計測する。車速パルス発生器１２１は、速度センサの一例である。
車速パルス発生器１２１は、車速パルスを出力する。車速パルスとは、計測した速度に比例する周波数を有するパルス信号である。車速パルスは、速度情報の一例である。
【００１７】
ジャイロスコープ１２２は、測位装置１００を相対した車両等の角速度を計測する。ジャイロスコープ１２２は、角速度センサの一例である。
ジャイロスコープ１２２は、計測した角速度に比例する電圧（あるいは、その電圧をアナログデジタル変換した数値を示す情報）を出力する。ジャイロスコープ１２２が出力する電圧（あるいは数値）は、角速度情報の一例である。
【００１８】
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報を入力する。
距離及び距離変化率情報は、観測情報の一例である。
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、車速パルス発生器１２１が出力した車速パルス（あるいは、車速パルスをＣＰＵ９１１が処理可能な情報に変換した情報）を入力する。
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、ジャイロスコープ１２２が出力した電圧（あるいは、電圧をＣＰＵ９１１が処理可能な情報に変換した情報）を入力する。
車速パルスなどの速度情報・角速度情報は、計測情報の一例である。
【００１９】
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、入力した観測情報と計測情報とに基づいて、センサの計測誤差を推定する。
計測誤差とは、センサの計測に伴う誤差である。測位装置１００は、誤差推定部１３０が推定した計測誤差を補正することにより、正確な位置を算出する。
【００２０】
例えば、車速パルス発生器１２１の場合、車速パルス発生器１２１が出力した車速パルスの周波数に、あらかじめ定められた定数（スケールファクタ）を乗じることにより、車速パルス発生器１２１が計測した車両等の速度を知ることができる。しかし、車速パルス発生器１２１のスケールファクタは、周囲の温度や製造時の精度などにより、公称値と異なる値となる場合があるので、正確なスケールファクタを知らなければ、車両等の正確な速度を求めることができない。このようなスケールファクタのずれを、スケールファクタ誤差という。スケールファクタ誤差は、計測誤差の一例である。
【００２１】
ジャイロスコープ１２２の場合も同様に、ジャイロスコープ１２２が出力した電圧に、あらかじめ定められた定数（スケールファクタ）を乗じることにより、ジャイロスコープ１２２が計測した角速度を知ることができる。したがって、ジャイロスコープ１２２にもスケールファクタ誤差が存在する。
また、ジャイロスコープ１２２は、回転していない状態でも一定の電圧を出力する場合があるので、これも誤差の要因となる。このようなずれをバイアス誤差という。バイアス誤差も、計測誤差の一例である。
【００２２】
また、距離及び距離変化率観測部１１２では、通常、ＧＰＳ衛星２００までの距離及び距離変化率を算出するため、内部に時計を有している。この時計のクロック周波数がずれると、ＧＰＳ衛星２００までの距離や擬似距離の変化率を正確に算出できない。クロックの誤差には、バイアス誤差、ドリフト誤差などがある。バイアス誤差、ドリフト誤差も、計測誤差の一例である。
【００２３】
誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差を示す情報を、計測誤差情報として出力する。
【００２４】
また、誤差推定部１３０は、ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差の推定誤差を算出する。
推定誤差とは、推定した計測誤差と真の計測誤差との間の誤差がどのくらいあるかを示すものである。推定した計測誤差は、真の計測誤差を中心として正規分布すると考えられるので、推定誤差は、その分散（あるいは、２つ以上の推定誤差の間に相関がある場合は共分散）で表わす。
誤差推定部１３０は、算出した推定誤差を示す情報を、推定誤差情報として出力する。
【００２５】
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、車速パルス発生器１２１が出力した車速パルスなどの速度情報を入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、ジャイロスコープ１２２が出力した角速度情報を入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、入力した速度情報が示す速度を、入力した推定誤差情報が示す推定誤差で補正する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、入力した角速度情報が示す角速度を、入力した推定誤差情報が示す推定誤差で補正する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、補正した物理量を示す情報を、補正計測情報として出力する。
【００２６】
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、センサ補正部１４１が出力した補正計測情報を入力する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、入力した補正計測情報に基づいて、現在位置を算出する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、算出した現在位置を示す情報を、位置情報として出力する。
位置算出部１４２が出力した位置情報は、例えば、表示装置９０１などに表示して利用者等に知らせる。
【００２７】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出する。
【００２８】
測位装置１００が動作を開始した直後は、誤差推定部１３０は計測誤差を正しく推定できない。距離及び距離変化率情報などの観測情報や、速度情報・角速度情報などの計測情報が集まるにつれて、計測誤差の推定の精度が向上していく。
誤差推定の進捗度は、計測誤差の推定の精度がどのくらいであるかを示すものである。
【００２９】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、算出した進捗度を示す情報を、進捗度情報として出力する。
【００３０】
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力する。
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報に基づいて、誤差推定部１３０の初期化が終わったか否か、すなわち、誤差推定部１３０が推定した計測誤差が信頼できるか否かを判定する。信頼性判定部１５２は、初期化終了判定部の一例である。
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、判定した判定結果を示す情報を、判定情報として出力する。
【００３１】
信頼性判定部１５２が出力した判定情報は、例えば、表示装置９０１などに表示して、利用者等に知らせる。
あるいは、位置算出部１４２が算出した位置情報をマップマッチング（Ｍａｐ Ｍａｔｃｈｉｎｇ：地図照合）する際に利用してもよい。
【００３２】
デッドレコニングによる位置算出では、車両等の移動ベクトルを積分して、現在位置を求める。移動ベクトルは、移動速度と移動方向とから求める。移動速度は、速度センサが計測した値である。移動方向は、角速度センサが計測した角速度を積分することにより求める。
したがって、速度センサや角速度センサに計測誤差があると、たとえ微小な誤差でも積分によって誤差が蓄積し、正しい位置算出ができない。
そのため、ＧＰＳ信号による補正が必要となる。
【００３３】
ＧＰＳ測位は、絶対精度が高いが、受信状態が悪いと測位できない場合があり、連続性が劣っている。そこで、ＧＰＳ測位とデッドレコニング測位とを複合計算することで、互いの長所を生かした高精度な測位を実現する。
【００３４】
一方、センサの計測誤差が正しく推定された後であれば、ＧＰＳ信号が受信できなくても、ある程度正確な位置算出ができる。これにより、高層ビル街のようなＧＰＳ信号の受信状態が悪い状況や、地下のようなＧＰＳ信号が受信できない状況であっても、正確な位置算出ができる。
したがって、ＧＰＳ信号が受信できない状況などになったとき、センサの計測誤差が正しく推定されているか否かにより、位置算出部１４２が算出する現在位置の信頼性が大きく変わることになる。
【００３５】
このため、計測誤差が正しく推定されているか否かを正確に判定することが重要である。
【００３６】
次に、誤差推定部１３０について、詳しく説明する。
【００３７】
図５は、この実施の形態における誤差推定部１３０が計測誤差を推定するために用いるカルマンフィルタの動作を示す図である。
【００３８】
カルマンフィルタは、離散的に得られる観測値を元に、知りたい状態量を推定するものである。
カルマンフィルタには、大きく分けて、時間外挿過程と観測更新過程との２つの工程がある。
時間外挿過程は、１つ前の状態から、現在の状態を予測する工程である。
観測更新過程は、現在の状態について得られた観測値を元に、真の状態を推定する工程である。
カルマンフィルタは、観測値が得られるたびにこの２つの工程を繰り返し、徐々に推定の精度を上げていく。
【００３９】
時間外挿過程には、状態方程式外挿処理と、誤差共分散外挿処理とがある。
【００４０】
状態方程式外挿処理とは、１つ前の状態量ベクトル（推定）ｘｋ＋から、現在の状態量ベクトル（予測）ｘｋ＋１−を予測する処理である。
状態量の予測には、状態遷移行列Φを用いる。
状態遷移行列Φは、例えば、力学モデルから導き出したものである。
【００４１】
誤差共分散外挿処理とは、１つ前の誤差共分散行列（推定）Ｐｋ＋から、現在の誤差共分散行列（予測）Ｐｋ＋１−を予測する処理である。
誤差共分散の予測には、状態遷移行列Φと、プロセスノイズ行列Ｑとを用いる。
状態遷移行列Φは、状態方程式外挿処理で用いたものと同じものである。
プロセスノイズ行列Ｑは、モデルの時間発展に伴う誤差を示すものである。
【００４２】
観測更新過程には、カルマンゲイン算出処理と、状態方程式更新処理と、誤差共分散更新処理とがある。
【００４３】
カルマンゲイン算出処理とは、状態量ベクトル（予測）ｘｋ＋１−を、観測値ベクトルｚｋ＋１で補正して、状態量ベクトル（推定）ｘｋ＋１＋を得るための行列であるカルマンゲインＫを算出する処理である。
カルマンゲインの算出には、誤差共分散行列（予測）Ｐｋ＋１−と、観測行列Ｈと、観測ノイズ行列Ｒとを用いる。
誤差共分散行列（予測）は、誤差共分散外挿処理で予測したものである。
観測行列Ｈは、状態量ベクトルｘを観測値ベクトルｚに変換する行列である。すなわち、状態量ベクトルが示す状態を観測した結果として、観測値ベクトルが示す観測値が得られるという関係を数式化したものである。
観測ノイズ行列Ｒは、観測に伴う誤差を示すものである。観測ノイズ行列Ｒが示す観測に伴う誤差は、スケールファクタ誤差などの組織的な観測誤差とは異なり、観測に伴ってランダムに混入する誤差（ノイズ）のことである。
【００４４】
状態方程式更新処理とは、状態量ベクトル（予測）ｘｋ＋１−を、観測値ベクトルｚｋ＋１で補正し、状態量ベクトル（推定）ｘｋ＋１＋を推定する処理である。
状態量ベクトルの推定には、カルマンゲインＫと、観測行列Ｈとを用いる。
カルマンゲインＫは、カルマンゲイン算出処理で算出したものである。
観測行列Ｈは、カルマンゲイン算出処理で用いたものと同じものである。
【００４５】
誤差共分散更新処理とは、誤差共分散行列（予測）Ｐｋ＋１−を補正して、誤差共分散行列（推定）Ｐｋ＋１＋を推定する処理である。
誤差共分散行列の推定には、カルマンゲインＫと、観測行列Ｈとを用いる。
カルマンゲインＫは、カルマンゲイン算出処理で算出したものである。
観測行列Ｈは、カルマンゲイン算出処理で用いたものと同じものである。
【００４６】
カルマンフィルタは、以上の処理を繰り返すことにより、状態を推定する。
【００４７】
図６は、この実施の形態における誤差推定部１３０の内部ブロックの構成を示す詳細ブロック図である。
誤差推定部１３０は、遷移行列生成部１３１、観測値生成部１３２、推定誤差記憶部１３３、推定誤差予測部１３４、カルマンゲイン算出部１３５、推定誤差算出部１３６、計測誤差記憶部１３７、計測誤差予測部１３８、計測誤差推定部１３９を有する。
【００４８】
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、時刻ｋ＋１における速度情報と角速度情報とを入力する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、入力した速度情報と角速度情報に基づいて、時刻ｋから時刻ｋ＋１への状態遷移行列Φｋ，ｋ＋１を生成する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、生成した状態遷移行列Φｋ，ｋ＋１を示す情報を出力する。
【００４９】
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、時刻ｋ＋１における距離及び距離変化率情報を入力する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報に基づいて、時刻ｋ＋１における観測値ベクトルｚｋ＋１を生成する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、生成した観測値ベクトルｚｋ＋１を示す情報を出力する。
【００５０】
なお、距離及び距離変化率情報の数が少ない場合には、観測更新過程を処理することができない。そのため、観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報の数を、所定の数（例えば２）と比較する。観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報の数が所定の数以上である場合のみ、観測値ベクトルｚｋ＋１を生成する。
観測値ベクトルｚｋ＋１を生成しない場合、観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、観測値ベクトルｚｋ＋１を生成しないことを示す情報を出力する。
【００５１】
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、時刻ｋにおける誤差共分散行列（推定）Ｐｋ＋を示す情報を、推定誤差情報として記憶している。
【００５２】
推定誤差予測部１３４は、上述した誤差共分散外挿処理を行う。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、推定誤差記憶部１３３が記憶した推定誤差情報を入力する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報を入力する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、誤差共分散行列（推定）Ｐｋ＋、状態遷移行列Φｋ，ｋ＋１から、時刻ｋ＋１における誤差共分散行列（予測）Ｐｋ＋１−を算出する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を出力する。
【００５３】
カルマンゲイン算出部１３５は、上述したカルマンゲイン算出処理を行う。
ただし、観測値生成部１３２が観測値ベクトルｚｋ＋１を生成しなかった場合、カルマンゲイン算出部１３５は、カルマンゲイン算出処理をしない。
観測値生成部１３２が出力した情報に基づいて、観測値生成部１３２が観測値ベクトルｚｋ＋１を生成したと判断した場合、カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、誤差共分散行列（予測）Ｐｋ＋１−から、カルマンゲインＫを算出する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、算出したカルマンゲインＫを示す情報を出力する。
【００５４】
推定誤差算出部１３６は、上述した誤差共分散更新処理を行う。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
【００５５】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成した場合、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、カルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報を入力する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、誤差共分散行列（予測）Ｐｋ＋１−、カルマンゲインＫから、時刻ｋ＋１における誤差共分散行列（推定）Ｐｋ＋１＋を算出する。
【００５６】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成しなかった場合、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した誤差共分散行列（予測）Ｐｋ＋１−を、そのまま誤差共分散行列（推定）Ｐｋ＋１＋とする。
【００５７】
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（推定）Ｐｋ＋１＋を示す情報を出力する。
【００５８】
推定誤差記憶部１３３は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した誤差共分散行列（推定）Ｐｋ＋１＋を示す情報を入力する。
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、入力した情報を推定誤差情報として記憶する。
【００５９】
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、時刻ｋにおける状態量ベクトル（推定）ｘｋ＋を示す情報を、計測誤差情報として記憶している。
【００６０】
計測誤差予測部１３８は、上述した状態方程式外挿処理を行う。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、計測誤差記憶部１３７が記憶した計測誤差情報を入力する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報を入力する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、状態量ベクトル（推定）ｘｋ＋、状態遷移行列Φｋ，ｋ＋１から、時刻ｋ＋１における状態量ベクトル（予測）ｘｋ＋１−を算出する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（予測）ｘｋ＋１−を示す情報を出力する。
【００６１】
計測誤差推定部１３９は、上述した状態方程式更新処理を行う。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、計測誤差予測部１３８が出力した状態量ベクトルｘｋ＋１−を示す情報を入力する。
【００６２】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成した場合、計測誤差推定部１３９は、観測値生成部１３２が出力した観測値ベクトルｚｋ＋１を示す情報を入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、カルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報を入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、状態量ベクトルｘｋ＋１−、観測値ベクトルｚｋ＋１、カルマンゲインＫから、時刻ｋ＋１における状態量ベクトル（推定）ｘｋ＋１＋を算出する。
【００６３】
観測値生成部１３２が観測値ベクトルｚｋ＋１を生成しなかった場合、計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した状態量ベクトル（予測）ｘｋ＋１−を、そのまま状態量ベクトル（推定）ｘｋ＋１＋とする。
【００６４】
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（推定）ｘｋ＋１＋を示す情報を出力する。
【００６５】
計測誤差記憶部１３７は、ＣＰＵ９１１などの処理装置を用いて、計測誤差推定部１３９が出力した状態量ベクトル（推定）ｘｋ＋１＋を示す情報を入力する。
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、入力した情報を、計測誤差情報として記憶する。
【００６６】
次に、動作について説明する。
【００６７】
図７は、この実施の形態における測位装置１００が現在位置を算出する測位処理の流れの一例を示すフローチャート図である。
【００６８】
Ｓ１１〜Ｓ１３の処理と、Ｓ２１〜Ｓ２５の処理とは、それぞれ独立して並列に動作する。
【００６９】
Ｓ１１において、ＧＰＳ受信機１１１は、ＧＰＳ衛星２００が発信したＧＰＳ信号を受信する。ＧＰＳ受信機１１１は、受信できるＧＰＳ信号が複数ある場合には、複数のＧＰＳ衛星２００が発信した複数のＧＰＳ信号をそれぞれ受信する。
【００７０】
Ｓ１２において、距離及び距離変化率観測部１１２は、Ｓ１１でＧＰＳ受信機１１１が受信したＧＰＳ信号に基づいて、ＧＰＳ衛星２００までの擬似距離及び擬似距離の変化率を算出する。
距離及び距離変化率観測部１１２は、算出した擬似距離及び擬似距離の変化率を示す情報を、距離及び距離変化率情報として出力する。
距離及び距離変化率観測部１１２は、ＧＰＳ受信機１１１が受信したＧＰＳ信号のうち、信頼できる擬似距離及び擬似距離の変化率を算出できるＧＰＳ信号についてのみ、距離及び距離変化率情報を出力する。
【００７１】
Ｓ１３において、観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、Ｓ１２で距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報を入力する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、入力した距離及び距離変化率情報の数が所定の数（例えば、２）以上である場合、観測値ベクトルｚｋ＋１を生成する。
観測値生成部１３２は、ＣＰＵ９１１などの処理装置を用いて、生成した観測値ベクトルｚｋ＋１を示す情報を出力する。
【００７２】
一方、Ｓ２１において、車速パルス発生器１２１は、速度を計測する。
車速パルス発生器１２１は、計測した速度を示す速度情報を出力する。
【００７３】
Ｓ２２において、ジャイロスコープ１２２は、角速度を計測する。
ジャイロスコープ１２２は、計測した角速度を示す角速度情報を出力する。
【００７４】
Ｓ２３において、遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２１で車速パルス発生器１２１が出力した速度情報と、Ｓ２２でジャイロスコープ１２２が出力した角速度情報とを入力する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、入力した速度情報と角速度情報とに基づいて、状態遷移行列Φｋ，ｋ＋１を生成する。
遷移行列生成部１３１は、ＣＰＵ９１１などの処理装置を用いて、生成した状態遷移行列Φｋ，ｋ＋１を示す情報を出力する。
【００７５】
Ｓ２４において、推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、推定誤差記憶部１３３が記憶した時刻ｋにおける誤差共分散行列（推定）Ｐｋ＋を示す推定誤差情報と、Ｓ２３で遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報とを入力する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における誤差共分散行列（予測）Ｐｋ＋１−＝Φｋ，ｋ＋１Ｐｋ＋Φｋ，ｋ＋１Ｔ＋Ｑｋ，ｋ＋１を算出する。
推定誤差予測部１３４は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を出力する。
【００７６】
Ｓ２５において、計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、計測誤差記憶部１３７が記憶した時刻ｋにおける状態量ベクトルｘｋ＋を示す計測誤差情報と、Ｓ２３で遷移行列生成部１３１が出力した状態遷移行列Φｋ，ｋ＋１を示す情報とを入力する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における状態量ベクトル（予測）ｘｋ＋１−＝Φｋ，ｋ＋１ｘｋ＋を算出する。
計測誤差予測部１３８は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（予測）ｘｋ＋１−を示す情報を出力する。
【００７７】
Ｓ３１において、観測更新可能であるか否かに基づいて、処理を分岐する。観測更新可能である場合とは、観測値生成部１３２が観測値ベクトルｚｋ＋１を生成した場合のことである。
観測更新可能な場合には、Ｓ３２へ進む。
観測更新できない場合には、Ｓ３５へ進む。
【００７８】
Ｓ３２において、カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２４で推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、カルマンゲインＫ＝Ｐｋ＋１−Ｈｋ＋１（Ｈｋ＋１ＴＰｋ＋１−Ｈｋ＋１＋Ｒｋ＋１）−１を算出する。
カルマンゲイン算出部１３５は、ＣＰＵ９１１などの処理装置を用いて、算出したカルマンゲインＫを示す情報を出力する。
【００７９】
Ｓ３３において、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２４で推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報と、Ｓ３２でカルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報とを入力する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における誤差共分散行列（推定）Ｐｋ＋１＋＝Ｐｋ＋１−−ＫＨｋ＋１ＴＰｋ＋１−を算出する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報を出力する。
推定誤差記憶部１３３は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報を入力する。
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、入力した推定誤差情報を記憶する。
【００８０】
Ｓ３４において、計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、Ｓ１３で観測値生成部１３２が出力した観測値ベクトルｚｋ＋１を示す情報と、Ｓ２５で計測誤差予測部１３８が出力した状態量ベクトル（予測）ｘｋ＋１−を示す情報と、Ｓ３２でカルマンゲイン算出部１３５が出力したカルマンゲインＫを示す情報とを入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した情報に基づいて、時刻ｋ＋１における状態量ベクトル（推定）ｘｋ＋１＋＝ｘｋ＋１−＋Ｋ（ｚｋ＋１−Ｈｋ＋１Ｔｘｋ＋１−）を算出する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、算出した状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報を出力する。
計測誤差記憶部１３７は、ＣＰＵ９１１などの処理装置を用いて、計測誤差推定部１３９が出力した状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報を入力する。
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、入力した計測誤差情報を記憶する。
その後、Ｓ４１へ進む。
【００８１】
一方、Ｓ３５において、推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２４で推定誤差予測部１３４が出力した誤差共分散行列（予測）Ｐｋ＋１−を示す情報を入力する。
推定誤差算出部１３６は、ＣＰＵ９１１などの処理装置を用いて、入力した情報をそのまま、誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報として出力する。
推定誤差記憶部１３３は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した誤差共分散行列（推定）Ｐｋ＋１＋を示す推定誤差情報を入力する。
推定誤差記憶部１３３は、ＲＡＭ９１４などの記憶装置を用いて、入力した推定誤差情報を記憶する。
【００８２】
Ｓ３６において、計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２５で計測誤差予測部１３８が出力した状態量ベクトル（予測）ｘｋ＋１−を示す情報を入力する。
計測誤差推定部１３９は、ＣＰＵ９１１などの処理装置を用いて、入力した情報をそのまま、状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報として出力する。
計測誤差記憶部１３７は、ＣＰＵ９１１などの処理装置を用いて、計測誤差推定部１３９が出力した状態量ベクトル（推定）ｘｋ＋１＋を示す計測誤差情報を入力する。
計測誤差記憶部１３７は、ＲＡＭ９１４などの記憶装置を用いて、入力した計測誤差情報を記憶する。
その後、Ｓ４１へ進む。
【００８３】
Ｓ４１において、センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、Ｓ２１で車速パルス発生器１２１が出力した速度情報と、Ｓ２２でジャイロスコープ１２２が出力した角速度情報と、Ｓ３４またはＳ３６で計測誤差推定部１３９が出力した計測誤差情報とを入力する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、入力した計測誤差情報に基づいて、入力した速度情報が示す速度と角速度情報が示す角速度とを補正する。
センサ補正部１４１は、ＣＰＵ９１１などの処理装置を用いて、補正した速度を示す補正速度情報と、補正した角速度を示す補正角速度情報とを出力する。
【００８４】
Ｓ４２において、位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、Ｓ４１でセンサ補正部１４１が出力した補正速度情報と、補正角速度情報とを入力する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、入力した補正速度情報と補正角速度情報とに基づいて、デッドレコニング処理を行い、現在位置を算出する。
位置算出部１４２は、ＣＰＵ９１１などの処理装置を用いて、算出した現在位置を示す情報を出力する。
【００８５】
Ｓ５１において、進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、Ｓ３３またはＳ３５で推定誤差算出部１３６が出力した推定誤差情報を入力する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差推定の進捗度を示す進捗度情報を出力する。
【００８６】
Ｓ５２において、信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、Ｓ５１で進捗度算出部１５１が出力した進捗度情報を入力する。
信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報に基づいて、初期化が終了したか否かを判定する。初期化が終了していればセンサ補正部１４１が出力した補正速度情報・補正角速度情報などの補正計測情報が信頼できるので、信頼性判定部１５２は、すなわち、センサ補正部１４１が出力した補正計測情報が信頼できるか否かを判定する。また、補正計測情報が信頼できれば位置算出部１４２が出力した位置情報が信頼できるので、信頼性判定部１５２は、すなわち、位置算出部１４２が出力した位置情報が信頼できるか否かを判定する。
【００８７】
なお、この例では、速度情報及び角速度情報を状態遷移行列Φの生成に使用し、距離及び距離変化率情報から観測値ベクトルｚを生成しているが、速度情報及び角速度情報を、観測値ベクトルの生成に使用してもよい。
しかし、ＧＰＳ信号の受信状態によっては、観測更新に必要な距離及び距離変化率情報が得られない場合がある。速度情報及び角速度情報を観測値ベクトルｚの生成に使用すると、観測更新に必要な距離及び距離変化率情報が得られず観測更新ができなかった場合、推定された計測誤差には、計測された速度情報や角速度情報が反映されないことになる。
この例のように、速度情報及び角速度情報を観測値ベクトルｚではなく、状態遷移行列Φの生成に使用すると、観測更新に必要な距離及び距離変化率情報が得られない場合でも、ある程度の確度をもって、計測誤差を推定できるので、好ましい。
【００８８】
また、この例では、ＧＰＳ信号から算出した擬似距離及び擬似距離の変化率に基づいて、観測値ベクトルｚを生成している（これを「密結合カルマンフィルタ方式」という）が、ＧＰＳ信号に基づいて位置や速度を算出し、算出した位置や速度に基づいて、観測値ベクトルｚを生成してもよい（これを「疎結合カルマンフィルタ方式」という）。
しかし、ＧＰＳ信号に基づいて位置や速度を算出するためには、３乃至４以上のＧＰＳ信号を受信する必要があるのに対し、観測更新に必要な距離及び距離変化率情報は２つ以上（２衛星以上）あればよいので、この例における方式のほうが、利用可能なＧＰＳ信号が少ない場合でも、観測更新が可能となり、好ましい。
【００８９】
また、この例では、速度情報及び角速度情報に基づくデッドレコニングによる位置算出を主とし、これをＧＰＳ信号に基づいて補正しているが、ＧＰＳ信号に基づく位置算出を主として、デッドレコニングに基づいて補正することとしてもよい。
【００９０】
次に、誤差推定の進捗度について説明する。
【００９１】
図８は、この実施の形態における進捗度算出部１５１及び信頼性判定部１５２を示す図である。
【００９２】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、推定誤差算出部１３６が出力した推定誤差情報を入力する。
推定誤差情報は、推定誤差算出部１３６が算出した時刻ｋ＋１における誤差共分散行列Ｐｋ＋１＋を示す情報である。
誤差共分散行列は、推定した状態量（この場合は計測誤差）と、真の状態量との差（推定誤差）を要素とするベクトルｐの外積の期待値〈ｐｐＴ〉である。
誤差共分散行列の対角要素は、対応する状態量の推定誤差の分散を示す。誤差共分散行列の非対角要素は、対応する２つの状態量の推定誤差の共分散を示し、２つの状態量の推定誤差の間に相関がなければ０となる。
したがって、誤差共分散行列の対角要素の値から、対応する状態量の推定誤差がどの程度あるかを知ることができる。
【００９３】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出すべき所定の状態量に対応する誤差共分散行列Ｐｋ＋１＋の対角要素の値（推定誤差の分散）Ｐｓｅｎｓｏｒを取得する。
【００９４】
推定した状態量が完全に真の状態量と一致し、推定誤差が０になれば、推定誤差の分散は０になる。しかし、実際には、推定した状態量が完全に真の状態量と一致することはないので、誤差推定が進むと、推定誤差の分散は０に近づくが０にはならない。
【００９５】
そこで、この実施の形態では、推定誤差の分散Ｐｓｅｎｓｏｒを、推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔと比較することにより、進捗度算出部１５１が誤差推定の進捗度を算出する。
推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔは、あらかじめ定めたものを、図示していない推定誤差初期値記憶部が、ＲＯＭ９１３などの記憶装置を用いて記憶しておく。推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔは、例えば、車速パルス発生器１２１の仕様書に記載の精度・誤差などに基づいて算出する。推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔは、観測開始時（時刻ｋ＝０）に推定誤差記憶部１３３が記憶している誤差共分散行列Ｐ０＋の要素である。
【００９６】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定の進捗度を算出すべき所定の状態量に対応する推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔ（誤差共分散行列の要素の初期値）を示す情報を入力する。
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、入力した情報が示す推定誤差の分散Ｐｓｅｎｓｏｒと、推定誤差の分散の初期値Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出する。
【００９７】
この実施の形態では、誤差推定の進捗度Ｌｃｏｎｖを、Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔと定義する。進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、誤差推定の進捗度Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出する。
この式は、推定誤差の分散Ｐｓｅｎｓｏｒが初期値Ｐｓｅｎｓｏｒ１ｓｔから比較してどこまで小さくなっているかを示すものであり、Ｐｓｅｎｓｏｒが初期値のままであれば推定誤差の進捗度（収束度合い）Ｌｃｏｎｖ＝０となる。逆に最大限に収束してＰｓｅｎｓｏｒ＝０となれば、Ｌｃｏｎｖ＝１となる。
したがって、誤差推定の進捗度Ｌｃｏｎｖは、０〜１の間の値をとり、大きいほど誤差推定が進んでいることを示す。
【００９８】
進捗度算出部１５１は、ＣＰＵ９１１などの処理装置を用いて、算出した誤差推定の進捗度Ｌｃｏｎｖを示す進捗度情報を出力する。信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力する。
【００９９】
上述したように、推定誤差の分散Ｐｓｅｎｓｏｒが完全に０になることはないので、誤差推定が進むと進捗度Ｌｃｏｎｖは１に近づくが、１にはならない。
そこで、信頼性判定部１５２は、進捗度Ｌｃｏｎｖを所定の閾値（例えば０．９５）と比較することにより、誤差推定の信頼性を判定する。
すなわち、信頼性判定部１５２は、ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖと所定の閾値とを比較して、進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、センサの初期化が終了したと判定する。
【０１００】
「センサの初期化」とは、すなわち、車速パルス発生器１２１、ジャイロスコープ１２２などの計測誤差が正しく推定できたことをいう。
観測開始時には、計測誤差がどれほどあるかわからない。その後、カルマンフィルタにより、速度情報や角速度情報などの計測情報と、受信したＧＰＳ信号に基づく距離及び距離変化率情報などの観測情報とから、それぞれの計測誤差が推定できる。
ひとたび正しい計測誤差が推定できれば、その後ＧＰＳ信号の受信状態が悪くなっても、計測誤差がわかっているので、速度情報や角速度情報などだけに基づいて位置を算出しても、大きく間違うことはなく、正確な位置が算出できる。
ＧＰＳ信号が受信できない状態が長く続くと、推定した計測誤差が真の計測誤差からすこしずつ離れていく場合もあるが、ＧＰＳ信号が受信できれば、すぐに真の計測誤差に追随することができるので、推定した計測誤差が真の計測誤差から大きく離れることはない。
したがって、計測開始後、最初に進捗度Ｌｃｏｎｖが所定の閾値を超えるまでの期間を、センサの初期化の期間と呼ぶ。
【０１０１】
次に、この実施の形態における測位装置１００を用いた実験結果について説明する。
【０１０２】
図９は、この実施の形態における測位装置１００による計測誤差推定の実験結果を示すグラフ図である。
図１０は、図９に示した実験において、進捗度算出部１５１が算出した誤差推定の進捗度を示すグラフ図である。
【０１０３】
図９及び図１０において、横軸は共に時刻を示し、一目盛は５０秒である。
図９において、縦軸は誤差推定部１３０が推定した計測誤差を示す。
一番上のグラフは、車速パルス発生器１２１のスケールファクタ誤差について推定した計測誤差である。
真ん中のグラフは、ジャイロスコープ１２２のスケールファクタ誤差について推定した計測誤差である。
一番下のグラフは、ジャイロスコープ１２２のバイアス誤差について推定した計測誤差である。
図１０において、縦軸は誤差推定の進捗度（収束度）を示す。
一番上のグラフは、車速パルス発生器１２１のスケールファクタ誤差についての誤差推定の進捗度である。
真ん中のグラフは、ジャイロスコープ１２２のスケールファクタ誤差についての誤差推定の進捗度である。
一番下のグラフは、ジャイロスコープ１２２のバイアス誤差についての誤差推定の進捗度である。
【０１０４】
図９において、車速パルス発生器１２１のスケールファクタ誤差及びジャイロスコープ１２２のスケールファクタ誤差は、時刻４．５４３付近で一定値に収束している。また、ジャイロスコープ１２２のバイアス誤差は、時刻４．５４４付近で一定値に収束している。
一方、図１０において、推定した計測誤差が一定値に収束するのとほぼ同じタイミングで、進捗度が１に近づいており、進捗度の定義が妥当であることがわかる。
【０１０５】
特に注目すべき点は、ジャイロスコープ１２２のバイアス誤差について推定した計測誤差は、階段状に変化しており、時刻４．５４３付近で、一定値に収束したかに見える。
しかし、この時点での進捗度は０．５程度であるから、センサの初期化が終了していないことがわかる。
【０１０６】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
情報を処理するＣＰＵ９１１などの処理装置と、物理量（速度・角速度）を計測するセンサ（車速パルス発生器１２１・ジャイロスコープ１２２）と、誤差推定部１３０と、進捗度算出部１５１とを有し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、観測量（擬似距離及び擬似距離の変化率）を示す情報（距離及び距離変化率情報）を観測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した観測情報（距離及び距離変化率情報）に基づいて、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）の計測誤差を推定し、
ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力し、
進捗度算出部１５１は、
ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した進捗度Ｌｃｏｎｖを示す情報を進捗度情報として出力することを特徴とする。
【０１０７】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、誤差推定部１３０が算出した推定誤差に基づいて、進捗度算出部１５１が誤差推定の進捗度Ｌｃｏｎｖを算出するので、誤差推定の進捗度を計算により見積もることができるという効果を奏する。
【０１０８】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
誤差推定部１３０が、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタを用いて計測誤差ｘｋ＋１＋を推定し、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタが算出する誤差共分散行列Ｐｋ＋１＋を示す情報を推定誤差情報として出力することを特徴とする。
【０１０９】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、カルマンフィルタを用いて計測誤差を推定する誤差推定部１３０が、計測誤差を推定するために算出する誤差共分散行列Ｐｋ＋１＋に基づいて、進捗度算出部１５１が誤差推定の進捗度Ｌｃｏｎｖを算出するので、進捗度算出のための計算量が少なく、ＣＰＵ９１１などの処理装置の資源を有効に活用できるという効果を奏する。
【０１１０】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
進捗度算出部１５１が、
入力した推定誤差情報が示す誤差共分散行列Ｐｋ＋１＋の要素のうち、少なくとも一つの要素について、ＣＰＵ９１１などの処理装置を用いて、上記要素の値（推定誤差の分散）Ｐｓｅｎｓｏｒと上記要素の初期値（推定誤差の分散の初期値）Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出することを特徴とする。
【０１１１】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、進捗度算出部１５１が、誤差共分散行列Ｐｋ＋１＋の要素のうち、少なくとも一つの要素について、上記要素の値と、上記要素の初期値とに基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出するので、誤差推定の進捗度を正しく見積もることができるという効果を奏する。
【０１１２】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
進捗度算出部１５１が、
ＣＰＵ９１１などの処理装置を用いて、上記要素の値（推定誤差の分散）Ｐｓｅｎｓｏｒと上記要素の初期値（推定誤差の分散の初期値）Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出することを特徴とする。
【０１１３】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、進捗度算出部１５１が、Ｌｃｏｎｖ＝１−Ｐｓｅｎｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出することにより誤差推定の進捗度を算出するので、簡単な計算で進捗度を算出でき、ＣＰＵ９１１などの処理装置の資源を有効に活用できるという効果を奏する。
【０１１４】
この実施の形態における測位装置１００（センサ誤差推定装置）は、更に、
初期化終了判定部（信頼性判定部１５２）を有し、
初期化終了判定部（信頼性判定部１５２）は、
ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖと所定の閾値とを比較して、進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、初期化が終了したと判定することを特徴とする。
【０１１５】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、進捗度算出部１５１が算出した誤差推定の進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、信頼性判定部１５２が初期化が終了したと判定するので、センサの初期化が終了したか否かを、簡単に判定できるという効果を奏する。
【０１１６】
この実施の形態における測位装置１００（センサ誤差推定装置）は、更に、
受信装置（ＧＰＳ受信機１１１）と、距離及び距離変化率観測部１１２とを有し、
受信装置（ＧＰＳ受信機１１１）は、
発信装置（ＧＰＳ衛星２００）が発信した電波（ＧＰＳ信号）を受信し、
距離及び距離変化率観測部１１２は、
受信装置（ＧＰＳ受信機１１１）が受信した電波（ＧＰＳ信号）に基づいて、発信装置（ＧＰＳ衛星２００）までの距離（擬似距離）及び距離変化率（擬似距離の変化率）を算出し、算出した距離（擬似距離）及び距離変化率（擬似距離の変化率）を示す情報を距離及び距離変化率情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報を、観測情報として入力することを特徴とする。
【０１１７】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、受信装置（ＧＰＳ受信機１１１）が受信した電波（ＧＰＳ信号）に基づいて距離及び距離変化率観測部１１２が算出した擬似距離及び擬似距離の変化率に基づいて、誤差推定部１３０が計測誤差を推定し、進捗度算出部１５１が進捗度を算出するので、推定したセンサの観測計測誤差が信頼できるか否かを判定できるという効果を奏する。
【０１１８】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
速度センサ（車速パルス発生器１２１）を有し、
速度センサ（車速パルス発生器１２１）は、
速度を計測し、計測した速度を示す情報を、速度情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、速度センサ（車速パルス発生器１２１）が出力した速度情報を、計測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、速度センサ（車速パルス発生器１２１）のスケールファクタ誤差を推定することを特徴とする。
【０１１９】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、速度センサ（車速パルス発生器１２１）が計測した速度について、誤差推定部１３０がスケールファクタ誤差を推定し、進捗度算出部１５１が進捗度を算出するので、推定したスケールファクタ誤差が信頼できるか否かを判定できるという効果を奏する。
【０１２０】
この実施の形態における測位装置１００（センサ誤差推定装置）は、
角速度センサ（ジャイロスコープ１２２）を有し、
角速度センサ（ジャイロスコープ１２２）は、
角速度を計測し、計測した角速度を示す情報を、角速度情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、角速度センサ（ジャイロスコープ１２２）が出力した角速度情報を、計測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、角速度センサ（ジャイロスコープ１２２）のスケールファクタ誤差及びバイアス誤差の少なくともいずれかを推定することを特徴とする。
【０１２１】
この実施の形態における測位装置１００（センサ誤差推定装置）によれば、角速度センサ（ジャイロスコープ１２２）が計測した角速度について、誤差推定部１３０がスケールファクタ誤差及びバイアス誤差の少なくともいずれかを推定し、進捗度算出部１５１が進捗度を算出するので、推定したスケールファクタ誤差またはバイアス誤差が信頼できるか否かを判定できるという効果を奏する。
【０１２２】
この実施の形態における測位装置１００（観測装置）は、
情報を処理するＣＰＵ９１１などの処理装置と、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）と、誤差推定部１３０と、進捗度算出部１５１と、センサ補正部１４１と、信頼性判定部１５２とを有し、
センサ（車速パルス発生器１２１・ジャイロスコープ１２２）は、
物理量（速度・角速度）を計測し、計測した物理量（速度・角速度）を示す情報を、計測情報（速度情報・角速度情報）として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、観測量（擬似距離及び擬似距離の変化率）を示す情報を観測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）が出力した計測情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した観測情報（距離及び距離変化率情報）と入力した計測情報（速度情報・角速度情報）とに基づいて、センサ（車速パルス発生器１２１・ジャイロスコープ１２２）の計測誤差を推定し、
ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差を示す情報を計測誤差情報として出力し、
ＣＰＵ９１１などの処理装置を用いて、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力し、
進捗度算出部１５１は、
ＣＰＵ９１１などの処理装置を用いて、誤差推定部１３０が出力した推定誤差情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力し、
センサ補正部１４１は、
ＣＰＵ９１１などの処理装置を用いて、計測情報（速度情報・角速度情報）と、誤差推定部１３０が出力した計測誤差情報とを入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した計測情報（速度情報・角速度情報）が示す物理量（速度・角速度）を、入力した計測誤差情報が示す計測誤差で補正し、
ＣＰＵ９１１などの処理装置を用いて、補正した物理量（速度・角速度）を示す情報（補正速度情報・補正角速度情報）を補正計測情報として出力し、
信頼性判定部１５２は、
ＣＰＵ９１１などの処理装置を用いて、進捗度算出部１５１が出力した進捗度情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖに基づいて、センサ補正部１４１が出力した補正計測情報が信頼できるか否かを判定することを特徴とする。
【０１２３】
この実施の形態における測位装置１００（観測装置）によれば、進捗度算出部１５１が算出した進捗度Ｌｃｏｎｖに基づいて、信頼性判定部１５２が補正計測情報の信頼性を判定するので、補正計測情報が信頼できるか否かを正しく判定できるという効果を奏する。
【０１２４】
この実施の形態における測位装置１００（観測装置）は、
誤差推定部１３０と、進捗度算出部１５１と、信頼性判定部１５２とを有し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタを用いて計測誤差ｘｋ＋１＋を推定し、
ＣＰＵ９１１などの処理装置を用いて、カルマンフィルタが算出する誤差共分散行列Ｐｋ＋１＋を示す情報を推定誤差情報として出力し、
進捗度算出部１５１は、
入力した推定誤差情報が示す誤差共分散行列Ｐｋ＋１＋の要素のうち、少なくとも一つの要素について、ＣＰＵ９１１などの処理装置を用いて、上記要素の値Ｐｓｅｎｓｏｒと上記要素の初期値Ｐｓｅｎｓｏｒ１ｓｔとに基づいて、誤差推定の進捗度Ｌｃｏｎｖ＝１−Ｐｓｅｎｓｏｒ／Ｐｓｅｎｓｏｒ１ｓｔを算出し、
信頼性判定部１５２は、
ＣＰＵ９１１などの処理装置を用いて、入力した進捗度情報が示す進捗度Ｌｃｏｎｖと所定の閾値とを比較して、進捗度Ｌｃｏｎｖが所定の閾値より大きい場合に、センサ補正部１４１が出力した補正計測情報が信頼できると判定することを特徴とする。
【０１２５】
この実施の形態における測位装置１００（観測装置）によれば、信頼性判定部１５２が、進捗度と所定の閾値とを比較して、補正計測情報の信頼性を判定するので、簡単な計算で信頼性を判定でき、ＣＰＵ９１１などの処理装置の資源を有効に活用できるという効果を奏する。
【０１２６】
この実施の形態におえる測位装置１００（観測装置）は、更に、
受信装置（ＧＰＳ受信機１１１）と、距離及び距離変化率観測部１１２と、速度センサ（車速パルス発生器１２１）と、角速度センサ（ジャイロスコープ１２２）と、位置算出部１４２とを有し、
受信装置（ＧＰＳ受信機１１１）は、
発信装置（ＧＰＳ衛星２００）が発信した電波（ＧＰＳ信号）を受信し、
距離及び距離変化率観測部１１２は、
受信装置（ＧＰＳ受信機１１１）が受信した電波（ＧＰＳ信号）に基づいて、発信装置（ＧＰＳ衛星２００）までの距離（擬似距離）及び距離変化率（擬似距離の変化率）を算出し、算出した距離（擬似距離）及び距離変化率（擬似距離の変化率）を示す情報を距離及び距離変化率情報として出力し、
速度センサ（車速パルス発生器１２１）は、
速度を計測し、計測した速度を示す情報を、速度情報として出力し、
各速度センサ（ジャイロスコープ１２２）は、
角速度を計測し、計測した角速度を示す情報を、角速度情報として出力し、
誤差推定部１３０は、
ＣＰＵ９１１などの処理装置を用いて、距離及び距離変化率観測部１１２が出力した距離及び距離変化率情報と、速度センサ（車速パルス発生器１２１）が出力した速度情報と、角速度センサ（ジャイロスコープ１２２）が出力した角速度情報とを、計測情報として入力し、
ＣＰＵ９１１などの処理装置を用いて、速度センサ（車速パルス発生器１２１）のスケールファクタ誤差と、角速度センサ（ジャイロスコープ１２２）のスケールファクタ誤差及びバイアス誤差とを推定し、
センサ補正部１４１は、
ＣＰＵ９１１などの処理装置を用いて、速度情報が示す速度と上記角速度情報が示す角速度とを、誤差推定部１３０が推定したスケールファクタ誤差及びバイアス誤差で補正し、
位置算出部１４２は、
ＣＰＵ９１１などの処理装置を用いて、センサ補正部１４１が出力した補正計測情報を入力し、
ＣＰＵ９１１などの処理装置を用いて、入力した補正計測情報に基づいて、現在位置を算出することを特徴とする。
【０１２７】
この実施の形態における測位装置１００（観測装置）によれば、センサ補正部１４１が補正した補正計測情報に基づいて、位置算出部１４２が位置を算出し、信頼性判定部１５２が信頼性を判定するので、位置算出部１４２が算出した現在位置が信頼できるか否かを判定できるという効果を奏する。
【０１２８】
この実施の形態における測位装置１００（センサ誤差推定装置）が計測誤差を推定する方法は、
情報を処理するＣＰＵ９１１などの処理装置と、物理量を計測するセンサ（車速パルス発生器１２１・ジャイロスコープ１２２）とを有するセンサ誤差推定装置（測位装置１００）が計測誤差を推定する方法において、
ＣＰＵ９１１などの処理装置が、観測量（擬似距離及び擬似距離の変化率）を示す情報（距離及び距離変化率情報）を観測情報として入力し、
ＣＰＵ９１１などの処理装置が、上記センサ（車速パルス発生器１２１・ジャイロスコープ１２２）が計測した物理量（速度・角速度）を示す情報（速度情報・角速度情報）を計測情報として入力し、
ＣＰＵ９１１などの処理装置が、入力した観測情報（距離及び距離変化率情報）と入力した計測情報とに基づいて、上記センサの計測誤差を推定し、
ＣＰＵ９１１などの処理装置が、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置が、算出した進捗度Ｌｃｏｎｖを示す情報を進捗度情報として出力することを特徴とする。
【０１２９】
この実施の形態における方法によれば、ＣＰＵ９１１などの処理装置が、センサの計測誤差を推定し、推定した計測誤差の推定誤差を算出し、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出するので、誤差推定の進捗度を計算により見積もることができるという効果を奏する。
【０１３０】
この実施の形態における測位装置１００（観測装置）が観測する方法は、
情報を処理するＣＰＵ９１１などの処理装置と、物理量を計測するセンサ（車速パルス発生器１２１・ジャイロスコープ１２２）とを有する観測装置（測位装置１００）が観測する方法において、
ＣＰＵ９１１などの処理装置が、観測量（擬似距離及び擬似距離の変化率）を示す情報（距離及び距離変化率情報）を観測情報として入力し、
ＣＰＵ９１１などの処理装置が、上記センサ（車速パルス発生器１２１・ジャイロスコープ１２２）が計測した物理量（速度・角速度）を示す情報（速度情報・角速度情報）を計測情報として入力し、
ＣＰＵ９１１などの処理装置が、入力した観測情報（距離及び距離変化率情報）と計測情報（速度情報・角速度情報）とに基づいて、上記センサ（車速パルス発生器１２１・ジャイロスコープ１２２）の計測誤差を推定し、
ＣＰＵ９１１などの処理装置が、推定した計測誤差の推定誤差を算出し、
ＣＰＵ９１１などの処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、
ＣＰＵ９１１などの処理装置が、入力した計測情報が示す物理量（速度・角速度）を、推定した計測誤差で補正し、
ＣＰＵ９１１などの処理装置が、補正した物理量（速度・角速度）を示す情報を補正計測情報として出力し、
ＣＰＵ９１１などの処理装置が、算出した誤差推定の進捗度Ｌｃｏｎｖに基づいて、出力した補正計測情報が信頼できるか否かを判定することを特徴とする。
【０１３１】
この実施の形態における方法によれば、ＣＰＵ９１１などの処理装置が、センサの計測誤差を推定し、推定した計測誤差の推定誤差を算出し、算出した推定誤差に基づいて、誤差推定の進捗度Ｌｃｏｎｖを算出し、算出した誤差推定の進捗度Ｌｃｏｎｖに基づいて、補正計測情報の信頼性を判定するので、補正計測情報の信頼性を正しく判定できるという効果を奏する。
【０１３２】
この実施の形態における測位装置１００は、情報を処理するＣＰＵ９１１などの処理装置を有するコンピュータをこの実施の形態における測位装置１００として機能させるプログラムを、上記コンピュータが実行することにより、実現することができる。
【０１３３】
この実施の形態におけるプログラムは、
情報を処理するＣＰＵ９１１などの処理装置を有するコンピュータを、この実施の形態における測位装置１００（センサ誤差推定装置）として機能させることを特徴とする。
【０１３４】
この実施の形態におけるプログラムによれば、誤差推定の進捗度を、コンピュータによる計算により見積もることができるという効果を奏する。
【０１３５】
この実施の形態におけるプログラムは、
情報を処理するＣＰＵ９１１などの処理装置を有するコンピュータを、この実施の形態における測位装置１００（観測装置）として機能させることを特徴とする。
【０１３６】
この実施の形態におけるプログラムによれば、補正計測情報の信頼性を、コンピュータによる計算により判定できるという効果を奏する。
【０１３７】
この実施の形態における測位装置１００（ＧＰＳ／センサ複合演算測位装置）は、センサ誤差推定計算の収束状況を計算により見積もることを特徴とする。
これにより、ＧＰＳ／センサ複合演算測位装置において、ジャイロ、車速パルス、加速度センサなどのセンサの誤差推定（初期化）の進行度を計算できる。
また、初期化の進行状況を見積もることができるので、現在のセンサデータの信頼度を推定でき、測位結果の信頼度を推定できる。
【図面の簡単な説明】
【０１３８】
【図１】実施の形態１における測位システム８００の全体構成の一例を示すシステム構成図。
【図２】実施の形態１における測位装置１００の外観の一例を示す図。
【図３】実施の形態１における測位装置１００のハードウェア資源の一例を示す図。
【図４】実施の形態１における測位装置１００の機能ブロックの一例を示すブロック構成図。
【図５】実施の形態１における誤差推定部１３０が計測誤差を推定するために用いるカルマンフィルタの動作を示す図。
【図６】実施の形態１における誤差推定部１３０の内部ブロックの構成を示す詳細ブロック図。
【図７】実施の形態１における測位装置１００が現在位置を算出する測位処理の流れの一例を示すフローチャート図。
【図８】実施の形態１における進捗度算出部１５１及び信頼性判定部１５２を示す図。
【図９】実施の形態１における測位装置１００による計測誤差推定の実験結果を示すグラフ図。
【図１０】図９に示した実験において、進捗度算出部１５１が算出した誤差推定の進捗度を示すグラフ図。
【符号の説明】
【０１３９】
１００ 測位装置、１１１ ＧＰＳ受信機、１１２ 距離及び距離変化率観測部、１２１ 車速パルス発生器、１２２ ジャイロスコープ、１３０ 誤差推定部、１３１ 遷移行列生成部、１３２ 観測値生成部、１３３ 推定誤差記憶部、１３４ 推定誤差予測部、１３５ カルマンゲイン算出部、１３６ 推定誤差算出部、１３７ 計測誤差記憶部、１３８ 計測誤差予測部、１３９ 計測誤差推定部、１４１ センサ補正部、１４２ 位置算出部、１５１ 進捗度算出部、１５２ 信頼性判定部、２００ ＧＰＳ衛星、８００ 測位システム、９０１ 表示装置、９０２ キーボード、９０３ マウス、９０４ ＦＤＤ、９０５ ＣＤＤ、９０６ プリンタ装置、９０７ スキャナ装置、９１０ システムユニット、９１１ ＣＰＵ、９１２ バス、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信装置、９２０ 磁気ディスク装置、９２１ ＯＳ、９２２ ウィンドウシステム、９２３ プログラム群、９２４ ファイル群、９３１ 電話器、９３２ ファクシミリ機、９４０ インターネット、９４１ ゲートウェイ、９４２ ＬＡＮ。
【特許請求の範囲】
【請求項１】
情報を処理する処理装置と、
物理量を計測するセンサと、
上記処理装置を用いて、観測量を示す情報を観測情報として入力し、上記処理装置を用いて、入力した観測情報に基づいて、上記センサの計測誤差を推定し、上記処理装置を用いて、推定した計測誤差の推定誤差を算出し、上記処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力する誤差推定部と、
上記処理装置を用いて、上記誤差推定部が出力した推定誤差情報を入力し、上記処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出し、上記処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力する進捗度算出部と、
を有することを特徴とするセンサ誤差推定装置。
【請求項２】
上記誤差推定部は、
上記処理装置を用いて、カルマンフィルタを用いて計測誤差を推定し、上記処理装置を用いて、カルマンフィルタが算出する誤差共分散行列を示す情報を推定誤差情報として出力する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項３】
上記進捗度算出部は、
入力した推定誤差情報が示す誤差共分散行列の要素のうち、少なくとも一つの要素について、上記処理装置を用いて、上記要素の値と上記要素の初期値とに基づいて、誤差推定の進捗度を算出する
ことを特徴とする請求項２に記載のセンサ誤差推定装置。
【請求項４】
上記進捗度算出部は、
上記処理装置を用いて、上記要素の値Ｐと上記要素の初期値Ｐ１ｓｔとに基づいて、誤差推定の進捗度Ｌ＝１−Ｐ／Ｐ１ｓｔを算出する
ことを特徴とする請求項３に記載のセンサ誤差推定装置。
【請求項５】
上記センサ誤差推定装置は、更に、
上記処理装置を用いて、上記進捗度算出部が出力した進捗度情報を入力し、上記処理装置を用いて、入力した進捗度情報が示す進捗度と所定の閾値とを比較して、上記進捗度が上記所定の閾値より大きい場合に、初期化が終了したと判定する初期化終了判定部
を有することを特徴とする請求項４に記載のセンサ誤差推定装置。
【請求項６】
上記センサ誤差推定装置は、更に、
発信装置が発信した電波を受信する受信装置と、
上記受信装置が受信した電波に基づいて、上記発信装置までの距離及び距離変化率を算出し、算出した距離及び距離変化率を示す情報を距離及び距離変化率情報として出力する距離及び距離変化率観測部と、
を有し、
上記誤差推定部は、
上記処理装置を用いて、上記距離及び距離変化率観測部が出力した距離及び距離変化率情報を、観測情報として入力する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項７】
上記センサは、
速度を計測し、計測した速度を示す情報を、速度情報として出力する速度センサを有し、 上記誤差推定部は、
上記処理装置を用いて、上記速度センサが出力した速度情報を、計測情報として入力し、上記処理装置を用いて、上記速度センサのスケールファクタ誤差を推定する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項８】
上記センサは、
角速度を計測し、計測した角速度を示す情報を、角速度情報として出力する角速度センサを有し、 上記誤差推定部は、
上記処理装置を用いて、上記角速度センサが出力した角速度情報を、計測情報として入力し、上記処理装置を用いて、上記角速度センサのスケールファクタ誤差及びバイアス誤差の少なくともいずれかを推定する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項９】
情報を処理する処理装置と、
物理量を計測し、計測した物理量を示す情報を計測情報として出力するセンサと、
上記処理装置を用いて、観測量を示す情報を観測情報として入力し、上記センサが出力した計測情報を入力し、上記処理装置を用いて、入力した観測情報と計測情報とに基づいて、上記センサの計測誤差を推定し、上記処理装置を用いて、推定した計測誤差を示す情報を計測誤差情報として出力し、上記処理装置を用いて、推定した計測誤差の推定誤差を算出し、上記処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力する誤差推定部と、
上記処理装置を用いて、上記誤差推定部が出力した推定誤差情報を入力し、上記処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出し、上記処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力する進捗度算出部と、
上記処理装置を用いて、上記センサが出力した計測情報と、上記誤差推定部が出力した計測誤差情報とを入力し、上記処理装置を用いて、入力した計測情報が示す物理量を、入力した計測誤差情報が示す計測誤差で補正し、上記処理装置を用いて、補正した物理量を示す情報を補正計測情報として出力するセンサ補正部と、
上記処理装置を用いて、上記進捗度算出部が出力した進捗度情報を入力し、上記処理装置を用いて、入力した進捗度情報が示す進捗度に基づいて、上記センサ補正部が出力した補正計測情報が信頼できるか否かを判定する信頼性判定部と、
を有することを特徴とする観測装置。
【請求項１０】
上記誤差推定部は、
上記処理装置を用いて、カルマンフィルタを用いて計測誤差を推定し、上記処理装置を用いて、カルマンフィルタが算出する誤差共分散行列を示す情報を推定誤差情報として出力し、
上記進捗度算出部は、
入力した推定誤差情報が示す誤差共分散行列の要素のうち、少なくとも一つの要素について、上記処理装置を用いて、上記要素の値Ｐと上記要素の初期値Ｐ１ｓｔとに基づいて、誤差推定の進捗度Ｌ＝１−Ｐ／Ｐ１ｓｔを算出し、
上記信頼性判定部は、
上記処理装置を用いて、入力した進捗度情報が示す進捗度と所定の閾値とを比較して、上記進捗度が上記所定の閾値より大きい場合に、上記センサ補正部が出力した補正計測情報が信頼できると判定する
ことを特徴とする請求項９に記載の観測装置。
【請求項１１】
上記観測装置は、更に、
発信装置が発信した電波を受信する受信装置と、
上記受信装置が受信した電波に基づいて、上記発信装置までの距離及び距離変化率を算出し、算出した距離及び距離変化率を示す情報を距離及び距離変化率情報として出力する距離及び距離変化率観測部を有し、
上記センサは、
速度を計測し、観測した速度を示す情報を、速度情報として出力する速度センサと、角速度を観測し、観測した角速度を示す情報を、角速度情報として出力する角速度センサとを有し、
上記誤差推定部は、
上記処理装置を用いて、上記距離及び距離変化率観測部が出力した距離及び距離変化率情報を観測情報として入力し、上記速度センサが出力した速度情報と、上記角速度センサが出力した角速度情報とを、計測情報として入力し、上記処理装置を用いて、上記速度センサのスケールファクタ誤差と、上記角速度センサのスケールファクタ誤差及びバイアス誤差とを推定し、
上記センサ補正部は、
上記処理装置を用いて、上記速度情報が示す速度と上記角速度情報が示す角速度とを、上記誤差推定部が推定したスケールファクタ誤差及びバイアス誤差で補正し、
上記観測装置は、更に、
上記処理装置を用いて、上記センサ補正部が出力した補正計測情報を入力し、上記処理装置を用いて、入力した補正計測情報に基づいて、現在位置を算出する位置算出部を有する
ことを特徴とする請求項１０に記載の観測装置。
【請求項１２】
情報を処理する処理装置と、物理量を計測するセンサとを有するセンサ誤差推定装置が上記センサの計測誤差を推定する方法において、
上記処理装置が、観測量を示す情報を観測情報として入力し、
上記処理装置が、入力した観測情報に基づいて、上記センサの計測誤差を推定し、
上記処理装置が、推定した計測誤差の推定誤差を算出し、
上記処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度を算出し、
上記処理装置が、算出した進捗度を示す情報を進捗度情報として出力する
ことを特徴とする方法。
【請求項１３】
情報を処理する処理装置と、物理量を計測するセンサとを有する観測装置が観測する方法において、
上記処理装置が、観測量を示す情報を観測情報として入力し、
上記処理装置が、上記センサが計測した物理量を示す情報を計測情報として入力し、
上記処理装置が、入力した観測情報と入力した計測情報とに基づいて、上記センサの計測誤差を推定し、
上記処理装置が、推定した計測誤差の推定誤差を算出し、
上記処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度を算出し、
上記処理装置が、入力した計測情報が示す物理量を、推定した計測誤差で補正し、
上記処理装置が、補正した物理量を示す情報を補正計測情報として出力し、
上記処理装置が、算出した誤差推定の進捗度に基づいて、出力した補正計測情報が信頼できるか否かを判定する
ことを特徴とする方法。
【請求項１４】
情報を処理する処理装置を有するコンピュータを、請求項１に記載のセンサ誤差推定装置として機能させることを特徴とするプログラム。
【請求項１５】
情報を処理する処理装置を有するコンピュータを、請求項９に記載の観測装置として機能させることを特徴とするプログラム。
【請求項１】
情報を処理する処理装置と、
物理量を計測するセンサと、
上記処理装置を用いて、観測量を示す情報を観測情報として入力し、上記処理装置を用いて、入力した観測情報に基づいて、上記センサの計測誤差を推定し、上記処理装置を用いて、推定した計測誤差の推定誤差を算出し、上記処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力する誤差推定部と、
上記処理装置を用いて、上記誤差推定部が出力した推定誤差情報を入力し、上記処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出し、上記処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力する進捗度算出部と、
を有することを特徴とするセンサ誤差推定装置。
【請求項２】
上記誤差推定部は、
上記処理装置を用いて、カルマンフィルタを用いて計測誤差を推定し、上記処理装置を用いて、カルマンフィルタが算出する誤差共分散行列を示す情報を推定誤差情報として出力する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項３】
上記進捗度算出部は、
入力した推定誤差情報が示す誤差共分散行列の要素のうち、少なくとも一つの要素について、上記処理装置を用いて、上記要素の値と上記要素の初期値とに基づいて、誤差推定の進捗度を算出する
ことを特徴とする請求項２に記載のセンサ誤差推定装置。
【請求項４】
上記進捗度算出部は、
上記処理装置を用いて、上記要素の値Ｐと上記要素の初期値Ｐ１ｓｔとに基づいて、誤差推定の進捗度Ｌ＝１−Ｐ／Ｐ１ｓｔを算出する
ことを特徴とする請求項３に記載のセンサ誤差推定装置。
【請求項５】
上記センサ誤差推定装置は、更に、
上記処理装置を用いて、上記進捗度算出部が出力した進捗度情報を入力し、上記処理装置を用いて、入力した進捗度情報が示す進捗度と所定の閾値とを比較して、上記進捗度が上記所定の閾値より大きい場合に、初期化が終了したと判定する初期化終了判定部
を有することを特徴とする請求項４に記載のセンサ誤差推定装置。
【請求項６】
上記センサ誤差推定装置は、更に、
発信装置が発信した電波を受信する受信装置と、
上記受信装置が受信した電波に基づいて、上記発信装置までの距離及び距離変化率を算出し、算出した距離及び距離変化率を示す情報を距離及び距離変化率情報として出力する距離及び距離変化率観測部と、
を有し、
上記誤差推定部は、
上記処理装置を用いて、上記距離及び距離変化率観測部が出力した距離及び距離変化率情報を、観測情報として入力する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項７】
上記センサは、
速度を計測し、計測した速度を示す情報を、速度情報として出力する速度センサを有し、 上記誤差推定部は、
上記処理装置を用いて、上記速度センサが出力した速度情報を、計測情報として入力し、上記処理装置を用いて、上記速度センサのスケールファクタ誤差を推定する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項８】
上記センサは、
角速度を計測し、計測した角速度を示す情報を、角速度情報として出力する角速度センサを有し、 上記誤差推定部は、
上記処理装置を用いて、上記角速度センサが出力した角速度情報を、計測情報として入力し、上記処理装置を用いて、上記角速度センサのスケールファクタ誤差及びバイアス誤差の少なくともいずれかを推定する
ことを特徴とする請求項１に記載のセンサ誤差推定装置。
【請求項９】
情報を処理する処理装置と、
物理量を計測し、計測した物理量を示す情報を計測情報として出力するセンサと、
上記処理装置を用いて、観測量を示す情報を観測情報として入力し、上記センサが出力した計測情報を入力し、上記処理装置を用いて、入力した観測情報と計測情報とに基づいて、上記センサの計測誤差を推定し、上記処理装置を用いて、推定した計測誤差を示す情報を計測誤差情報として出力し、上記処理装置を用いて、推定した計測誤差の推定誤差を算出し、上記処理装置を用いて、算出した推定誤差を示す情報を推定誤差情報として出力する誤差推定部と、
上記処理装置を用いて、上記誤差推定部が出力した推定誤差情報を入力し、上記処理装置を用いて、入力した推定誤差情報に基づいて、誤差推定の進捗度を算出し、上記処理装置を用いて、算出した進捗度を示す情報を進捗度情報として出力する進捗度算出部と、
上記処理装置を用いて、上記センサが出力した計測情報と、上記誤差推定部が出力した計測誤差情報とを入力し、上記処理装置を用いて、入力した計測情報が示す物理量を、入力した計測誤差情報が示す計測誤差で補正し、上記処理装置を用いて、補正した物理量を示す情報を補正計測情報として出力するセンサ補正部と、
上記処理装置を用いて、上記進捗度算出部が出力した進捗度情報を入力し、上記処理装置を用いて、入力した進捗度情報が示す進捗度に基づいて、上記センサ補正部が出力した補正計測情報が信頼できるか否かを判定する信頼性判定部と、
を有することを特徴とする観測装置。
【請求項１０】
上記誤差推定部は、
上記処理装置を用いて、カルマンフィルタを用いて計測誤差を推定し、上記処理装置を用いて、カルマンフィルタが算出する誤差共分散行列を示す情報を推定誤差情報として出力し、
上記進捗度算出部は、
入力した推定誤差情報が示す誤差共分散行列の要素のうち、少なくとも一つの要素について、上記処理装置を用いて、上記要素の値Ｐと上記要素の初期値Ｐ１ｓｔとに基づいて、誤差推定の進捗度Ｌ＝１−Ｐ／Ｐ１ｓｔを算出し、
上記信頼性判定部は、
上記処理装置を用いて、入力した進捗度情報が示す進捗度と所定の閾値とを比較して、上記進捗度が上記所定の閾値より大きい場合に、上記センサ補正部が出力した補正計測情報が信頼できると判定する
ことを特徴とする請求項９に記載の観測装置。
【請求項１１】
上記観測装置は、更に、
発信装置が発信した電波を受信する受信装置と、
上記受信装置が受信した電波に基づいて、上記発信装置までの距離及び距離変化率を算出し、算出した距離及び距離変化率を示す情報を距離及び距離変化率情報として出力する距離及び距離変化率観測部を有し、
上記センサは、
速度を計測し、観測した速度を示す情報を、速度情報として出力する速度センサと、角速度を観測し、観測した角速度を示す情報を、角速度情報として出力する角速度センサとを有し、
上記誤差推定部は、
上記処理装置を用いて、上記距離及び距離変化率観測部が出力した距離及び距離変化率情報を観測情報として入力し、上記速度センサが出力した速度情報と、上記角速度センサが出力した角速度情報とを、計測情報として入力し、上記処理装置を用いて、上記速度センサのスケールファクタ誤差と、上記角速度センサのスケールファクタ誤差及びバイアス誤差とを推定し、
上記センサ補正部は、
上記処理装置を用いて、上記速度情報が示す速度と上記角速度情報が示す角速度とを、上記誤差推定部が推定したスケールファクタ誤差及びバイアス誤差で補正し、
上記観測装置は、更に、
上記処理装置を用いて、上記センサ補正部が出力した補正計測情報を入力し、上記処理装置を用いて、入力した補正計測情報に基づいて、現在位置を算出する位置算出部を有する
ことを特徴とする請求項１０に記載の観測装置。
【請求項１２】
情報を処理する処理装置と、物理量を計測するセンサとを有するセンサ誤差推定装置が上記センサの計測誤差を推定する方法において、
上記処理装置が、観測量を示す情報を観測情報として入力し、
上記処理装置が、入力した観測情報に基づいて、上記センサの計測誤差を推定し、
上記処理装置が、推定した計測誤差の推定誤差を算出し、
上記処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度を算出し、
上記処理装置が、算出した進捗度を示す情報を進捗度情報として出力する
ことを特徴とする方法。
【請求項１３】
情報を処理する処理装置と、物理量を計測するセンサとを有する観測装置が観測する方法において、
上記処理装置が、観測量を示す情報を観測情報として入力し、
上記処理装置が、上記センサが計測した物理量を示す情報を計測情報として入力し、
上記処理装置が、入力した観測情報と入力した計測情報とに基づいて、上記センサの計測誤差を推定し、
上記処理装置が、推定した計測誤差の推定誤差を算出し、
上記処理装置が、算出した推定誤差に基づいて、誤差推定の進捗度を算出し、
上記処理装置が、入力した計測情報が示す物理量を、推定した計測誤差で補正し、
上記処理装置が、補正した物理量を示す情報を補正計測情報として出力し、
上記処理装置が、算出した誤差推定の進捗度に基づいて、出力した補正計測情報が信頼できるか否かを判定する
ことを特徴とする方法。
【請求項１４】
情報を処理する処理装置を有するコンピュータを、請求項１に記載のセンサ誤差推定装置として機能させることを特徴とするプログラム。
【請求項１５】
情報を処理する処理装置を有するコンピュータを、請求項９に記載の観測装置として機能させることを特徴とするプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２００８−１７５７２１（Ｐ２００８−１７５７２１Ａ）
【公開日】平成２０年７月３１日（２００８．７．３１）
【国際特許分類】
【出願番号】特願２００７−１０１６３（Ｐ２００７−１０１６３）
【出願日】平成１９年１月１９日（２００７．１．１９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２０年７月３１日（２００８．７．３１）
【国際特許分類】
【出願日】平成１９年１月１９日（２００７．１．１９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]