位置算出方法及び位置算出装置
【課題】慣性測位用ユニット及び衛星測位用ユニットそれぞれの計測結果を併用して、位置をより正確に算出するための手法の提案。
【解決手段】位置算出装置１において、移動体に設置された慣性測位用ユニット２の計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理が第１の演算処理部５により実行される。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニット３の計測結果とを用いて、移動体の位置を演算する第２の演算処理が第２の演算処理部７により実行される。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数が演算係数調整部９により調整される。
【解決手段】位置算出装置１において、移動体に設置された慣性測位用ユニット２の計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理が第１の演算処理部５により実行される。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニット３の計測結果とを用いて、移動体の位置を演算する第２の演算処理が第２の演算処理部７により実行される。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数が演算係数調整部９により調整される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、慣性測位用ユニット及び衛星測位用ユニットそれぞれの計測結果を併用した位置算出方法等に関する。
【背景技術】
【０００２】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行う慣性航法システム（以下、「ＩＮＳ（Inertial Navigation System）」と称する。）も考案されている。
【０００３】
ＩＮＳでは、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、ＩＮＳ演算結果を、ＧＰＳ（Global Positioning System）を利用して補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１０／００１９９６３号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ＧＰＳを利用してＩＮＳ演算結果を補正する技術は、ＧＰＳ演算結果が正しいことを前提としている。特許文献１に開示された技術も同様である。しかし、ＧＰＳ演算結果は、ＧＰＳ衛星から受信したＧＰＳ衛星信号の信号強度、受信環境、ＧＰＳ衛星の天空配置、マルチパスといった種々の要因により、演算結果の精度が低下する場合がある。
【０００６】
ＧＰＳ演算結果の精度が低下しているにも関わらず、特許文献１の技術のようにＧＰＳ演算結果を用いてＩＮＳ演算結果を補正してしまうと、位置算出の正確性が低下するという問題があった。
【０００７】
本発明は上述した課題に鑑みて為されたものであり、衛星測位用ユニット及び慣性測位用ユニットそれぞれの計測結果を併用して、位置をより正確に算出するための手法を提案することを目的とする。
【課題を解決するための手段】
【０００８】
以上の課題を解決するための第１の形態は、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行することと、前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行することと、前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整することと、を含む位置算出方法である。
【０００９】
また、他の形態として、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行する第１の演算処理部と、前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行する第２の演算処理部と、前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整する調整部と、を備えた位置算出装置を構成してもよい。
【００１０】
この第１の形態等によれば、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理を実行する。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニットの計測結果とを用いて、移動体の位置を演算する第２の演算処理を実行する。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数を調整する。
【００１１】
演算処理には、慣性測位用ユニットの計測結果を用いた第１の演算処理と、第１の演算処理の結果及び衛星測位用ユニットの計測結果を用いた第２の演算処理とがある。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数を調整することで、第１の演算処理における位置算出の正確性を向上させることができる。
【００１２】
また、第２の形態として、第１の形態の位置算出方法において、前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、前記第１の演算処理は、演算した第１の演算位置に内在する第１の位置誤差を推定する所定の誤差推定演算を含み、前記調整することは、前記第１の位置誤差と、前記第２の演算処理で求めた第２の演算位置とを用いて、前記慣性測位位置に内在する慣性測位誤差を推定することと、前記慣性測位誤差を用いて前記演算係数を調整することと、を含む、位置算出方法を構成することとしてもよい。
【００１３】
この第２の形態によれば、所定の誤差推定演算を行って、第１の演算処理で演算した第１の演算位置に内在する第１の位置誤差を推定する。そして、第１の位置誤差と、第２の演算処理で求めた第２の演算位置とを用いて、慣性測位用ユニットの計測結果に含まれる慣性測位位置に内在する慣性測位誤差を推定する。第１の位置誤差と併せて第２の演算位置を用いることで、慣性測位誤差を適切に推定することができる。そして、当該慣性測位誤差を用いて第１の演算処理に係る演算係数を調整することで、第１の演算処理における位置算出の正確性を向上させることができる。
【００１４】
また、第３の形態として、第２の形態の位置算出方法において、前記推定することは、前記慣性測位位置と前記第２の演算位置との差分を算出することと、前記第１の位置誤差と前記差分とを平均処理して前記慣性測位誤差を算出することと、を含む、位置算出方法を構成することとしてもよい。
【００１５】
この第３の形態によれば、慣性測位位置と第２の演算位置との差分を算出する。そして、第１の位置誤差と当該差分とを平均処理することで、慣性測位誤差を適切に推定することができる。
【００１６】
また、第４の形態として、第３の形態の位置算出方法において、前記平均処理は、測位環境と前記衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、前記第１の位置誤差と前記差分との重みを設定して加重平均する処理である、位置算出方法を構成することとしてもよい。
【００１７】
この第４の形態によれば、平均処理において、測位環境と衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、第１の位置誤差と差分との重みを設定して加重平均する。これにより、測位環境や衛星測位用ユニットの計測結果の信頼性に基づいて、慣性測位誤差をより正確に推定することができる。
【００１８】
また、第５の形態として、第１〜第４の何れかの形態の位置算出方法において、前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、前記第１の演算処理は、前記慣性測位位置を入力とするカルマンフィルター処理である、位置算出方法を構成することとしてもよい。
【００１９】
この第５の形態によれば、第１の演算処理として慣性測位位置を入力とするカルマンフィルター処理を行うことで、移動体の位置等を簡易且つ適切に求めることができる。
【００２０】
また、第６の形態として、第５の形態の位置算出方法において、前記衛星測位用ユニットの計測結果には速度が含まれ、前記カルマンフィルター処理は、前記速度を観測量とする処理である、位置算出方法を構成してもよい。
【００２１】
この第６の形態によれば、第５の形態のカルマンフィルター処理において、さらに衛星測位用ユニットの計測結果に含まれる速度を観測量とすることで、移動体の位置等をより正確に求めることができる。
【００２２】
また、第７の形態として、第１〜第６の何れかの形態の位置算出方法において、前記衛星測位用ユニットの計測結果には衛星測位位置が含まれ、前記第２の演算処理は、前記第１の演算処理の結果を入力とし、前記衛星測位位置を観測量とするカルマンフィルター処理である、位置算出方法を構成することとしてもよい。
【００２３】
この第７の形態によれば、第２の演算処理として、第１の演算処理の結果を入力とし、衛星測位用ユニットの計測結果に含まれる衛星測位位置を観測量とするカルマンフィルター処理を実行することで、移動体の位置を簡易且つ適切に求めることができる。
【図面の簡単な説明】
【００２４】
【図１】位置算出装置の構成図。
【図２】第１の位置算出装置の構成図。
【図３】第１のカルマンフィルター処理の入出力データの説明図。
【図４】第１の重み設定条件の説明図。
【図５】第２の重み設定条件の説明図。
【図６】位置算出を行った実験結果の一例を示す図。
【図７】位置算出を行った実験結果の一例を示す図。
【図８】ナビゲーションシステムのシステム構成図。
【図９】カーナビゲーション装置の機能構成を示すブロック図。
【図１０】ナビゲーション処理の流れを示すフローチャート。
【図１１】ＩＮＳ演算誤差推定処理の流れを示すフローチャート。
【発明を実施するための形態】
【００２５】
１．原理
１−１．構成
図１は、本実施形態における位置算出装置１の主要構成図である。位置算出装置１は、移動体に備えられ、当該移動体の位置を算出する装置（システム）である。移動体は、自動車やオートバイ、自転車、船、電車といった物の他、人間自体であってもよい。人間が位置算出装置１を携帯し、人間自身が位置算出装置１を備えることとしてもよい。
【００２６】
本明細書で参照する図面では、ユニット（モジュール）を二重線で図示し、ユニットの計測結果を利用して演算処理を行う処理ブロックを一重線で図示することで、両者を区別する。
【００２７】
位置算出装置１には、ユニット（モジュール）として、慣性測位用ユニット２と、衛星測位用ユニット３とが含まれる。また、位置算出装置１は、主要な処理ブロックとして、第１の演算処理部５と、第２の演算処理部７と、演算係数調整部９とを有する。なお、ユニットと処理ブロックとで位置算出装置１を構成してもよいし、ユニットを除外して処理ブロックのみで位置算出装置１を構成してもよい。
【００２８】
慣性測位用ユニット２は、慣性航法を利用して測位を行うためのユニットである。慣性測位用ユニット２は、加速度センサーやジャイロセンサー等の慣性センサーや、慣性センサーをパッケージ化した慣性計測ユニット（ＩＭＵ（Inertial Measurement Unit））、慣性計測ユニット及び演算処理部をパッケージ化した慣性航法システム（ＩＮＳ（Inertial Navigation System））等を適用したユニットである。
【００２９】
衛星測位用ユニット３は、衛星測位システムを利用して測位を行うためのユニットであり、例えば衛星測位システムの一種であるＧＰＳ（Global Positioning System）を適用可能である。
【００３０】
第１の演算処理部５は、慣性測位用ユニット２の計測結果と、所与の観測量とを併用して、予め定められた第１の演算処理を行って、少なくとも移動体の位置を算出する。本実施形態において、第１の演算処理に好適な適用例は、カルマンフィルター処理である。
【００３１】
第２の演算処理部７は、第１の演算処理部５の演算結果である第１の演算結果と、衛星測位用ユニット３の計測結果とを用いて、予め定められた第２の演算処理を行って、移動体の位置を算出する。本実施形態において、第２の演算処理に好適な適用例は、カルマンフィルター処理やシグマポイントフィルター処理、回帰フィルター処理である。
【００３２】
演算係数調整部９は、第１の演算処理部５から入力した第１の演算結果と、第２の演算処理部７から入力した第２の演算結果とを用いて、第１の演算処理に係る演算係数を調整する。第１の演算処理部５は、演算係数調整部９により調整された演算係数を用いて第１の演算処理を行う。
【００３３】
図２は、図１の位置算出装置１を適用した第１の位置算出装置１Ａの構成図である。第１の位置算出装置１Ａは、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａと、第１のカルマンフィルター処理部５Ａと、第２のカルマンフィルター処理部７Ａと、カルマンフィルター係数調整部９Ａとを有する。
【００３４】
第１の位置算出装置１Ａは、慣性測位用ユニット２としてＩＮＳユニット２Ａを適用し、衛星測位用ユニット３としてＧＰＳユニット３Ａを適用したシステムである。また、第１及び第２の演算処理部５，７として、第１及び第２のカルマンフィルター処理部５Ａ，７Ａをそれぞれ適用したシステムである。
【００３５】
ＩＮＳユニット２Ａは、ＩＭＵがローカル座標系で計測した加速度や角速度等のＩＮＳメジャメント情報を出力可能に構成されている。また、ＩＮＳユニット２Ａは、ＩＮＳメジャメント情報を用いた慣性航法演算を行って、移動体の位置や速度、姿勢角を演算して出力可能に構成されている。本実施形態では、ＩＮＳユニット２Ａは、ＩＮＳ演算位置（慣性測位位置）、ＩＮＳ演算速度及びＩＮＳ演算姿勢角を絶対座標系で演算して出力する。
【００３６】
ＧＰＳユニット３Ａは、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を受信して、コード位相やドップラー周波数、擬似距離、擬似距離変化率等のＧＰＳメジャメント情報を計測して出力可能に構成されている。また、ＧＰＳユニット３Ａは、ＧＰＳメジャメント情報を用いたＧＰＳ演算を行って、移動体の位置や速度を演算して出力可能に構成されている。本実施形態では、ＧＰＳユニット３Ａは、ＧＰＳ演算位置（衛星測位位置）及びＧＰＳ演算速度を絶対座標系で演算して出力する。
【００３７】
ローカル座標系は、ＩＮＳユニット２Ａが有する慣性センサーに対応付けられたローカルな座標系（センサー座標系）である。それに対し、絶対座標系は、移動体の移動空間を定める座標系である。例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、東北上座標系として知られるＥＮＵ（East North Up）座標系、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系といった座標系を適用可能である。
【００３８】
以下の数式では、ＩＮＳユニット２Ａは、移動体の位置、速度及び姿勢角をＥＮＵ座標系で演算して出力し、ＧＰＳユニット３Ａは、移動体の位置及び速度をＥＣＥＦ座標系で演算して出力するものとして説明する。また、第１及び第２のカルマンフィルター処理では、ＥＮＵ座標系で演算するものとして説明する。また、加速度や速度といった諸量は、実際には大きさ及び方向を持ったベクトルとして表される。しかし、本明細書では、ベクトルの文言を省略して、簡単に加速度や速度として説明する。
【００３９】
第１のカルマンフィルター処理部５Ａは、カルマンフィルターの理論に基づく演算処理を行って、移動体の位置や速度、姿勢角等を演算する。例えば、ＩＮＳユニット２Ａから入力したＩＮＳ演算位置、ＩＮＳ演算速度及びＩＮＳ演算姿勢角を入力Ｕ（制御入力）とする。また、ＧＰＳユニット３Ａから入力したＧＰＳ演算速度や、移動体の運動モデルに基づき定められる所与の制約条件を観測量Ｚとする。そして、カルマンフィルターの予測演算（時刻更新）及び補正演算（観測更新）を行って、状態推定値を求める。
【００４０】
第１のカルマンフィルター処理では、推定対象とする移動体の状態“Ｘ”を、例えば次式（１）及び（２）のように設定する。
【数１】
【数２】
【００４１】
式（１）で表される状態“Ｘ1”において、“（δＶE，δＶN，δＶU）”は、ＥＮＵ座標系で演算したＩＮＳ演算速度の誤差である。“（Ψx，Ψy，Ψz）”は、ローカル座標系で演算したＩＮＳ演算姿勢角の誤差である。“（ｂax，ｂay，ｂaz）”は、ローカル座標系で計測した加速度のバイアスである。“（ｂgx，ｂgy，ｂgz）”は、ローカル座標系で計測した角速度のバイアスである。また、“ｄ”は、ＧＰＳユニット３Ａの内部クロックのドリフト（クロックドリフト）である。
【００４２】
また、式（２）で表される状態“Ｘ2”において、“（δＰE，δＰN，δＰU）”は、ＥＮＵ座標系で演算したＩＮＳ演算位置の誤差である。
【００４３】
式（１）及び（２）から分かるように、本実施形態における第１のカルマンフィルター処理は、ＩＮＳユニット２Ａの演算結果に含まれる誤差を推定する誤差推定型のカルマンフィルター処理である。つまり、ＩＮＳユニット２Ａの演算結果に含まれる誤差を、状態“Ｘ1”及び“Ｘ2”として推定する。
【００４４】
また、第１のカルマンフィルター処理では、状態“Ｘ1”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ11”と、状態“Ｘ2”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ22”と、状態“Ｘ1”の各成分に対する状態“Ｘ2”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ21”と、状態“Ｘ2”の各成分に対する状態“Ｘ1”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ12”と、を併せて演算する。
【００４５】
第１のカルマンフィルター処理の補正演算では、ＧＰＳユニット３Ａにより演算された速度（以下、「ＧＰＳ演算速度」と称す。）を観測量“Ｚ”として、予測演算で予測された状態“Ｘ”を補正する。
【００４６】
具体的には、例えば次式（３）で与えられる観測量“Ｚ”を用いて補正演算を行う。
【数３】
但し、“（ＶE，ＶN，ＶU）INS”は、ＩＮＳユニット２ＡがＥＮＵ座標系で演算したＩＮＳ演算速度である。また、“（ＶX，ＶY，ＶZ）GPS”は、ＧＰＳユニット３ＡがＥＣＥＦ座標系で演算したＧＰＳ演算速度である。また、“ＣECEFENU”は、ＥＣＥＦ座標系からＥＮＵ座標系への座標変換行列である。
【００４７】
さらに、第１のカルマンフィルター処理では、ＧＰＳ演算速度とは別に、移動体の運動モデルに基づく制約条件を観測量“Ｚ”として適用可能に構成されている。具体的には、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との２種類の速度制約条件を適用可能である。
【００４８】
停止時速度制約条件（第１の制約条件）は、移動体の停止時に適用可能な制約条件である。移動体が停止しているのであれば、理想的には移動体の速度はゼロである。従って、移動体が停止していると判定した場合は「移動体の各軸の速度成分＝０」を観測量“Ｚ”として与えることができる。
【００４９】
移動時速度制約条件（第２の制約条件）は、移動体の移動時に適用可能な制約条件である。例えば、移動体として四輪自動車を想定した場合、通常、四輪自動車はジャンプや横滑りすることはないと仮定することができる。従って、移動体が移動していると判定した場合は「移動体の縦横方向の速度成分＝０」を観測量“Ｚ”として与えることができる。四輪自動車以外の移動体についても、当該移動体の移動方向や速度の大きさに関する制約に基づいて、移動時制約条件を適宜設定可能である。
【００５０】
第１のカルマンフィルター処理で求めた状態“Ｘ1”について、その速度誤差“（δＶE，δＶN，δＶU）”を用いてＩＮＳ演算速度を補正し、その結果を第１の演算速度として第２のカルマンフィルター処理部７Ａに出力する。クロックドリフト“ｄ”は、第１の演算クロックドリフトとして第２のカルマンフィルター処理部７Ａに出力する。
【００５１】
また、加速度バイアス“（ｂax，ｂay，ｂaz）”及び角速度バイアス“（ｂgx，ｂgy，ｂgz）”をＩＮＳユニット２Ａにフィードバックする。ＩＮＳユニット２Ａは、第１のカルマンフィルター処理部５Ａから入力した加速度バイアス“（ｂax，ｂay，ｂaz）”及び角速度バイアス“（ｂgx，ｂgy，ｂgz）”を用いて、加速度センサー及びジャイロセンサーを補償（キャリブレーション）する。
【００５２】
また、第１のカルマンフィルター処理で求めた状態“Ｘ2”について、その位置誤差“（δＰE，δＰN，δＰU）”を用いてＩＮＳ演算位置を補正し、その結果を第１の演算位置として出力する。第１の演算位置は、最終的な移動体の位置として、各種のアプリケーション処理に利用される。
【００５３】
また、第１のカルマンフィルター処理では、所定の誤差推定演算を行って、第１の演算位置に内在する第１の演算位置誤差を推定する。具体的には、例えば次式（４）に従って第１の演算位置誤差“Ｐ１err”を算出・推定する。
【数４】
【００５４】
式（４）において、“Δｔ”は、第１のカルマンフィルター処理の演算時間間隔である。状態“Ｘ1”の第１〜第３成分である速度誤差“（δＶE，δＶN，δＶU）”に演算時間間隔“Δｔ”を乗算することで、その演算時間間隔での位置誤差の変化分を求める。そして、当該変化分を状態“Ｘ2”の位置誤差“（δＰE，δＰN，δＰU）”に加算することで、第１の演算位置誤差“Ｐ１err”を算出する。
【００５５】
第２のカルマンフィルター処理部７Ａは、第１のカルマンフィルター処理部５Ａから入力した第１の演算速度及び第１の演算クロックドリフトを入力Ｕとし、ＧＰＳユニット３Ａから入力したＧＰＳ演算位置を観測量“Ｚ”として、移動体の位置を演算する。第２のカルマンフィルター処理部７Ａにより演算された位置は、第２の演算位置として加減算部８に出力される。
【００５６】
第２のカルマンフィルター処理では、推定対象とする移動体の状態“Ｘ”を、例えば次式（５）のように設定する。
【数５】
但し、“（ＰE，ＰN，ＰU）”は、ＥＮＵ座標系で表した移動体の位置である。“ｄ”はクロックドリフトである。
【００５７】
また、第２のカルマンフィルター処理では、観測量“Ｚ”を、例えば次式（６）のように設定する。
【数６】
但し、“（ＰX，ＰY，ＰZ）GPS”は、ＥＣＥＦ座標系で表したＧＰＳ演算位置である。
【００５８】
本実施形態において特徴的であるのは、第１のカルマンフィルター処理では、ＧＰＳ演算位置を観測量“Ｚ”とせず、ＧＰＳ演算速度を観測量“Ｚ”としている点である。その代わり、ＧＰＳ演算位置は、第２のカルマンフィルター処理の観測量“Ｚ”とする。これは、ＧＰＳでは種々の誤差要因が存在するため、ＧＰＳ演算位置に大きな誤差が含まれ得ることを想定したためである。その典型例はマルチパス環境である。
【００５９】
マルチパス環境でＧＰＳ演算を行うと、演算される位置の正確性が低下する。そのため、ＧＰＳ演算位置とＩＮＳ演算位置とを単純にカップリングしてしまうと、ＧＰＳ演算位置の誤差に引きずられて、演算位置の正確性が低下するおそれがある。そこで、位置演算を、第１の演算処理（第１のカルマンフィルター処理）と第２の演算処理（第２のカルマンフィルター処理）とに分離し、第１の演算処理では、ＧＰＳ演算位置を用いずに移動体の位置を演算することとした。
【００６０】
また、ＧＰＳでは、移動体の位置と併せて、移動体の速度も演算することができる。速度は位置と比べてマルチパスの影響を受けにくく、ＩＮＳ演算結果とのカップリングにそれほど大きな影響を与えない。そこで、第１のカルマンフィルター処理では、ＧＰＳ演算速度を観測量として用いて移動体の位置を演算することとした。
【００６１】
加減算部８は、第２のカルマンフィルター処理部７Ａから出力された第２の演算位置と、ＩＮＳユニット２Ａから出力されたＩＮＳ演算位置との差分を算出する。便宜的に、この差分のことを「第２の演算位置誤差」と呼称する。加減算部８により算出された第２の演算位置誤差“Ｐ２err”は、カルマンフィルター係数調整部９Ａに出力される。
【００６２】
カルマンフィルター係数調整部９Ａは、ＩＮＳ演算誤差推定部９１及びフィルター係数算出部９３を有し、第１のカルマンフィルター処理に係るフィルター係数を調整する。本実施形態では、フィルター係数算出部９３を、第１のカルマンフィルター処理部５Ａの機能部として図示・説明する。
【００６３】
フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１により推定されたＩＮＳ演算誤差を利用して、第１のカルマンフィルター処理に係るフィルター係数を算出する。調整するフィルター係数は、例えば、状態“Ｘ1”,“Ｘ2”、及び、誤差共分散行列“Ｐ11”，“Ｐ22”，“Ｐ21”，“Ｐ12”である。フィルター係数の調整方法については詳細に後述する。
【００６４】
ＩＮＳ演算誤差推定部９１は、ＩＮＳ演算結果に含まれる誤差であるＩＮＳ演算誤差を推定する機能部であり、例えば、位置誤差加重平均部９１１と、重み設定部９１３と、誤差変換部９１５とを有する。
【００６５】
位置誤差加重平均部９１１は、重み設定部９１３により設定された重みを用いて、第１の演算位置誤差“Ｐ１err”と第２の演算位置誤差“Ｐ２err”とを加重平均して、ＩＮＳユニット２Ａの演算位置誤差（慣性測位誤差）を算出する。具体的には、例えば次式（７）に従って平均位置誤差“ａｖｅＰerr”を算出する。
【数７】
但し、“α”は、第１の演算位置誤差“Ｐ１err”に対する重みであり、“０≦α≦１”である。第２の演算位置誤差“Ｐ２err”に対する重みは“１−α”である。
【００６６】
重み設定部９１３は、測位環境やＧＰＳユニット３Ａの計測結果の信頼性といった複数の要素に基づいて、位置誤差加重平均部９１１の加重平均の重み“α”を設定する。重み設定方法については詳細に後述する。
【００６７】
誤差変換部９１５は、第１のカルマンフィルター処理部５Ａから入力した誤差共分散“Ｐ”を用いて、位置誤差加重平均部９１１から入力した平均位置誤差“ａｖｅＰerr”をＩＮＳ演算誤差に変換する。ＩＮＳ演算誤差の変換方法についても詳細に後述する。
【００６８】
図３は、第１のカルマンフィルター処理部５Ａの入出力データの説明図である。状態“Ｘ”と、入力“Ｕ”と、観測量“Ｚ”との対応関係のテーブルを図示している。カップリングには種々の方式が存在する。その中でも、ルーズカップリング（疎結合）と呼ばれる方式と、タイトカップリング（密結合）と呼ばれる方式とが一般的に用いられる。
【００６９】
ルーズカップリング方式は、ＧＰＳとＩＮＳとの結び付きが比較的弱いカップリング方式である。この方式では、例えば、状態“Ｘ”をＩＮＳ演算誤差（ＩＮＳ演算位置誤差、ＩＮＳ演算速度誤差、ＩＮＳ演算姿勢角誤差、ＩＮＳ加速度バイアス、ＩＮＳ角速度バイアス、クロックバイアス等）とする。また、入力“Ｕ”をＩＮＳ演算結果（ＩＮＳ演算位置、ＩＮＳ演算速度、ＩＮＳ演算姿勢角等）とし、観測量“Ｚ”をＧＰＳ演算結果（ＧＰＳ演算速度等）や運動モデルに基づく制約条件（停止時又は移動時）とする。
【００７０】
タイトカップリング方式は、ＧＰＳとＩＮＳとの結び付きが比較的強いカップリング方式である。この方式では、例えば、入力“Ｕ”及び状態“Ｘ”を上記と同様とし、観測量“Ｚ”をＧＰＳメジャメント情報（擬似距離変化率等）や運動モデルに基づく制約条件とする。
【００７１】
上記の状態“Ｘ”、入力“Ｕ”、観測量“Ｚ”の各成分は、適宜追加／削除可能である。例えば、状態“Ｘ”について、バイアス成分を削除して、ＩＮＳ演算位置誤差、ＩＮＳ演算速度誤差及びＩＮＳ演算姿勢角誤差を状態“Ｘ”の成分としてもよい。また、観測量“Ｚ”について、ＧＰＳ演算速度又は擬似距離変化率と、運動モデルに基づく制約条件とを併用することとしてもよいし、何れか一方のみを用いることとしてもよい。
【００７２】
１−２．重み設定方法
次に、重み“α”の設定方法を説明する。本実施形態では、比較的高い値として定められた高設定値“αhigh”と、中程度の値として定められた中設定値“αmiddle”と、比較的低い値として定められた低設定値“αlow”との３種類の値の何れかを重み“α”に設定する。これらの具体的な値は、適宜定めることとしてよい。重み“α”は「０〜１」の範囲で、「αhigh＝０．９，αmiddle＝０．５，αlow＝０．１」といった値や、「αhigh＝０．７５，αmiddle＝０．５，αlow＝０．２５」といった値を定めておくことができる。
【００７３】
図４は、重み設定に係る第１の重み設定条件を定めた第１の重み設定条件テーブルの一例を示す図である。第１の重み設定条件テーブルには、低重み設定条件と、高重み設定条件とが対応付けて定められている。低重み設定条件は、重み“α”に低設定値“αlow”を設定するための条件である。高重み設定条件は、重み“α”に高設定値“αhigh”を設定するための条件である。低重み設定条件及び高重み設定条件の何れの条件も成立しない場合は、重み“α”に中設定値“αmiddle”を設定する。
【００７４】
第１の重み設定条件は、測位環境に基づき定められた条件である。低重み設定条件には、測位環境がマルチパス環境であることが定められており、高重み設定条件には、測位環境がオープンスカイ環境であることが定められている。
【００７５】
測位環境がマルチパス環境である場合は、間接波の影響により、観測される擬似距離に誤差が生ずる。そのため、ＧＰＳユニット３Ａにより演算される位置の正確性が低下する。第２のカルマンフィルター処理部７ＡではＧＰＳ演算位置を観測量“Ｚ”とするため、ＧＰＳ演算位置の正確性が低いと、第２の演算位置にもその影響が現れる。
【００７６】
この場合、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”は、マルチパスの影響を含む値となる。従って、測位環境がマルチパス環境である場合は、マルチパスの影響が顕著に反映された第２の演算位置誤差“Ｐ２err”を重視するために第２の演算位置誤差“Ｐ２err”に対する重み“１−α”を大きくする。これは、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくすることに相当する。
【００７７】
図５は、重み設定に係る第２の重み設定条件を定めた第２の重み設定条件テーブルの一例を示す図である。第２の重み設定条件テーブルには、条件の番号である条件Ｎｏと、低重み設定条件と、高重み設定条件とが対応付けて定められている。
【００７８】
条件“Ａ”は、ＧＰＳ演算位置の信頼性に関する条件である。低重み設定条件には「ＧＰＳ演算位置の信頼性＝低」が定められており、高重み設定条件には「ＧＰＳ演算位置の信頼性＝高」が定められている。
【００７９】
ＧＰＳ演算位置の信頼性が低いと、第２のカルマンフィルター処理部７Ａで演算される第２の演算位置の正確性が低下する。そのため、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”には、その影響が顕著に反映される。そこで、ＧＰＳ演算位置の信頼性が低い場合には、第２の演算位置誤差“Ｐ２err”を重視するために、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８０】
ＧＰＳ演算位置の信頼性は、ＧＰＳ衛星信号の受信環境や信号強度、ＧＰＳ衛星の天空配置、位置の急激な変化の検出といった複数の要素を総合的に考慮して判定することができる。例えば、ＧＰＳ衛星の天空配置が良好でない場合（例えばＤＯＰ（Dilution Of Precision）値が大きい場合）や、大きな位置飛び（真の位置から大きく離れた位置が演算結果として得られること）が発生した場合、高度方向の急激な位置変化が発生した場合に、ＧＰＳ演算位置の信頼性が低いと判定することができる。
【００８１】
条件“Ｂ”は、ＧＰＳメジャメント情報の信頼性に関する条件である。低重み設定条件には「ＧＰＳメジャメント情報の信頼性＝低」が定められており、高重み設定条件には「ＧＰＳメジャメント情報の信頼性＝高」が定められている。ＧＰＳメジャメント情報の信頼性が低いと、演算されるＧＰＳ演算位置の信頼性が低下する。従って、この場合は条件“Ａ”と同様に、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８２】
例えば、アーバンキャニオン環境等においてＧＰＳメジャメント情報に突発的に異常値が混入したような場合や、擬似距離変化率（レンジレート）と擬似距離（シュードレンジ）との相関関係に矛盾が生じている場合に、ＧＰＳメジャメント情報の信頼性は低いと判定する。
【００８３】
条件“Ｃ”は、第２の演算位置の信頼性に関する条件である。低重み設定条件には「第２の演算位置の信頼性＝低」が定められており、高重み設定条件には「第２の演算位置の信頼性＝高」が定められている。第２のカルマンフィルター処理で演算された第２の演算位置の信頼性が低い場合は、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”を重視するように、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８４】
例えば、移動体の速度はゼロであるか微小値であるにも関わらず第２の演算位置が不連続的に変化している場合や、第２の演算位置に突発的な位置飛びが発生した場合に、第２の演算位置の信頼性は低いと判定する。
【００８５】
条件“Ｄ”は、時刻情報の信頼性に関する条件である。低重み設定条件には「時刻情報の信頼性＝高」が定められており、高重み設定条件には「時刻情報の信頼性＝低」が定められている。時刻情報の信頼性が低いと、ＧＰＳユニット３Ａにより演算されるＧＰＳ演算位置の信頼性が低下する。そのため、条件“Ａ”と同様に、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８６】
例えば、ＧＰＳの時刻同期誤差が所定の閾値を超えている（或いは閾値以上）場合に、時刻情報の信頼性は低いと判定する。
【００８７】
上記の複数の条件を用いて重み“α”を設定する。まず、複数の条件それぞれについて、判定項目の信頼性を数値化する。例えば、判定項目の信頼性が最も低い状態を信頼度“０”、信頼性が最も高い状態を信頼度“１”として、各判定項目の信頼度を「０〜１」の範囲で数値化する。
【００８８】
そして、例えば次式（８）に従って、重み設定指標値“Ｗ”を算出する。
【数８】
但し、“ｋ＝ｋA，ｋB，ｋC，・・・”は、各々の条件“Ａ，Ｂ，Ｃ，・・・”の信頼度に対する重みであり、“ｋA＋ｋB＋ｋC＋・・・＝Σｋ＝１”である。単純化するのであれば“ｋA＝ｋB＝ｋC＝・・・＝１／Ｎ”としてもよい。但し、“Ｎ”は、重み設定に用いる条件の総数である。
【００８９】
重み設定指標値“Ｗ”に基づいて、設定する重み“α”を決定する。例えば、重み設定指標値“Ｗ”が第１の閾値“θ1”よりも小さい（又は以下）場合は、重み“α”を低設定値とする（α＝αlow）。また、重み設定指標値“Ｗ”が第２の閾値“θ2（＞θ1）”よりも大きい（又は以上）場合は、重み“α”を高設定値とする（α＝αhigh）。また、上記以外の場合には、重み“α”を中設定値とする（α＝αmiddle）。
【００９０】
図４の第１の重み設定条件と、図５の第２の重み設定条件とは、それぞれ単体で用いてもよいし、併用してもよい。つまり、測位環境に基づいて重みを設定して加重平均することも可能であるし、ＧＰＳユニット３Ａの計測結果の信頼性等に基づいて重みを設定して加重平均することも可能であるし、これらの条件を併用して重みを設定して加重平均することも可能である。条件を併用する場合は、第１の重み設定条件の判定結果と第２の重み設定条件の判定結果とをＡＮＤ条件又はＯＲ条件として、重み“α”を設定することとしてもよい。
【００９１】
また、第１の重み設定条件を第２の重み設定条件に含めて判定を行うことも可能である。具体的には、例えば、測位環境が最も良好な状態（例えばオープンスカイ環境）を“１”、測位環境が最も悪い状態（例えばアーバンキャニオン環境）を“０”として、測位環境の良否を数値化する。そして、数値化した測位環境の良否を含めて、式（８）の重み設定指標値“Ｗ”を算出することとしてもよい。
【００９２】
１−３．ＩＮＳ演算誤差推定方法
次に、ＩＮＳ演算誤差の推定方法を説明する。平均位置誤差をＩＮＳ演算誤差に変換する方法としては、例えばカルマンフィルターを用いることができる。
【００９３】
最初に、次式（９）に従って、観測量“Ｚ2”と予測した観測量“Ｈ22Ｘ2”との差分“Ｖ2”を算出する。
【数９】
【００９４】
式（９）において、観測量“Ｚ2”は平均位置誤差“ａｖｅＰerr”である。すなわち、“Ｚ2＝ａｖｅＰerr”である。また、“Ｈ22Ｘ2”は観測量“Ｚ2”の予測値であり、平均位置誤差“ａｖｅＰerr”の予測値に相当する。つまり、式（２）で与えられる状態“Ｘ2”に観測行列“Ｈ22”を作用させて平均位置誤差“ａｖｅＰerr”を予測することを示す式である。観測行列“Ｈ22”は、状態“Ｘ2”から観測量“Ｚ2”への変換行列である。
【００９５】
次に、次式（１０）及び（１１）に従って、カルマンゲイン“Ｋ12”及び“Ｋ22”を算出する。
【数１０】
【数１１】
【００９６】
“Ｋ12”は差分“Ｖ2”から状態“Ｘ1”を演算するためのカルマンゲインであり、“Ｋ22”は差分“Ｖ2”から状態“Ｘ2”を演算するためのカルマンゲインである。また、“Ｒ2”は観測量“Ｚ2”の観測誤差（観測ノイズ）である。
【００９７】
そして、上記のカルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、状態“Ｘ1”及び“Ｘ2”を、次式（１２）及び（１３）に従って算出する。
【数１２】
【数１３】
【００９８】
式（１２）に従って算出された状態“Ｘ1”の成分のうち、速度誤差及び姿勢角誤差を、推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”として第１のカルマンフィルター処理部５Ａにフィードバックする。また、式（１３）に従って算出された状態“Ｘ2”の成分である位置誤差を、推定位置誤差“Ｐerr”として第１のカルマンフィルター処理部５Ａにフィードバックする。また、カルマンゲイン“Ｋ12”及び“Ｋ22”を第１のカルマンフィルター処理部５Ａにフィードバックする。
【００９９】
１−４．フィルター係数調整方法
次に、フィルター係数の調整方法を説明する。フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１から入力したＩＮＳ演算誤差（推定位置誤差“Ｐerr”、推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”）やカルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、フィルター係数を算出する。
【０１００】
第１に、フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１から入力した推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”を、状態“Ｘ1”のうちの対応する成分（速度誤差“（δＶE，δＶN，δＶU）”、姿勢角誤差“（Ψx，Ψy，Ψz）”）に加味して、状態“Ｘ1”を算出・更新する。また、ＩＮＳ演算誤差推定部９１から入力した推定位置誤差“Ｐerr”を状態“Ｘ2”の各成分（位置誤差“（δＰE，δＰN，δＰU）”）に加味して、状態“Ｘ2”を算出・更新する。
【０１０１】
第２に、フィルター係数算出部９３は、カルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、例えば次式（１４）〜（１７）に従って誤差共分散行列“Ｐ11”，“Ｐ22”，“Ｐ21”，“Ｐ12”をそれぞれ算出・更新する。
【数１４】
【数１５】
【数１６】
【数１７】
【０１０２】
２．実験結果
図６及び図７は、上記の位置算出方法を用いて位置算出を行った実験結果の一例を示す図である。第１の位置算出装置１Ａを自動車に設置し、都市部の道路を自動車で走行する実験を行った。走行した都市部は高層ビルが数多く存在するマルチパス環境である。図６及び図７の都市部の道路を反時計回りに周回して位置を算出し、その軌跡を描いてみた。スタート，ゴール地点は、図面中央下部である。
【０１０３】
図６（１）は、自動車の真の軌跡（参照軌跡）である。図６（２）は、ＧＰＳユニット３Ａの出力であるＧＰＳ演算位置の軌跡である。図６（３）は、第２のカルマンフィルター処理部７Ａの出力である第２の演算位置の軌跡である。
【０１０４】
図７（１）は、演算係数の調整を行わない場合の第１のカルマンフィルター処理部５Ａの出力である第１の演算位置の軌跡（演算係数調整なし）である。図７（２）は、演算係数の調整は行ったが、重み“α”の設定を行わなかった場合の第１の演算位置の軌跡（演算係数調整あり、重み設定なし）である。図７（３）は、演算係数の調整及び重み“α”の設定を両方とも行った場合の第１の演算位置の軌跡（演算係数調整あり、重み設定あり）である。
【０１０５】
最初に、図６（２）のＧＰＳ演算位置の軌跡を見ると、所々に大きく蛇行しており、位置誤差が非常に大きくなっていることがわかる。これは、マルチパスの影響によりＧＰＳの演算精度が低下したことによるものである。図６（３）の第２の演算位置の軌跡を見ると、ＧＰＳ演算位置の軌跡と比べると滑らかな軌跡を描いているが、それでもやはり、図６（１）の真の軌跡に沿った軌跡とはなっていない。
【０１０６】
次に、図７（１）の第１の演算位置の軌跡（演算係数調整なし）を見ると、点線部分Ｓ１において、真の軌跡に対してバイアスがかかったような軌跡が得られている。これに本実施形態の演算係数の調整を追加すると、図７（２）の結果が得られる。この図７（２）の第１の演算位置の軌跡（演算係数調整あり、重み設定なし）を見ると、演算係数の調整により位置算出精度が改善されていることがわかる。しかし、それでもなお、点線部分Ｓ２において真の軌跡からの位置ズレが生じている。
【０１０７】
これにさらに本実施形態の重み“α”の設定を追加すると、図７（３）の結果が得られる。この図７（３）の第１の演算位置の軌跡（演算係数調整あり、重み設定あり）を見ると、図６（１）の真の軌跡に沿った正確な軌跡が得られていることがわかる。特に、図７（１）の点線部分Ｓ１や図７（２）の点線部分Ｓ２に対応する部分についても位置算出精度が改善されている。この実験結果から、本実施形態の位置算出方法が有効であることが実証された。
【０１０８】
３．実施例
次に、上記の位置算出装置を備えた電子機器の実施例について説明する。ここでは、位置算出装置を具備するカーナビゲーション装置の実施例を説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【０１０９】
３−１．システム構成
図８は、本実施例におけるナビゲーションシステム１０００のシステム構成の説明図である。ナビゲーションシステム１０００は、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、位置算出装置１を具備したカーナビゲーション装置１００が設置されてなる。
【０１１０】
カーナビゲーション装置１００は、自動車に設置され、自動車の運転者に対するナビゲーションを行う電子機器である。カーナビゲーション装置１００は、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａとを備える。
【０１１１】
カーナビゲーション装置１００は、ＩＮＳユニット２Ａから入力したＩＮＳ演算結果と、ＧＰＳユニット３Ａから入力したＧＰＳ演算結果とを用いて、原理で説明した位置算出方法に従って自動車の位置を算出する。そして、算出位置をプロットしたナビゲーション画面を生成して、表示部３０であるディスプレイに表示させる。
【０１１２】
３−２．機能構成
図９は、カーナビゲーション装置１００の機能構成の一例を示すブロック図である。カーナビゲーション装置１００は、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａと、処理部１０と、操作部２０と、表示部３０と、通信部４０と、記憶部５０とを備える。
【０１１３】
処理部１０は、記憶部５０に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。処理部１０は、記憶部５０に記憶されたナビゲーションプログラム５１に従ってナビゲーション処理を行い、自動車の現在位置を指し示した地図を表示部３０に表示させる。
【０１１４】
操作部２０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部１０に出力する。この操作部２０の操作により、目的地の入力等の各種指示入力がなされる。
【０１１５】
表示部３０は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部１０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部３０には、ナビゲーション画面等が表示される。
【０１１６】
通信部４０は、処理部１０の制御に従って、装置内部で利用される情報をインターネット等の通信ネットワークを介して外部とやりとりするための通信装置である。この通信には、公知の無線通信技術を適用可能である。
【０１１７】
記憶部５０は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【０１１８】
記憶部５０には、プログラムとして、処理部１０により読み出され、ナビゲーション処理（図１０参照）として実行されるナビゲーションプログラム５１が記憶されている。ナビゲーションプログラム５１は、ＩＮＳ演算誤差推定処理（図１１参照）として実行されるＩＮＳ演算誤差推定プログラム５１１をサブルーチンとして含む。
【０１１９】
また、記憶部５０には、データとして、例えば、重み設定条件テーブル５３と、ＩＮＳ演算結果５５と、ＧＰＳ演算結果５６と、第１の演算結果５７と、第２の演算結果５８と、演算係数５９とが記憶される。
【０１２０】
重み設定条件テーブル５３は、第１の重み設定条件テーブル（図４参照）や第２の重み設定条件テーブル（図５参照）といった重み設定用の条件が定められたテーブルである。
【０１２１】
３−３．処理の流れ
図１０は、処理部１０が、記憶部５０に記憶されたナビゲーションプログラム５１に従って実行するナビゲーション処理の流れを示すフローチャートである。
【０１２２】
先ず、処理部１０は、ＩＮＳユニット２Ａ及びＧＰＳユニット３Ａから、ＩＮＳ演算結果５５及びＧＰＳ演算結果５６の取得を開始する（ステップＡ１）。そして、処理部１０は、第１のカルマンフィルター処理を実行する（ステップＡ３）。
【０１２３】
第１のカルマンフィルター処理では、前回のＩＮＳ演算誤差推定処理（ステップＡ９）の推定結果を用いてフィルター係数を調整し、記憶部５０の演算係数５９を更新する。具体的には、原理で説明したように、推定したＩＮＳ演算誤差を用いて状態“Ｘ”を調整するとともに、例えば式（１４）〜（１７）に従って誤差共分散“Ｐ”を調整する。そして、調整したフィルター係数を用いてカルマンフィルター処理を行い、その演算結果で記憶部５０の第１の演算結果５７を更新する。
【０１２４】
次いで、処理部１０は、第１のカルマンフィルター処理で演算した演算位置に対するマップマッチング処理等を行い、その結果で表示部３０のナビゲーション画面を表示更新する（ステップＡ５）。
【０１２５】
その後、処理部１０は、第２のカルマンフィルター処理を実行する（ステップＡ７）。処理部１０は、第２のカルマンフィルター処理の演算結果で記憶部５０の第２の演算結果５８を更新する。そして、処理部１０は、記憶部５０に記憶されたＩＮＳ演算誤差推定プログラム５１１に従ってＩＮＳ演算誤差推定処理を行う（ステップＡ９）。
【０１２６】
図１１は、ＩＮＳ演算誤差推定処理の流れを示すフローチャートである。
先ず、処理部１０は、重み設定処理を行う（ステップＢ１）。具体的には、記憶部５０に記憶されている重み設定条件テーブル５３を参照して、原理で説明した手法に従って重み“α”を設定する。
【０１２７】
次いで、処理部１０は、位置誤差加重平均処理を行う（ステップＢ３）。具体的には、第１のカルマンフィルター処理で演算した第１の演算位置誤差“Ｐ１err”と、第２のカルマンフィルター処理で演算した第２の演算位置誤差“Ｐ２err”とを、ステップＢ１で設定した重み“α”を用いて式（７）に従って加重平均して、平均位置誤差“ａｖｅＰerr”を算出する。
【０１２８】
その後、処理部１０は、誤差変換処理を行う（ステップＢ５）。具体的には、ステップＢ３で算出した平均位置誤差“ａｖｅＰerr”を、式（９）〜式（１３）に従ってＩＮＳ演算誤差に変換する。そして、処理部１０は、ＩＮＳ演算誤差推定処理を終了する。
【０１２９】
図１０のナビゲーション処理に戻り、ＩＮＳ演算誤差推定処理を行った後、処理部１０は、処理を終了するか否かを判定する（ステップＡ１１）。例えば、操作部２０を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。
【０１３０】
処理を終了しないと判定した場合は（ステップＡ１１；Ｎｏ）、処理部１０は、ステップＡ３に戻る。また、処理を終了すると判定した場合は（ステップＡ１１；Ｙｅｓ）、ナビゲーション処理を終了する。
【０１３１】
４．作用効果
本実施形態によれば、位置算出装置１において、移動体に設置された慣性測位用ユニット２の計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理が第１の演算処理部５により実行される。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニット３の計測結果とを用いて、移動体の位置を演算する第２の演算処理が第２の演算処理部７により実行される。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数が演算係数調整部９により調整される。
【０１３２】
例えば、位置算出装置１を適用した第１の位置算出装置１Ａでは、第１のカルマンフィルター処理部５Ａにおいて、ＩＮＳユニットの演算結果（ＩＮＳ演算結果）を入力とし、ＧＰＳユニット３Ａの演算速度（ＧＰＳ演算速度）を観測量とするカルマンフィルター処理が実行される。また、第２のカルマンフィルター処理部７Ａにおいて、第１のカルマンフィルター処理部５Ａの演算結果（第１の演算結果）を入力とし、ＧＰＳユニット３Ａの演算位置（ＧＰＳ演算位置）を観測量とするカルマンフィルター処理が実行される。
【０１３３】
加減算部８では、ＩＮＳ演算位置と第２の演算位置との差分が、第２の演算位置誤差として算出される。ＩＮＳ演算誤差推定部９１では、重み設定部９１３により、第１の演算位置誤差と第２の演算位置誤差とを加重平均する際の重みが設定される。そして、第１の演算位置誤差と第２の演算位置誤差とが、位置誤差加重平均部９１１により加重平均される。そして、加重平均結果である平均位置誤差を用いて、誤差変換部９１５により平均位置誤差がＩＮＳ演算誤差に変換され、その変換結果が第１のカルマンフィルター処理部５Ａにフィードバックされる。
【０１３４】
重み設定部９１３は、例えば、測位環境に基づき定められた第１の重み設定条件や、ＧＰＳ演算位置の信頼性、ＧＰＳメジャメント情報の信頼性、第２の演算位置の信頼性、時刻情報の信頼性といった複数の要素に基づき定められた第２の重み設定条件に従って、加重平均の重みを設定する。これにより、加重平均の重みを適正化し、ＩＮＳ演算位置に含まれる誤差を適切に推定することができる。
【０１３５】
また、誤差変換部９１５は、第１のカルマンフィルター処理部５Ａから入力した誤差共分散と、位置誤差加重平均部９１１から入力した平均位置誤差とを用いて、カルマンフィルターを利用した変換演算を行って、ＩＮＳ演算誤差を推定する。カルマンフィルターを利用することで、平均位置誤差からＩＮＳ演算誤差を適切に推定することができる。
【０１３６】
また、フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１により推定されたＩＮＳ演算誤差やカルマンゲインを用いて、第１のカルマンフィルター処理で用いるフィルター係数を算出・調整する。例えば、第１のカルマンフィルター処理で用いる状態（状態ベクトル）や誤差共分散（誤差共分散行列）を算出・調整する。このようにして調整したフィルター係数を用いて第１のカルマンフィルター処理を行うことで、移動体の位置を高い正確性で求めることが可能となる。
【０１３７】
５．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【０１３８】
５−１．ユニット
上記の実施形態では、衛星測位用ユニット３として、ＧＰＳを適用したＧＰＳユニット３Ａを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星測位システムを適用したユニットとしてもよいことは勿論である。
【０１３９】
また、上記の実施形態では、慣性測位用ユニット２として、ＩＮＳユニット２Ａを適用した場合を例に挙げて説明したが、ＩＮＳメジャメント情報（加速度や角速度）を計測する慣性センサーや慣性計測ユニット（ＩＭＵ）を慣性測位用ユニット２としてもよい。この場合は、慣性測位用ユニット２により計測されたＩＮＳメジャメント情報を入力Ｕとして、第１の演算処理部５が第１の演算処理を行うように構成すればよい。
【０１４０】
５−２．演算処理
第１の演算処理部５及び第２の演算処理部７が行う演算処理は、カルマンフィルター処理に限定されるわけではない。例えば、第２の演算処理について、シグマ点（シグマポイント）を利用したシグマポイントフィルター処理や、回帰フィルター処理を適用することとしてもよい。
【０１４１】
５−３．座標系
上記の実施形態では、ＩＮＳユニット２Ａは、移動体の位置、速度及び姿勢角をＥＮＵ座標系で演算するものとして説明したが、ＮＥＤ座標系や、ＥＣＥＦ座標系で演算することとしてもよいことは勿論である。また、第１の演算処理や第２の演算処理についても、ＥＮＵ座標系で演算するのではなく、ＮＥＤ座標系やＥＣＥＦ座標系で演算してもよい。
【０１４２】
５−４．重み設定条件
上記の実施形態で例示した重み設定条件はあくまでも一例であり、適宜追加／削除可能である。例えば、ＩＮＳユニット２Ａの演算結果の信頼性に基づく条件を定めておいてもよい。ＩＮＳユニット２Ａが有する慣性センサー（ジャイロセンサーや加速度センサー）の計測結果に大きな誤差が混入すると、ＩＮＳユニット２Ａの演算精度が低下する。この場合、ＩＮＳ演算結果を入力とする第１の演算処理の演算精度が低下する。
【０１４３】
その結果、第１の演算処理で演算される第１の演算位置誤差“Ｐ１err”には、その影響が顕著に現れる。そこで、ＩＮＳユニット２Ａの演算結果の信頼性が低い場合は、第１の演算位置誤差“Ｐ１err”を重視して加重平均するように、第１の演算位置誤差“Ｐ１err”に対する重み“α”を大きくすると効果的である。この場合は、高重み設定条件として「ＩＮＳユニットの信頼性＝低」を定めておけばよい。
【０１４４】
また、上記の実施形態では、第２の重み設定条件に関して、複数の条件“Ａ，Ｂ，Ｃ，Ｄ，・・・”を組み合わせて重み“α”を設定する方法を説明した。しかし、複数の条件“Ａ，Ｂ，Ｃ，Ｄ，・・・”をそれぞれ単体で用いて重み“α”を設定するといったことも当然に可能である。
【０１４５】
５−５．処理主体
上記の実施形態では、ＩＮＳユニット２ＡがＩＮＳ演算を行い、ＧＰＳユニット３ＡがＧＰＳ演算を行うものとして説明した。しかし、電子機器の処理部１０がＩＮＳ演算やＧＰＳ演算を実行する構成としてもよい。
【０１４６】
この場合は、ＩＮＳユニット２Ａは、ＩＮＳメジャメント情報（加速度や角速度等）を出力する。また、ＧＰＳユニット３Ａは、ＧＰＳメジャメント情報（コード位相やドップラー周波数、擬似距離、擬似距離変化率等）を出力する。
【０１４７】
そして、電子機器の処理部１０は、ＩＮＳユニット２Ａから入力したＩＮＳメジャメント情報を用いてＩＮＳ演算を行うとともに、ＧＰＳユニット３Ａから入力したＧＰＳメジャメント情報を用いてＧＰＳ演算を行う。そして、処理部１０は、ＩＮＳ演算結果及びＧＰＳ演算結果を用いて、第１及び第２の演算処理を行う。
【０１４８】
５−６．電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【０１４９】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【符号の説明】
【０１５０】
１、１Ａ 位置算出装置、 ２ 慣性測位用ユニット、 ２Ａ ＩＮＳユニット、 ３ 衛星測位用ユニット、 ３Ａ ＧＰＳユニット、 ５ 第１の演算処理部、 ５Ａ 第１のカルマンフィルター処理部、 ７ 第２の演算処理部、 ７Ａ 第２のカルマンフィルター処理部、 ９ 演算係数調整部、 ９Ａ カルマンフィルター係数調整部、 １０ 処理部、 ２０ 操作部、 ３０ 表示部、 ４０ 通信部、 ５０ 記憶部、 １００ カーナビゲーション装置、 １０００ ナビゲーションシステム
【技術分野】
【０００１】
本発明は、慣性測位用ユニット及び衛星測位用ユニットそれぞれの計測結果を併用した位置算出方法等に関する。
【背景技術】
【０００２】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行う慣性航法システム（以下、「ＩＮＳ（Inertial Navigation System）」と称する。）も考案されている。
【０００３】
ＩＮＳでは、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、ＩＮＳ演算結果を、ＧＰＳ（Global Positioning System）を利用して補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１０／００１９９６３号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ＧＰＳを利用してＩＮＳ演算結果を補正する技術は、ＧＰＳ演算結果が正しいことを前提としている。特許文献１に開示された技術も同様である。しかし、ＧＰＳ演算結果は、ＧＰＳ衛星から受信したＧＰＳ衛星信号の信号強度、受信環境、ＧＰＳ衛星の天空配置、マルチパスといった種々の要因により、演算結果の精度が低下する場合がある。
【０００６】
ＧＰＳ演算結果の精度が低下しているにも関わらず、特許文献１の技術のようにＧＰＳ演算結果を用いてＩＮＳ演算結果を補正してしまうと、位置算出の正確性が低下するという問題があった。
【０００７】
本発明は上述した課題に鑑みて為されたものであり、衛星測位用ユニット及び慣性測位用ユニットそれぞれの計測結果を併用して、位置をより正確に算出するための手法を提案することを目的とする。
【課題を解決するための手段】
【０００８】
以上の課題を解決するための第１の形態は、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行することと、前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行することと、前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整することと、を含む位置算出方法である。
【０００９】
また、他の形態として、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行する第１の演算処理部と、前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行する第２の演算処理部と、前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整する調整部と、を備えた位置算出装置を構成してもよい。
【００１０】
この第１の形態等によれば、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理を実行する。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニットの計測結果とを用いて、移動体の位置を演算する第２の演算処理を実行する。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数を調整する。
【００１１】
演算処理には、慣性測位用ユニットの計測結果を用いた第１の演算処理と、第１の演算処理の結果及び衛星測位用ユニットの計測結果を用いた第２の演算処理とがある。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数を調整することで、第１の演算処理における位置算出の正確性を向上させることができる。
【００１２】
また、第２の形態として、第１の形態の位置算出方法において、前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、前記第１の演算処理は、演算した第１の演算位置に内在する第１の位置誤差を推定する所定の誤差推定演算を含み、前記調整することは、前記第１の位置誤差と、前記第２の演算処理で求めた第２の演算位置とを用いて、前記慣性測位位置に内在する慣性測位誤差を推定することと、前記慣性測位誤差を用いて前記演算係数を調整することと、を含む、位置算出方法を構成することとしてもよい。
【００１３】
この第２の形態によれば、所定の誤差推定演算を行って、第１の演算処理で演算した第１の演算位置に内在する第１の位置誤差を推定する。そして、第１の位置誤差と、第２の演算処理で求めた第２の演算位置とを用いて、慣性測位用ユニットの計測結果に含まれる慣性測位位置に内在する慣性測位誤差を推定する。第１の位置誤差と併せて第２の演算位置を用いることで、慣性測位誤差を適切に推定することができる。そして、当該慣性測位誤差を用いて第１の演算処理に係る演算係数を調整することで、第１の演算処理における位置算出の正確性を向上させることができる。
【００１４】
また、第３の形態として、第２の形態の位置算出方法において、前記推定することは、前記慣性測位位置と前記第２の演算位置との差分を算出することと、前記第１の位置誤差と前記差分とを平均処理して前記慣性測位誤差を算出することと、を含む、位置算出方法を構成することとしてもよい。
【００１５】
この第３の形態によれば、慣性測位位置と第２の演算位置との差分を算出する。そして、第１の位置誤差と当該差分とを平均処理することで、慣性測位誤差を適切に推定することができる。
【００１６】
また、第４の形態として、第３の形態の位置算出方法において、前記平均処理は、測位環境と前記衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、前記第１の位置誤差と前記差分との重みを設定して加重平均する処理である、位置算出方法を構成することとしてもよい。
【００１７】
この第４の形態によれば、平均処理において、測位環境と衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、第１の位置誤差と差分との重みを設定して加重平均する。これにより、測位環境や衛星測位用ユニットの計測結果の信頼性に基づいて、慣性測位誤差をより正確に推定することができる。
【００１８】
また、第５の形態として、第１〜第４の何れかの形態の位置算出方法において、前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、前記第１の演算処理は、前記慣性測位位置を入力とするカルマンフィルター処理である、位置算出方法を構成することとしてもよい。
【００１９】
この第５の形態によれば、第１の演算処理として慣性測位位置を入力とするカルマンフィルター処理を行うことで、移動体の位置等を簡易且つ適切に求めることができる。
【００２０】
また、第６の形態として、第５の形態の位置算出方法において、前記衛星測位用ユニットの計測結果には速度が含まれ、前記カルマンフィルター処理は、前記速度を観測量とする処理である、位置算出方法を構成してもよい。
【００２１】
この第６の形態によれば、第５の形態のカルマンフィルター処理において、さらに衛星測位用ユニットの計測結果に含まれる速度を観測量とすることで、移動体の位置等をより正確に求めることができる。
【００２２】
また、第７の形態として、第１〜第６の何れかの形態の位置算出方法において、前記衛星測位用ユニットの計測結果には衛星測位位置が含まれ、前記第２の演算処理は、前記第１の演算処理の結果を入力とし、前記衛星測位位置を観測量とするカルマンフィルター処理である、位置算出方法を構成することとしてもよい。
【００２３】
この第７の形態によれば、第２の演算処理として、第１の演算処理の結果を入力とし、衛星測位用ユニットの計測結果に含まれる衛星測位位置を観測量とするカルマンフィルター処理を実行することで、移動体の位置を簡易且つ適切に求めることができる。
【図面の簡単な説明】
【００２４】
【図１】位置算出装置の構成図。
【図２】第１の位置算出装置の構成図。
【図３】第１のカルマンフィルター処理の入出力データの説明図。
【図４】第１の重み設定条件の説明図。
【図５】第２の重み設定条件の説明図。
【図６】位置算出を行った実験結果の一例を示す図。
【図７】位置算出を行った実験結果の一例を示す図。
【図８】ナビゲーションシステムのシステム構成図。
【図９】カーナビゲーション装置の機能構成を示すブロック図。
【図１０】ナビゲーション処理の流れを示すフローチャート。
【図１１】ＩＮＳ演算誤差推定処理の流れを示すフローチャート。
【発明を実施するための形態】
【００２５】
１．原理
１−１．構成
図１は、本実施形態における位置算出装置１の主要構成図である。位置算出装置１は、移動体に備えられ、当該移動体の位置を算出する装置（システム）である。移動体は、自動車やオートバイ、自転車、船、電車といった物の他、人間自体であってもよい。人間が位置算出装置１を携帯し、人間自身が位置算出装置１を備えることとしてもよい。
【００２６】
本明細書で参照する図面では、ユニット（モジュール）を二重線で図示し、ユニットの計測結果を利用して演算処理を行う処理ブロックを一重線で図示することで、両者を区別する。
【００２７】
位置算出装置１には、ユニット（モジュール）として、慣性測位用ユニット２と、衛星測位用ユニット３とが含まれる。また、位置算出装置１は、主要な処理ブロックとして、第１の演算処理部５と、第２の演算処理部７と、演算係数調整部９とを有する。なお、ユニットと処理ブロックとで位置算出装置１を構成してもよいし、ユニットを除外して処理ブロックのみで位置算出装置１を構成してもよい。
【００２８】
慣性測位用ユニット２は、慣性航法を利用して測位を行うためのユニットである。慣性測位用ユニット２は、加速度センサーやジャイロセンサー等の慣性センサーや、慣性センサーをパッケージ化した慣性計測ユニット（ＩＭＵ（Inertial Measurement Unit））、慣性計測ユニット及び演算処理部をパッケージ化した慣性航法システム（ＩＮＳ（Inertial Navigation System））等を適用したユニットである。
【００２９】
衛星測位用ユニット３は、衛星測位システムを利用して測位を行うためのユニットであり、例えば衛星測位システムの一種であるＧＰＳ（Global Positioning System）を適用可能である。
【００３０】
第１の演算処理部５は、慣性測位用ユニット２の計測結果と、所与の観測量とを併用して、予め定められた第１の演算処理を行って、少なくとも移動体の位置を算出する。本実施形態において、第１の演算処理に好適な適用例は、カルマンフィルター処理である。
【００３１】
第２の演算処理部７は、第１の演算処理部５の演算結果である第１の演算結果と、衛星測位用ユニット３の計測結果とを用いて、予め定められた第２の演算処理を行って、移動体の位置を算出する。本実施形態において、第２の演算処理に好適な適用例は、カルマンフィルター処理やシグマポイントフィルター処理、回帰フィルター処理である。
【００３２】
演算係数調整部９は、第１の演算処理部５から入力した第１の演算結果と、第２の演算処理部７から入力した第２の演算結果とを用いて、第１の演算処理に係る演算係数を調整する。第１の演算処理部５は、演算係数調整部９により調整された演算係数を用いて第１の演算処理を行う。
【００３３】
図２は、図１の位置算出装置１を適用した第１の位置算出装置１Ａの構成図である。第１の位置算出装置１Ａは、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａと、第１のカルマンフィルター処理部５Ａと、第２のカルマンフィルター処理部７Ａと、カルマンフィルター係数調整部９Ａとを有する。
【００３４】
第１の位置算出装置１Ａは、慣性測位用ユニット２としてＩＮＳユニット２Ａを適用し、衛星測位用ユニット３としてＧＰＳユニット３Ａを適用したシステムである。また、第１及び第２の演算処理部５，７として、第１及び第２のカルマンフィルター処理部５Ａ，７Ａをそれぞれ適用したシステムである。
【００３５】
ＩＮＳユニット２Ａは、ＩＭＵがローカル座標系で計測した加速度や角速度等のＩＮＳメジャメント情報を出力可能に構成されている。また、ＩＮＳユニット２Ａは、ＩＮＳメジャメント情報を用いた慣性航法演算を行って、移動体の位置や速度、姿勢角を演算して出力可能に構成されている。本実施形態では、ＩＮＳユニット２Ａは、ＩＮＳ演算位置（慣性測位位置）、ＩＮＳ演算速度及びＩＮＳ演算姿勢角を絶対座標系で演算して出力する。
【００３６】
ＧＰＳユニット３Ａは、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を受信して、コード位相やドップラー周波数、擬似距離、擬似距離変化率等のＧＰＳメジャメント情報を計測して出力可能に構成されている。また、ＧＰＳユニット３Ａは、ＧＰＳメジャメント情報を用いたＧＰＳ演算を行って、移動体の位置や速度を演算して出力可能に構成されている。本実施形態では、ＧＰＳユニット３Ａは、ＧＰＳ演算位置（衛星測位位置）及びＧＰＳ演算速度を絶対座標系で演算して出力する。
【００３７】
ローカル座標系は、ＩＮＳユニット２Ａが有する慣性センサーに対応付けられたローカルな座標系（センサー座標系）である。それに対し、絶対座標系は、移動体の移動空間を定める座標系である。例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、東北上座標系として知られるＥＮＵ（East North Up）座標系、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系といった座標系を適用可能である。
【００３８】
以下の数式では、ＩＮＳユニット２Ａは、移動体の位置、速度及び姿勢角をＥＮＵ座標系で演算して出力し、ＧＰＳユニット３Ａは、移動体の位置及び速度をＥＣＥＦ座標系で演算して出力するものとして説明する。また、第１及び第２のカルマンフィルター処理では、ＥＮＵ座標系で演算するものとして説明する。また、加速度や速度といった諸量は、実際には大きさ及び方向を持ったベクトルとして表される。しかし、本明細書では、ベクトルの文言を省略して、簡単に加速度や速度として説明する。
【００３９】
第１のカルマンフィルター処理部５Ａは、カルマンフィルターの理論に基づく演算処理を行って、移動体の位置や速度、姿勢角等を演算する。例えば、ＩＮＳユニット２Ａから入力したＩＮＳ演算位置、ＩＮＳ演算速度及びＩＮＳ演算姿勢角を入力Ｕ（制御入力）とする。また、ＧＰＳユニット３Ａから入力したＧＰＳ演算速度や、移動体の運動モデルに基づき定められる所与の制約条件を観測量Ｚとする。そして、カルマンフィルターの予測演算（時刻更新）及び補正演算（観測更新）を行って、状態推定値を求める。
【００４０】
第１のカルマンフィルター処理では、推定対象とする移動体の状態“Ｘ”を、例えば次式（１）及び（２）のように設定する。
【数１】
【数２】
【００４１】
式（１）で表される状態“Ｘ1”において、“（δＶE，δＶN，δＶU）”は、ＥＮＵ座標系で演算したＩＮＳ演算速度の誤差である。“（Ψx，Ψy，Ψz）”は、ローカル座標系で演算したＩＮＳ演算姿勢角の誤差である。“（ｂax，ｂay，ｂaz）”は、ローカル座標系で計測した加速度のバイアスである。“（ｂgx，ｂgy，ｂgz）”は、ローカル座標系で計測した角速度のバイアスである。また、“ｄ”は、ＧＰＳユニット３Ａの内部クロックのドリフト（クロックドリフト）である。
【００４２】
また、式（２）で表される状態“Ｘ2”において、“（δＰE，δＰN，δＰU）”は、ＥＮＵ座標系で演算したＩＮＳ演算位置の誤差である。
【００４３】
式（１）及び（２）から分かるように、本実施形態における第１のカルマンフィルター処理は、ＩＮＳユニット２Ａの演算結果に含まれる誤差を推定する誤差推定型のカルマンフィルター処理である。つまり、ＩＮＳユニット２Ａの演算結果に含まれる誤差を、状態“Ｘ1”及び“Ｘ2”として推定する。
【００４４】
また、第１のカルマンフィルター処理では、状態“Ｘ1”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ11”と、状態“Ｘ2”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ22”と、状態“Ｘ1”の各成分に対する状態“Ｘ2”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ21”と、状態“Ｘ2”の各成分に対する状態“Ｘ1”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ12”と、を併せて演算する。
【００４５】
第１のカルマンフィルター処理の補正演算では、ＧＰＳユニット３Ａにより演算された速度（以下、「ＧＰＳ演算速度」と称す。）を観測量“Ｚ”として、予測演算で予測された状態“Ｘ”を補正する。
【００４６】
具体的には、例えば次式（３）で与えられる観測量“Ｚ”を用いて補正演算を行う。
【数３】
但し、“（ＶE，ＶN，ＶU）INS”は、ＩＮＳユニット２ＡがＥＮＵ座標系で演算したＩＮＳ演算速度である。また、“（ＶX，ＶY，ＶZ）GPS”は、ＧＰＳユニット３ＡがＥＣＥＦ座標系で演算したＧＰＳ演算速度である。また、“ＣECEFENU”は、ＥＣＥＦ座標系からＥＮＵ座標系への座標変換行列である。
【００４７】
さらに、第１のカルマンフィルター処理では、ＧＰＳ演算速度とは別に、移動体の運動モデルに基づく制約条件を観測量“Ｚ”として適用可能に構成されている。具体的には、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との２種類の速度制約条件を適用可能である。
【００４８】
停止時速度制約条件（第１の制約条件）は、移動体の停止時に適用可能な制約条件である。移動体が停止しているのであれば、理想的には移動体の速度はゼロである。従って、移動体が停止していると判定した場合は「移動体の各軸の速度成分＝０」を観測量“Ｚ”として与えることができる。
【００４９】
移動時速度制約条件（第２の制約条件）は、移動体の移動時に適用可能な制約条件である。例えば、移動体として四輪自動車を想定した場合、通常、四輪自動車はジャンプや横滑りすることはないと仮定することができる。従って、移動体が移動していると判定した場合は「移動体の縦横方向の速度成分＝０」を観測量“Ｚ”として与えることができる。四輪自動車以外の移動体についても、当該移動体の移動方向や速度の大きさに関する制約に基づいて、移動時制約条件を適宜設定可能である。
【００５０】
第１のカルマンフィルター処理で求めた状態“Ｘ1”について、その速度誤差“（δＶE，δＶN，δＶU）”を用いてＩＮＳ演算速度を補正し、その結果を第１の演算速度として第２のカルマンフィルター処理部７Ａに出力する。クロックドリフト“ｄ”は、第１の演算クロックドリフトとして第２のカルマンフィルター処理部７Ａに出力する。
【００５１】
また、加速度バイアス“（ｂax，ｂay，ｂaz）”及び角速度バイアス“（ｂgx，ｂgy，ｂgz）”をＩＮＳユニット２Ａにフィードバックする。ＩＮＳユニット２Ａは、第１のカルマンフィルター処理部５Ａから入力した加速度バイアス“（ｂax，ｂay，ｂaz）”及び角速度バイアス“（ｂgx，ｂgy，ｂgz）”を用いて、加速度センサー及びジャイロセンサーを補償（キャリブレーション）する。
【００５２】
また、第１のカルマンフィルター処理で求めた状態“Ｘ2”について、その位置誤差“（δＰE，δＰN，δＰU）”を用いてＩＮＳ演算位置を補正し、その結果を第１の演算位置として出力する。第１の演算位置は、最終的な移動体の位置として、各種のアプリケーション処理に利用される。
【００５３】
また、第１のカルマンフィルター処理では、所定の誤差推定演算を行って、第１の演算位置に内在する第１の演算位置誤差を推定する。具体的には、例えば次式（４）に従って第１の演算位置誤差“Ｐ１err”を算出・推定する。
【数４】
【００５４】
式（４）において、“Δｔ”は、第１のカルマンフィルター処理の演算時間間隔である。状態“Ｘ1”の第１〜第３成分である速度誤差“（δＶE，δＶN，δＶU）”に演算時間間隔“Δｔ”を乗算することで、その演算時間間隔での位置誤差の変化分を求める。そして、当該変化分を状態“Ｘ2”の位置誤差“（δＰE，δＰN，δＰU）”に加算することで、第１の演算位置誤差“Ｐ１err”を算出する。
【００５５】
第２のカルマンフィルター処理部７Ａは、第１のカルマンフィルター処理部５Ａから入力した第１の演算速度及び第１の演算クロックドリフトを入力Ｕとし、ＧＰＳユニット３Ａから入力したＧＰＳ演算位置を観測量“Ｚ”として、移動体の位置を演算する。第２のカルマンフィルター処理部７Ａにより演算された位置は、第２の演算位置として加減算部８に出力される。
【００５６】
第２のカルマンフィルター処理では、推定対象とする移動体の状態“Ｘ”を、例えば次式（５）のように設定する。
【数５】
但し、“（ＰE，ＰN，ＰU）”は、ＥＮＵ座標系で表した移動体の位置である。“ｄ”はクロックドリフトである。
【００５７】
また、第２のカルマンフィルター処理では、観測量“Ｚ”を、例えば次式（６）のように設定する。
【数６】
但し、“（ＰX，ＰY，ＰZ）GPS”は、ＥＣＥＦ座標系で表したＧＰＳ演算位置である。
【００５８】
本実施形態において特徴的であるのは、第１のカルマンフィルター処理では、ＧＰＳ演算位置を観測量“Ｚ”とせず、ＧＰＳ演算速度を観測量“Ｚ”としている点である。その代わり、ＧＰＳ演算位置は、第２のカルマンフィルター処理の観測量“Ｚ”とする。これは、ＧＰＳでは種々の誤差要因が存在するため、ＧＰＳ演算位置に大きな誤差が含まれ得ることを想定したためである。その典型例はマルチパス環境である。
【００５９】
マルチパス環境でＧＰＳ演算を行うと、演算される位置の正確性が低下する。そのため、ＧＰＳ演算位置とＩＮＳ演算位置とを単純にカップリングしてしまうと、ＧＰＳ演算位置の誤差に引きずられて、演算位置の正確性が低下するおそれがある。そこで、位置演算を、第１の演算処理（第１のカルマンフィルター処理）と第２の演算処理（第２のカルマンフィルター処理）とに分離し、第１の演算処理では、ＧＰＳ演算位置を用いずに移動体の位置を演算することとした。
【００６０】
また、ＧＰＳでは、移動体の位置と併せて、移動体の速度も演算することができる。速度は位置と比べてマルチパスの影響を受けにくく、ＩＮＳ演算結果とのカップリングにそれほど大きな影響を与えない。そこで、第１のカルマンフィルター処理では、ＧＰＳ演算速度を観測量として用いて移動体の位置を演算することとした。
【００６１】
加減算部８は、第２のカルマンフィルター処理部７Ａから出力された第２の演算位置と、ＩＮＳユニット２Ａから出力されたＩＮＳ演算位置との差分を算出する。便宜的に、この差分のことを「第２の演算位置誤差」と呼称する。加減算部８により算出された第２の演算位置誤差“Ｐ２err”は、カルマンフィルター係数調整部９Ａに出力される。
【００６２】
カルマンフィルター係数調整部９Ａは、ＩＮＳ演算誤差推定部９１及びフィルター係数算出部９３を有し、第１のカルマンフィルター処理に係るフィルター係数を調整する。本実施形態では、フィルター係数算出部９３を、第１のカルマンフィルター処理部５Ａの機能部として図示・説明する。
【００６３】
フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１により推定されたＩＮＳ演算誤差を利用して、第１のカルマンフィルター処理に係るフィルター係数を算出する。調整するフィルター係数は、例えば、状態“Ｘ1”,“Ｘ2”、及び、誤差共分散行列“Ｐ11”，“Ｐ22”，“Ｐ21”，“Ｐ12”である。フィルター係数の調整方法については詳細に後述する。
【００６４】
ＩＮＳ演算誤差推定部９１は、ＩＮＳ演算結果に含まれる誤差であるＩＮＳ演算誤差を推定する機能部であり、例えば、位置誤差加重平均部９１１と、重み設定部９１３と、誤差変換部９１５とを有する。
【００６５】
位置誤差加重平均部９１１は、重み設定部９１３により設定された重みを用いて、第１の演算位置誤差“Ｐ１err”と第２の演算位置誤差“Ｐ２err”とを加重平均して、ＩＮＳユニット２Ａの演算位置誤差（慣性測位誤差）を算出する。具体的には、例えば次式（７）に従って平均位置誤差“ａｖｅＰerr”を算出する。
【数７】
但し、“α”は、第１の演算位置誤差“Ｐ１err”に対する重みであり、“０≦α≦１”である。第２の演算位置誤差“Ｐ２err”に対する重みは“１−α”である。
【００６６】
重み設定部９１３は、測位環境やＧＰＳユニット３Ａの計測結果の信頼性といった複数の要素に基づいて、位置誤差加重平均部９１１の加重平均の重み“α”を設定する。重み設定方法については詳細に後述する。
【００６７】
誤差変換部９１５は、第１のカルマンフィルター処理部５Ａから入力した誤差共分散“Ｐ”を用いて、位置誤差加重平均部９１１から入力した平均位置誤差“ａｖｅＰerr”をＩＮＳ演算誤差に変換する。ＩＮＳ演算誤差の変換方法についても詳細に後述する。
【００６８】
図３は、第１のカルマンフィルター処理部５Ａの入出力データの説明図である。状態“Ｘ”と、入力“Ｕ”と、観測量“Ｚ”との対応関係のテーブルを図示している。カップリングには種々の方式が存在する。その中でも、ルーズカップリング（疎結合）と呼ばれる方式と、タイトカップリング（密結合）と呼ばれる方式とが一般的に用いられる。
【００６９】
ルーズカップリング方式は、ＧＰＳとＩＮＳとの結び付きが比較的弱いカップリング方式である。この方式では、例えば、状態“Ｘ”をＩＮＳ演算誤差（ＩＮＳ演算位置誤差、ＩＮＳ演算速度誤差、ＩＮＳ演算姿勢角誤差、ＩＮＳ加速度バイアス、ＩＮＳ角速度バイアス、クロックバイアス等）とする。また、入力“Ｕ”をＩＮＳ演算結果（ＩＮＳ演算位置、ＩＮＳ演算速度、ＩＮＳ演算姿勢角等）とし、観測量“Ｚ”をＧＰＳ演算結果（ＧＰＳ演算速度等）や運動モデルに基づく制約条件（停止時又は移動時）とする。
【００７０】
タイトカップリング方式は、ＧＰＳとＩＮＳとの結び付きが比較的強いカップリング方式である。この方式では、例えば、入力“Ｕ”及び状態“Ｘ”を上記と同様とし、観測量“Ｚ”をＧＰＳメジャメント情報（擬似距離変化率等）や運動モデルに基づく制約条件とする。
【００７１】
上記の状態“Ｘ”、入力“Ｕ”、観測量“Ｚ”の各成分は、適宜追加／削除可能である。例えば、状態“Ｘ”について、バイアス成分を削除して、ＩＮＳ演算位置誤差、ＩＮＳ演算速度誤差及びＩＮＳ演算姿勢角誤差を状態“Ｘ”の成分としてもよい。また、観測量“Ｚ”について、ＧＰＳ演算速度又は擬似距離変化率と、運動モデルに基づく制約条件とを併用することとしてもよいし、何れか一方のみを用いることとしてもよい。
【００７２】
１−２．重み設定方法
次に、重み“α”の設定方法を説明する。本実施形態では、比較的高い値として定められた高設定値“αhigh”と、中程度の値として定められた中設定値“αmiddle”と、比較的低い値として定められた低設定値“αlow”との３種類の値の何れかを重み“α”に設定する。これらの具体的な値は、適宜定めることとしてよい。重み“α”は「０〜１」の範囲で、「αhigh＝０．９，αmiddle＝０．５，αlow＝０．１」といった値や、「αhigh＝０．７５，αmiddle＝０．５，αlow＝０．２５」といった値を定めておくことができる。
【００７３】
図４は、重み設定に係る第１の重み設定条件を定めた第１の重み設定条件テーブルの一例を示す図である。第１の重み設定条件テーブルには、低重み設定条件と、高重み設定条件とが対応付けて定められている。低重み設定条件は、重み“α”に低設定値“αlow”を設定するための条件である。高重み設定条件は、重み“α”に高設定値“αhigh”を設定するための条件である。低重み設定条件及び高重み設定条件の何れの条件も成立しない場合は、重み“α”に中設定値“αmiddle”を設定する。
【００７４】
第１の重み設定条件は、測位環境に基づき定められた条件である。低重み設定条件には、測位環境がマルチパス環境であることが定められており、高重み設定条件には、測位環境がオープンスカイ環境であることが定められている。
【００７５】
測位環境がマルチパス環境である場合は、間接波の影響により、観測される擬似距離に誤差が生ずる。そのため、ＧＰＳユニット３Ａにより演算される位置の正確性が低下する。第２のカルマンフィルター処理部７ＡではＧＰＳ演算位置を観測量“Ｚ”とするため、ＧＰＳ演算位置の正確性が低いと、第２の演算位置にもその影響が現れる。
【００７６】
この場合、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”は、マルチパスの影響を含む値となる。従って、測位環境がマルチパス環境である場合は、マルチパスの影響が顕著に反映された第２の演算位置誤差“Ｐ２err”を重視するために第２の演算位置誤差“Ｐ２err”に対する重み“１−α”を大きくする。これは、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくすることに相当する。
【００７７】
図５は、重み設定に係る第２の重み設定条件を定めた第２の重み設定条件テーブルの一例を示す図である。第２の重み設定条件テーブルには、条件の番号である条件Ｎｏと、低重み設定条件と、高重み設定条件とが対応付けて定められている。
【００７８】
条件“Ａ”は、ＧＰＳ演算位置の信頼性に関する条件である。低重み設定条件には「ＧＰＳ演算位置の信頼性＝低」が定められており、高重み設定条件には「ＧＰＳ演算位置の信頼性＝高」が定められている。
【００７９】
ＧＰＳ演算位置の信頼性が低いと、第２のカルマンフィルター処理部７Ａで演算される第２の演算位置の正確性が低下する。そのため、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”には、その影響が顕著に反映される。そこで、ＧＰＳ演算位置の信頼性が低い場合には、第２の演算位置誤差“Ｐ２err”を重視するために、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８０】
ＧＰＳ演算位置の信頼性は、ＧＰＳ衛星信号の受信環境や信号強度、ＧＰＳ衛星の天空配置、位置の急激な変化の検出といった複数の要素を総合的に考慮して判定することができる。例えば、ＧＰＳ衛星の天空配置が良好でない場合（例えばＤＯＰ（Dilution Of Precision）値が大きい場合）や、大きな位置飛び（真の位置から大きく離れた位置が演算結果として得られること）が発生した場合、高度方向の急激な位置変化が発生した場合に、ＧＰＳ演算位置の信頼性が低いと判定することができる。
【００８１】
条件“Ｂ”は、ＧＰＳメジャメント情報の信頼性に関する条件である。低重み設定条件には「ＧＰＳメジャメント情報の信頼性＝低」が定められており、高重み設定条件には「ＧＰＳメジャメント情報の信頼性＝高」が定められている。ＧＰＳメジャメント情報の信頼性が低いと、演算されるＧＰＳ演算位置の信頼性が低下する。従って、この場合は条件“Ａ”と同様に、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８２】
例えば、アーバンキャニオン環境等においてＧＰＳメジャメント情報に突発的に異常値が混入したような場合や、擬似距離変化率（レンジレート）と擬似距離（シュードレンジ）との相関関係に矛盾が生じている場合に、ＧＰＳメジャメント情報の信頼性は低いと判定する。
【００８３】
条件“Ｃ”は、第２の演算位置の信頼性に関する条件である。低重み設定条件には「第２の演算位置の信頼性＝低」が定められており、高重み設定条件には「第２の演算位置の信頼性＝高」が定められている。第２のカルマンフィルター処理で演算された第２の演算位置の信頼性が低い場合は、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”を重視するように、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８４】
例えば、移動体の速度はゼロであるか微小値であるにも関わらず第２の演算位置が不連続的に変化している場合や、第２の演算位置に突発的な位置飛びが発生した場合に、第２の演算位置の信頼性は低いと判定する。
【００８５】
条件“Ｄ”は、時刻情報の信頼性に関する条件である。低重み設定条件には「時刻情報の信頼性＝高」が定められており、高重み設定条件には「時刻情報の信頼性＝低」が定められている。時刻情報の信頼性が低いと、ＧＰＳユニット３Ａにより演算されるＧＰＳ演算位置の信頼性が低下する。そのため、条件“Ａ”と同様に、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
【００８６】
例えば、ＧＰＳの時刻同期誤差が所定の閾値を超えている（或いは閾値以上）場合に、時刻情報の信頼性は低いと判定する。
【００８７】
上記の複数の条件を用いて重み“α”を設定する。まず、複数の条件それぞれについて、判定項目の信頼性を数値化する。例えば、判定項目の信頼性が最も低い状態を信頼度“０”、信頼性が最も高い状態を信頼度“１”として、各判定項目の信頼度を「０〜１」の範囲で数値化する。
【００８８】
そして、例えば次式（８）に従って、重み設定指標値“Ｗ”を算出する。
【数８】
但し、“ｋ＝ｋA，ｋB，ｋC，・・・”は、各々の条件“Ａ，Ｂ，Ｃ，・・・”の信頼度に対する重みであり、“ｋA＋ｋB＋ｋC＋・・・＝Σｋ＝１”である。単純化するのであれば“ｋA＝ｋB＝ｋC＝・・・＝１／Ｎ”としてもよい。但し、“Ｎ”は、重み設定に用いる条件の総数である。
【００８９】
重み設定指標値“Ｗ”に基づいて、設定する重み“α”を決定する。例えば、重み設定指標値“Ｗ”が第１の閾値“θ1”よりも小さい（又は以下）場合は、重み“α”を低設定値とする（α＝αlow）。また、重み設定指標値“Ｗ”が第２の閾値“θ2（＞θ1）”よりも大きい（又は以上）場合は、重み“α”を高設定値とする（α＝αhigh）。また、上記以外の場合には、重み“α”を中設定値とする（α＝αmiddle）。
【００９０】
図４の第１の重み設定条件と、図５の第２の重み設定条件とは、それぞれ単体で用いてもよいし、併用してもよい。つまり、測位環境に基づいて重みを設定して加重平均することも可能であるし、ＧＰＳユニット３Ａの計測結果の信頼性等に基づいて重みを設定して加重平均することも可能であるし、これらの条件を併用して重みを設定して加重平均することも可能である。条件を併用する場合は、第１の重み設定条件の判定結果と第２の重み設定条件の判定結果とをＡＮＤ条件又はＯＲ条件として、重み“α”を設定することとしてもよい。
【００９１】
また、第１の重み設定条件を第２の重み設定条件に含めて判定を行うことも可能である。具体的には、例えば、測位環境が最も良好な状態（例えばオープンスカイ環境）を“１”、測位環境が最も悪い状態（例えばアーバンキャニオン環境）を“０”として、測位環境の良否を数値化する。そして、数値化した測位環境の良否を含めて、式（８）の重み設定指標値“Ｗ”を算出することとしてもよい。
【００９２】
１−３．ＩＮＳ演算誤差推定方法
次に、ＩＮＳ演算誤差の推定方法を説明する。平均位置誤差をＩＮＳ演算誤差に変換する方法としては、例えばカルマンフィルターを用いることができる。
【００９３】
最初に、次式（９）に従って、観測量“Ｚ2”と予測した観測量“Ｈ22Ｘ2”との差分“Ｖ2”を算出する。
【数９】
【００９４】
式（９）において、観測量“Ｚ2”は平均位置誤差“ａｖｅＰerr”である。すなわち、“Ｚ2＝ａｖｅＰerr”である。また、“Ｈ22Ｘ2”は観測量“Ｚ2”の予測値であり、平均位置誤差“ａｖｅＰerr”の予測値に相当する。つまり、式（２）で与えられる状態“Ｘ2”に観測行列“Ｈ22”を作用させて平均位置誤差“ａｖｅＰerr”を予測することを示す式である。観測行列“Ｈ22”は、状態“Ｘ2”から観測量“Ｚ2”への変換行列である。
【００９５】
次に、次式（１０）及び（１１）に従って、カルマンゲイン“Ｋ12”及び“Ｋ22”を算出する。
【数１０】
【数１１】
【００９６】
“Ｋ12”は差分“Ｖ2”から状態“Ｘ1”を演算するためのカルマンゲインであり、“Ｋ22”は差分“Ｖ2”から状態“Ｘ2”を演算するためのカルマンゲインである。また、“Ｒ2”は観測量“Ｚ2”の観測誤差（観測ノイズ）である。
【００９７】
そして、上記のカルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、状態“Ｘ1”及び“Ｘ2”を、次式（１２）及び（１３）に従って算出する。
【数１２】
【数１３】
【００９８】
式（１２）に従って算出された状態“Ｘ1”の成分のうち、速度誤差及び姿勢角誤差を、推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”として第１のカルマンフィルター処理部５Ａにフィードバックする。また、式（１３）に従って算出された状態“Ｘ2”の成分である位置誤差を、推定位置誤差“Ｐerr”として第１のカルマンフィルター処理部５Ａにフィードバックする。また、カルマンゲイン“Ｋ12”及び“Ｋ22”を第１のカルマンフィルター処理部５Ａにフィードバックする。
【００９９】
１−４．フィルター係数調整方法
次に、フィルター係数の調整方法を説明する。フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１から入力したＩＮＳ演算誤差（推定位置誤差“Ｐerr”、推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”）やカルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、フィルター係数を算出する。
【０１００】
第１に、フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１から入力した推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”を、状態“Ｘ1”のうちの対応する成分（速度誤差“（δＶE，δＶN，δＶU）”、姿勢角誤差“（Ψx，Ψy，Ψz）”）に加味して、状態“Ｘ1”を算出・更新する。また、ＩＮＳ演算誤差推定部９１から入力した推定位置誤差“Ｐerr”を状態“Ｘ2”の各成分（位置誤差“（δＰE，δＰN，δＰU）”）に加味して、状態“Ｘ2”を算出・更新する。
【０１０１】
第２に、フィルター係数算出部９３は、カルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、例えば次式（１４）〜（１７）に従って誤差共分散行列“Ｐ11”，“Ｐ22”，“Ｐ21”，“Ｐ12”をそれぞれ算出・更新する。
【数１４】
【数１５】
【数１６】
【数１７】
【０１０２】
２．実験結果
図６及び図７は、上記の位置算出方法を用いて位置算出を行った実験結果の一例を示す図である。第１の位置算出装置１Ａを自動車に設置し、都市部の道路を自動車で走行する実験を行った。走行した都市部は高層ビルが数多く存在するマルチパス環境である。図６及び図７の都市部の道路を反時計回りに周回して位置を算出し、その軌跡を描いてみた。スタート，ゴール地点は、図面中央下部である。
【０１０３】
図６（１）は、自動車の真の軌跡（参照軌跡）である。図６（２）は、ＧＰＳユニット３Ａの出力であるＧＰＳ演算位置の軌跡である。図６（３）は、第２のカルマンフィルター処理部７Ａの出力である第２の演算位置の軌跡である。
【０１０４】
図７（１）は、演算係数の調整を行わない場合の第１のカルマンフィルター処理部５Ａの出力である第１の演算位置の軌跡（演算係数調整なし）である。図７（２）は、演算係数の調整は行ったが、重み“α”の設定を行わなかった場合の第１の演算位置の軌跡（演算係数調整あり、重み設定なし）である。図７（３）は、演算係数の調整及び重み“α”の設定を両方とも行った場合の第１の演算位置の軌跡（演算係数調整あり、重み設定あり）である。
【０１０５】
最初に、図６（２）のＧＰＳ演算位置の軌跡を見ると、所々に大きく蛇行しており、位置誤差が非常に大きくなっていることがわかる。これは、マルチパスの影響によりＧＰＳの演算精度が低下したことによるものである。図６（３）の第２の演算位置の軌跡を見ると、ＧＰＳ演算位置の軌跡と比べると滑らかな軌跡を描いているが、それでもやはり、図６（１）の真の軌跡に沿った軌跡とはなっていない。
【０１０６】
次に、図７（１）の第１の演算位置の軌跡（演算係数調整なし）を見ると、点線部分Ｓ１において、真の軌跡に対してバイアスがかかったような軌跡が得られている。これに本実施形態の演算係数の調整を追加すると、図７（２）の結果が得られる。この図７（２）の第１の演算位置の軌跡（演算係数調整あり、重み設定なし）を見ると、演算係数の調整により位置算出精度が改善されていることがわかる。しかし、それでもなお、点線部分Ｓ２において真の軌跡からの位置ズレが生じている。
【０１０７】
これにさらに本実施形態の重み“α”の設定を追加すると、図７（３）の結果が得られる。この図７（３）の第１の演算位置の軌跡（演算係数調整あり、重み設定あり）を見ると、図６（１）の真の軌跡に沿った正確な軌跡が得られていることがわかる。特に、図７（１）の点線部分Ｓ１や図７（２）の点線部分Ｓ２に対応する部分についても位置算出精度が改善されている。この実験結果から、本実施形態の位置算出方法が有効であることが実証された。
【０１０８】
３．実施例
次に、上記の位置算出装置を備えた電子機器の実施例について説明する。ここでは、位置算出装置を具備するカーナビゲーション装置の実施例を説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【０１０９】
３−１．システム構成
図８は、本実施例におけるナビゲーションシステム１０００のシステム構成の説明図である。ナビゲーションシステム１０００は、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、位置算出装置１を具備したカーナビゲーション装置１００が設置されてなる。
【０１１０】
カーナビゲーション装置１００は、自動車に設置され、自動車の運転者に対するナビゲーションを行う電子機器である。カーナビゲーション装置１００は、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａとを備える。
【０１１１】
カーナビゲーション装置１００は、ＩＮＳユニット２Ａから入力したＩＮＳ演算結果と、ＧＰＳユニット３Ａから入力したＧＰＳ演算結果とを用いて、原理で説明した位置算出方法に従って自動車の位置を算出する。そして、算出位置をプロットしたナビゲーション画面を生成して、表示部３０であるディスプレイに表示させる。
【０１１２】
３−２．機能構成
図９は、カーナビゲーション装置１００の機能構成の一例を示すブロック図である。カーナビゲーション装置１００は、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａと、処理部１０と、操作部２０と、表示部３０と、通信部４０と、記憶部５０とを備える。
【０１１３】
処理部１０は、記憶部５０に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。処理部１０は、記憶部５０に記憶されたナビゲーションプログラム５１に従ってナビゲーション処理を行い、自動車の現在位置を指し示した地図を表示部３０に表示させる。
【０１１４】
操作部２０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部１０に出力する。この操作部２０の操作により、目的地の入力等の各種指示入力がなされる。
【０１１５】
表示部３０は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部１０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部３０には、ナビゲーション画面等が表示される。
【０１１６】
通信部４０は、処理部１０の制御に従って、装置内部で利用される情報をインターネット等の通信ネットワークを介して外部とやりとりするための通信装置である。この通信には、公知の無線通信技術を適用可能である。
【０１１７】
記憶部５０は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【０１１８】
記憶部５０には、プログラムとして、処理部１０により読み出され、ナビゲーション処理（図１０参照）として実行されるナビゲーションプログラム５１が記憶されている。ナビゲーションプログラム５１は、ＩＮＳ演算誤差推定処理（図１１参照）として実行されるＩＮＳ演算誤差推定プログラム５１１をサブルーチンとして含む。
【０１１９】
また、記憶部５０には、データとして、例えば、重み設定条件テーブル５３と、ＩＮＳ演算結果５５と、ＧＰＳ演算結果５６と、第１の演算結果５７と、第２の演算結果５８と、演算係数５９とが記憶される。
【０１２０】
重み設定条件テーブル５３は、第１の重み設定条件テーブル（図４参照）や第２の重み設定条件テーブル（図５参照）といった重み設定用の条件が定められたテーブルである。
【０１２１】
３−３．処理の流れ
図１０は、処理部１０が、記憶部５０に記憶されたナビゲーションプログラム５１に従って実行するナビゲーション処理の流れを示すフローチャートである。
【０１２２】
先ず、処理部１０は、ＩＮＳユニット２Ａ及びＧＰＳユニット３Ａから、ＩＮＳ演算結果５５及びＧＰＳ演算結果５６の取得を開始する（ステップＡ１）。そして、処理部１０は、第１のカルマンフィルター処理を実行する（ステップＡ３）。
【０１２３】
第１のカルマンフィルター処理では、前回のＩＮＳ演算誤差推定処理（ステップＡ９）の推定結果を用いてフィルター係数を調整し、記憶部５０の演算係数５９を更新する。具体的には、原理で説明したように、推定したＩＮＳ演算誤差を用いて状態“Ｘ”を調整するとともに、例えば式（１４）〜（１７）に従って誤差共分散“Ｐ”を調整する。そして、調整したフィルター係数を用いてカルマンフィルター処理を行い、その演算結果で記憶部５０の第１の演算結果５７を更新する。
【０１２４】
次いで、処理部１０は、第１のカルマンフィルター処理で演算した演算位置に対するマップマッチング処理等を行い、その結果で表示部３０のナビゲーション画面を表示更新する（ステップＡ５）。
【０１２５】
その後、処理部１０は、第２のカルマンフィルター処理を実行する（ステップＡ７）。処理部１０は、第２のカルマンフィルター処理の演算結果で記憶部５０の第２の演算結果５８を更新する。そして、処理部１０は、記憶部５０に記憶されたＩＮＳ演算誤差推定プログラム５１１に従ってＩＮＳ演算誤差推定処理を行う（ステップＡ９）。
【０１２６】
図１１は、ＩＮＳ演算誤差推定処理の流れを示すフローチャートである。
先ず、処理部１０は、重み設定処理を行う（ステップＢ１）。具体的には、記憶部５０に記憶されている重み設定条件テーブル５３を参照して、原理で説明した手法に従って重み“α”を設定する。
【０１２７】
次いで、処理部１０は、位置誤差加重平均処理を行う（ステップＢ３）。具体的には、第１のカルマンフィルター処理で演算した第１の演算位置誤差“Ｐ１err”と、第２のカルマンフィルター処理で演算した第２の演算位置誤差“Ｐ２err”とを、ステップＢ１で設定した重み“α”を用いて式（７）に従って加重平均して、平均位置誤差“ａｖｅＰerr”を算出する。
【０１２８】
その後、処理部１０は、誤差変換処理を行う（ステップＢ５）。具体的には、ステップＢ３で算出した平均位置誤差“ａｖｅＰerr”を、式（９）〜式（１３）に従ってＩＮＳ演算誤差に変換する。そして、処理部１０は、ＩＮＳ演算誤差推定処理を終了する。
【０１２９】
図１０のナビゲーション処理に戻り、ＩＮＳ演算誤差推定処理を行った後、処理部１０は、処理を終了するか否かを判定する（ステップＡ１１）。例えば、操作部２０を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。
【０１３０】
処理を終了しないと判定した場合は（ステップＡ１１；Ｎｏ）、処理部１０は、ステップＡ３に戻る。また、処理を終了すると判定した場合は（ステップＡ１１；Ｙｅｓ）、ナビゲーション処理を終了する。
【０１３１】
４．作用効果
本実施形態によれば、位置算出装置１において、移動体に設置された慣性測位用ユニット２の計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理が第１の演算処理部５により実行される。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニット３の計測結果とを用いて、移動体の位置を演算する第２の演算処理が第２の演算処理部７により実行される。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数が演算係数調整部９により調整される。
【０１３２】
例えば、位置算出装置１を適用した第１の位置算出装置１Ａでは、第１のカルマンフィルター処理部５Ａにおいて、ＩＮＳユニットの演算結果（ＩＮＳ演算結果）を入力とし、ＧＰＳユニット３Ａの演算速度（ＧＰＳ演算速度）を観測量とするカルマンフィルター処理が実行される。また、第２のカルマンフィルター処理部７Ａにおいて、第１のカルマンフィルター処理部５Ａの演算結果（第１の演算結果）を入力とし、ＧＰＳユニット３Ａの演算位置（ＧＰＳ演算位置）を観測量とするカルマンフィルター処理が実行される。
【０１３３】
加減算部８では、ＩＮＳ演算位置と第２の演算位置との差分が、第２の演算位置誤差として算出される。ＩＮＳ演算誤差推定部９１では、重み設定部９１３により、第１の演算位置誤差と第２の演算位置誤差とを加重平均する際の重みが設定される。そして、第１の演算位置誤差と第２の演算位置誤差とが、位置誤差加重平均部９１１により加重平均される。そして、加重平均結果である平均位置誤差を用いて、誤差変換部９１５により平均位置誤差がＩＮＳ演算誤差に変換され、その変換結果が第１のカルマンフィルター処理部５Ａにフィードバックされる。
【０１３４】
重み設定部９１３は、例えば、測位環境に基づき定められた第１の重み設定条件や、ＧＰＳ演算位置の信頼性、ＧＰＳメジャメント情報の信頼性、第２の演算位置の信頼性、時刻情報の信頼性といった複数の要素に基づき定められた第２の重み設定条件に従って、加重平均の重みを設定する。これにより、加重平均の重みを適正化し、ＩＮＳ演算位置に含まれる誤差を適切に推定することができる。
【０１３５】
また、誤差変換部９１５は、第１のカルマンフィルター処理部５Ａから入力した誤差共分散と、位置誤差加重平均部９１１から入力した平均位置誤差とを用いて、カルマンフィルターを利用した変換演算を行って、ＩＮＳ演算誤差を推定する。カルマンフィルターを利用することで、平均位置誤差からＩＮＳ演算誤差を適切に推定することができる。
【０１３６】
また、フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１により推定されたＩＮＳ演算誤差やカルマンゲインを用いて、第１のカルマンフィルター処理で用いるフィルター係数を算出・調整する。例えば、第１のカルマンフィルター処理で用いる状態（状態ベクトル）や誤差共分散（誤差共分散行列）を算出・調整する。このようにして調整したフィルター係数を用いて第１のカルマンフィルター処理を行うことで、移動体の位置を高い正確性で求めることが可能となる。
【０１３７】
５．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【０１３８】
５−１．ユニット
上記の実施形態では、衛星測位用ユニット３として、ＧＰＳを適用したＧＰＳユニット３Ａを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星測位システムを適用したユニットとしてもよいことは勿論である。
【０１３９】
また、上記の実施形態では、慣性測位用ユニット２として、ＩＮＳユニット２Ａを適用した場合を例に挙げて説明したが、ＩＮＳメジャメント情報（加速度や角速度）を計測する慣性センサーや慣性計測ユニット（ＩＭＵ）を慣性測位用ユニット２としてもよい。この場合は、慣性測位用ユニット２により計測されたＩＮＳメジャメント情報を入力Ｕとして、第１の演算処理部５が第１の演算処理を行うように構成すればよい。
【０１４０】
５−２．演算処理
第１の演算処理部５及び第２の演算処理部７が行う演算処理は、カルマンフィルター処理に限定されるわけではない。例えば、第２の演算処理について、シグマ点（シグマポイント）を利用したシグマポイントフィルター処理や、回帰フィルター処理を適用することとしてもよい。
【０１４１】
５−３．座標系
上記の実施形態では、ＩＮＳユニット２Ａは、移動体の位置、速度及び姿勢角をＥＮＵ座標系で演算するものとして説明したが、ＮＥＤ座標系や、ＥＣＥＦ座標系で演算することとしてもよいことは勿論である。また、第１の演算処理や第２の演算処理についても、ＥＮＵ座標系で演算するのではなく、ＮＥＤ座標系やＥＣＥＦ座標系で演算してもよい。
【０１４２】
５−４．重み設定条件
上記の実施形態で例示した重み設定条件はあくまでも一例であり、適宜追加／削除可能である。例えば、ＩＮＳユニット２Ａの演算結果の信頼性に基づく条件を定めておいてもよい。ＩＮＳユニット２Ａが有する慣性センサー（ジャイロセンサーや加速度センサー）の計測結果に大きな誤差が混入すると、ＩＮＳユニット２Ａの演算精度が低下する。この場合、ＩＮＳ演算結果を入力とする第１の演算処理の演算精度が低下する。
【０１４３】
その結果、第１の演算処理で演算される第１の演算位置誤差“Ｐ１err”には、その影響が顕著に現れる。そこで、ＩＮＳユニット２Ａの演算結果の信頼性が低い場合は、第１の演算位置誤差“Ｐ１err”を重視して加重平均するように、第１の演算位置誤差“Ｐ１err”に対する重み“α”を大きくすると効果的である。この場合は、高重み設定条件として「ＩＮＳユニットの信頼性＝低」を定めておけばよい。
【０１４４】
また、上記の実施形態では、第２の重み設定条件に関して、複数の条件“Ａ，Ｂ，Ｃ，Ｄ，・・・”を組み合わせて重み“α”を設定する方法を説明した。しかし、複数の条件“Ａ，Ｂ，Ｃ，Ｄ，・・・”をそれぞれ単体で用いて重み“α”を設定するといったことも当然に可能である。
【０１４５】
５−５．処理主体
上記の実施形態では、ＩＮＳユニット２ＡがＩＮＳ演算を行い、ＧＰＳユニット３ＡがＧＰＳ演算を行うものとして説明した。しかし、電子機器の処理部１０がＩＮＳ演算やＧＰＳ演算を実行する構成としてもよい。
【０１４６】
この場合は、ＩＮＳユニット２Ａは、ＩＮＳメジャメント情報（加速度や角速度等）を出力する。また、ＧＰＳユニット３Ａは、ＧＰＳメジャメント情報（コード位相やドップラー周波数、擬似距離、擬似距離変化率等）を出力する。
【０１４７】
そして、電子機器の処理部１０は、ＩＮＳユニット２Ａから入力したＩＮＳメジャメント情報を用いてＩＮＳ演算を行うとともに、ＧＰＳユニット３Ａから入力したＧＰＳメジャメント情報を用いてＧＰＳ演算を行う。そして、処理部１０は、ＩＮＳ演算結果及びＧＰＳ演算結果を用いて、第１及び第２の演算処理を行う。
【０１４８】
５−６．電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【０１４９】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【符号の説明】
【０１５０】
１、１Ａ 位置算出装置、 ２ 慣性測位用ユニット、 ２Ａ ＩＮＳユニット、 ３ 衛星測位用ユニット、 ３Ａ ＧＰＳユニット、 ５ 第１の演算処理部、 ５Ａ 第１のカルマンフィルター処理部、 ７ 第２の演算処理部、 ７Ａ 第２のカルマンフィルター処理部、 ９ 演算係数調整部、 ９Ａ カルマンフィルター係数調整部、 １０ 処理部、 ２０ 操作部、 ３０ 表示部、 ４０ 通信部、 ５０ 記憶部、 １００ カーナビゲーション装置、 １０００ ナビゲーションシステム
【特許請求の範囲】
【請求項１】
移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行することと、
前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行することと、
前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整することと、
を含む位置算出方法。
【請求項２】
前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、
前記第１の演算処理は、演算した第１の演算位置に内在する第１の位置誤差を推定する所定の誤差推定演算を含み、
前記調整することは、
前記第１の位置誤差と、前記第２の演算処理で求めた第２の演算位置とを用いて、前記慣性測位位置に内在する慣性測位誤差を推定することと、
前記慣性測位誤差を用いて前記演算係数を調整することと、
を含む、
請求項１に記載の位置算出方法。
【請求項３】
前記推定することは、
前記慣性測位位置と前記第２の演算位置との差分を算出することと、
前記第１の位置誤差と前記差分とを平均処理して前記慣性測位誤差を算出することと、
を含む、
請求項２に記載の位置算出方法。
【請求項４】
前記平均処理は、測位環境と前記衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、前記第１の位置誤差と前記差分との重みを設定して加重平均する処理である、
請求項３に記載の位置算出方法。
【請求項５】
前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、
前記第１の演算処理は、前記慣性測位位置を入力とするカルマンフィルター処理である、
請求項１〜４の何れか一項に記載の位置算出方法。
【請求項６】
前記衛星測位用ユニットの計測結果には速度が含まれ、
前記カルマンフィルター処理は、前記速度を観測量とする処理である、
請求項５に記載の位置算出方法。
【請求項７】
前記衛星測位用ユニットの計測結果には衛星測位位置が含まれ、
前記第２の演算処理は、前記第１の演算処理の結果を入力とし、前記衛星測位位置を観測量とするカルマンフィルター処理である、
請求項１〜６の何れか一項に記載の位置算出方法。
【請求項８】
移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行する第１の演算処理部と、
前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行する第２の演算処理部と、
前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整する調整部と、
を備えた位置算出装置。
【請求項１】
移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行することと、
前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行することと、
前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整することと、
を含む位置算出方法。
【請求項２】
前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、
前記第１の演算処理は、演算した第１の演算位置に内在する第１の位置誤差を推定する所定の誤差推定演算を含み、
前記調整することは、
前記第１の位置誤差と、前記第２の演算処理で求めた第２の演算位置とを用いて、前記慣性測位位置に内在する慣性測位誤差を推定することと、
前記慣性測位誤差を用いて前記演算係数を調整することと、
を含む、
請求項１に記載の位置算出方法。
【請求項３】
前記推定することは、
前記慣性測位位置と前記第２の演算位置との差分を算出することと、
前記第１の位置誤差と前記差分とを平均処理して前記慣性測位誤差を算出することと、
を含む、
請求項２に記載の位置算出方法。
【請求項４】
前記平均処理は、測位環境と前記衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、前記第１の位置誤差と前記差分との重みを設定して加重平均する処理である、
請求項３に記載の位置算出方法。
【請求項５】
前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、
前記第１の演算処理は、前記慣性測位位置を入力とするカルマンフィルター処理である、
請求項１〜４の何れか一項に記載の位置算出方法。
【請求項６】
前記衛星測位用ユニットの計測結果には速度が含まれ、
前記カルマンフィルター処理は、前記速度を観測量とする処理である、
請求項５に記載の位置算出方法。
【請求項７】
前記衛星測位用ユニットの計測結果には衛星測位位置が含まれ、
前記第２の演算処理は、前記第１の演算処理の結果を入力とし、前記衛星測位位置を観測量とするカルマンフィルター処理である、
請求項１〜６の何れか一項に記載の位置算出方法。
【請求項８】
移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行する第１の演算処理部と、
前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行する第２の演算処理部と、
前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整する調整部と、
を備えた位置算出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−２３７６０６（Ｐ２０１２−２３７６０６Ａ）
【公開日】平成２４年１２月６日（２０１２．１２．６）
【国際特許分類】
【出願番号】特願２０１１−１０５６１４（Ｐ２０１１−１０５６１４）
【出願日】平成２３年５月１０日（２０１１．５．１０）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年１２月６日（２０１２．１２．６）
【国際特許分類】
【出願日】平成２３年５月１０日（２０１１．５．１０）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]