JP5119967B2 - 測位方法、プログラム及び測位装置 - Google Patents

Description

本発明は、移動体に搭載される測位装置が所定の慣性航法用センサを用いた所定の慣性
航法演算処理を実行して前記移動体の現在位置を測位する測位方法等に関する。

人工衛星を利用した測位システムとしては、ＧＰＳ（Global Positioning System）が
広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された測位装置に利
用されている。ＧＰＳでは、自機の位置を示す３次元の座標値と、時計誤差との４つのパ
ラメータの値を、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星から自機までの擬似距離等の情
報に基づいて求める測位演算を行うことで、自機の現在位置を測位する。

しかし、トンネル内や屋内等、ＧＰＳ衛星信号を受信することができない環境では、Ｇ
ＰＳによる測位を行うことができないため、ジャイロセンサや加速度センサ等の慣性航法
用センサを用いた慣性航法演算処理を行って、現在位置を測位する技術が広く用いられて
いる。この技術に関連して、例えば非特許文献１や非特許文献２には、カルマンフィルタ
を用いて、慣性航法演算処理の演算結果を補正する技術が開示されている。
S.Godha and M.E.Cannon, "Integration of DGPS with a Low Cost MEMS - Based Inertial Measurement Unit(IMU) for Land Vehicle Navigation Application", ION(The Institute Of Navigation) GPS -05, September13-16, 2005, Long Beach, CA. S.Godha, M.G.Petovello, and G.Lachapelle, "Performance Analysis of MEMS IMU/HSGPS/MagneticSensor Integrated System in Urban Canyons", ION(The Institute Of Navigation) GNSS 2005, September13-16, 2005, Long Beach, CA.

非特許文献１や非特許文献２に開示された技術は、測位装置が搭載された移動体の速度
や高度に制約条件を設け、当該制約条件の下で慣性航法演算処理の演算結果に含まれる誤
差を推定して、当該演算結果を補正する技術である。具体的には、非特許文献１では、測
位装置が搭載された移動体の直進方向に対する左右方向及び高さ方向の速度を“０”とす
る速度制約条件が設けられており、非特許文献２では、移動体が一定の高度を走行すると
いう高度制約条件が設けられている。

しかし、自動車等の移動体を想定した場合、上述した速度制約条件や高度制約条件が満
たされる状況は稀であると考えられる。すなわち、自動車は左右に転回したり、坂道を走
行したりする場合が多々あるため、直進方向に対する左右方向及び高さ方向の速度を“０
”としたり、走行高度が一定であるとする制約条件は現実的ではない。

本発明は、上述した非現実的な制約条件の下での誤差推定ではなく、現実的な移動体の
移動を想定した適切な誤差推定を実現する測位方法等を提供することを目的として為され
たものである。

以上の課題を解決するための第１の発明は、移動体に搭載される測位装置が所定の慣性
航法用センサを用いた所定の慣性航法演算処理を実行して前記移動体の現在位置を測位す
る測位方法であって、前記慣性航法演算処理により随時求められる相対高度を第１高度変
化として算出することと、前記移動体の外気圧を検出する気圧センサの検出結果に基づい
て、相対高度を第２高度変化として算出することと、前記第１高度変化と前記第２高度変
化との差を算出することと、前記高度変化の差、前記慣性航法演算処理により求められる
前記移動体の直進方向に対する左右方向の速度、及び、前記慣性航法演算処理により求め
られる前記移動体の高度方向の速度に応じた観測値を用いたカルマンフィルタ処理を行っ
て、前記慣性航法演算処理の演算結果に含まれる誤差を推定することと、前記誤差を用い
て前記慣性航法演算処理の演算結果を補正して、前記移動体の現在位置を測位することと
、を含む測位方法である。

また、他の発明として、移動体に搭載され、所定の慣性航法用センサを用いた所定の慣
性航法演算処理を実行して前記移動体の現在位置を測位する測位装置であって、前記慣性
航法演算処理により随時求められる相対高度を第１高度変化として算出する第１高度変化
算出部と、前記移動体の外気圧を検出する気圧センサの検出結果に基づいて、相対高度を
第２高度変化として算出する第２高度変化算出部と、前記第１高度変化と前記第２高度変
化との差を算出する高度変化差算出部と、前記高度変化の差、前記慣性航法演算処理によ
り求められる前記移動体の直進方向に対する左右方向の速度、及び、前記慣性航法演算処
理により求められる前記移動体の高度方向の速度に応じた観測値を用いたカルマンフィル
タ処理を行って、前記慣性航法演算処理の演算結果に含まれる誤差を推定するカルマンフ
ィルタ処理部と、前記誤差を用いて前記慣性航法演算処理の演算結果を補正して、前記移
動体の現在位置を測位する測位部と、を備えた測位装置を構成してもよい。

この第１の発明等によれば、慣性航法演算処理により随時求められる相対高度である第
１高度変化と、移動体の外気圧を検出する気圧センサの検出結果に基づいて算出される相
対高度である第２高度変化との差が算出される。そして、算出された高度変化の差、慣性
航法演算処理により求められる移動体の直進方向に対する左右方向の速度、及び、慣性航
法演算処理により求められる高度方向の速度に応じた観測値を用いたカルマンフィルタ処
理が行われて、慣性航法演算処理の演算結果に含まれる誤差が推定され、推定された誤差
を用いて慣性航法演算処理の演算結果が補正されて、移動体の現在位置が測位される。

例えば、移動体が直進方向に対する左右方向に回転していることが想定される場合は、
左右方向の速度を観測値として用いず、移動体が高度方向に変位していることが想定され
る場合は、高度方向の速度を観測値として用いずにカルマンフィルタ処理を行うようにす
る。これにより、現実的な移動体の移動を想定した適切な誤差推定を実現することができ
、ひいては、移動体の現在位置をより正確に測位することが可能となる。

また、第２の発明として、第１の発明の測位方法であって、前記慣性航法用センサの検
出結果から、前記移動体の直進方向に対する左右方向の回転有無を判定することを更に含
み、前記誤差を推定することは、前記左右方向の回転が無いと判定された場合には、前記
左右方向の速度及び前記高度変化の差それぞれに応じた観測値を少なくとも用いたカルマ
ンフィルタ処理を行い、前記左右方向の回転が有ると判定された場合には、前記左右方向
の速度に応じた観測値を用いずに、前記高度変化の差に応じた観測値を少なくとも用いた
カルマンフィルタ処理を行って、前記慣性航法演算処理の演算結果に含まれる誤差を推定
することである測位方法を構成してもよい。

この第２の発明によれば、慣性航法用センサの検出結果から、移動体の直進方向に対す
る左右方向の回転有無が判定される。そして、左右方向の回転が無いと判定された場合に
は、左右方向の速度及び高度変化の差それぞれに応じた観測値を少なくとも用いたカルマ
ンフィルタ処理が行われる。また、左右方向の回転が有ると判定された場合には、左右方
向の速度に応じた観測値を用いずに、高度変化の差に応じた観測値を少なくとも用いたカ
ルマンフィルタ処理が行われて、慣性航法演算処理の演算結果に含まれる誤差が推定され
る。

移動体が直進方向に対する左右方向に回転していることが想定される場合に、当該左右
方向の速度が“０”であるとする速度制約条件を用いてカルマンフィルタ処理を行ったの
では、慣性航法演算処理の演算結果に含まれる誤差を適切に推定することができない。し
かし、第２の発明では、移動体の直進方向に対する左右方向の回転が有ると判定された場
合には、左右方向の速度をカルマンフィルタ処理の観測値として用いないこととしている
ため、上述した問題は生じず、慣性航法演算処理の演算結果に含まれる誤差を適切に推定
することができる。

また、第３の発明として、第２の発明の測位方法であって、前記慣性航法用センサは角
速度を検出するセンサを有して構成され、前記左右方向の回転有無を判定することは、前
記検出された角速度又は当該角速度の時間微分である角加速度が所定の閾値条件を満たす
か否かによって判定することである測位方法を構成してもよい。

この第３の発明によれば、慣性航法用センサにより検出された角速度又は当該角速度の
時間微分である角加速度が所定の閾値条件を満たすか否かによって、左右方向の回転有無
が判定される。

また、第４の発明として、第１〜第３の何れかの発明の測位方法であって、前記誤差を
推定することは、前記第２高度変化が無しの場合には、前記高度方向の速度及び前記高度
変化の差それぞれに応じた観測値を少なくとも用いたカルマンフィルタ処理を行い、前記
第２高度変化が有る場合には、前記高度方向の速度に応じた観測値を用いずに、前記高度
変化の差に応じた観測値を少なくとも用いたカルマンフィルタ処理を行って、前記慣性航
法演算処理の演算結果に含まれる誤差を推定することである測位方法を構成してもよい。

この第４の発明によれば、第２高度変化が無しの場合には、高度方向の速度及び高度変
化の差それぞれに応じた観測値を少なくとも用いたカルマンフィルタ処理が行われる。一
方、第２高度変化が有る場合には、高度方向の速度に応じた観測値を用いずに、高度変化
の差に応じた観測値を少なくとも用いたカルマンフィルタ処理が行われて、慣性航法演算
処理の演算結果に含まれる誤差が推定される。

移動体が高度方向に変位していることが想定される場合に、移動体の高度方向の速度が
“０”であるとする速度制約条件を用いてカルマンフィルタ処理を行ったのでは、慣性航
法演算処理の演算結果に含まれる誤差を適切に推定することができない。しかし、第４の
発明では、気圧センサの検出結果に基づいて算出された移動体の高度変化が有りである場
合は、高度方向の速度に応じた観測値を用いないこととしているため、上述した問題は生
じず、慣性航法演算処理の演算結果に含まれる誤差を適切に推定することができる。

また、第５の発明として、第１〜第４の発明の測位方法であって、前記移動体が停止し
たときに、気温を検出する温度センサの検出温度と前記慣性航法用センサの検出結果とを
対応付けて記憶することと、前記記憶された前記温度センサの検出温度と前記慣性航法用
センサの検出結果とから、任意の気温に対する前記慣性航法用センサの検出誤差を補償す
る温度補償モデル式を算出することと、前記温度センサの現在の検出温度に対応する前記
慣性航法用センサの検出誤差の補償値を前記温度補償モデル式から求め、前記慣性航法用
センサの検出結果を補償することと、を更に含む測位方法を構成してもよい。

この第５の発明によれば、移動体が停止したときに、気温を検出する温度センサの検出
温度と慣性航法用センサの検出結果とが対応付けて記憶され、記憶された温度センサの検
出温度と慣性航法用センサの検出結果とから、任意の気温に対する慣性航法用センサの検
出誤差を補償する温度補償モデル式が算出される。そして、温度センサの現在の検出温度
に対応する慣性航法用センサの検出誤差の補償値が温度補償モデル式から求められ、慣性
航法用センサの検出結果が補償される。

また、第６の発明として、第１〜第５の何れかの発明の測位方法を、移動体に搭載され
る測位装置に内蔵されたコンピュータに実行させるためのプログラムを構成してもよい。

以下、図面を参照して、本発明に好適な実施形態の一例を説明する。尚、以下では、測
位装置として、移動体の一種である自動車に搭載されるカーナビゲーション装置を例に挙
げ、測位システムとしてＧＰＳを用いた場合について説明するが、本発明を適用可能な実
施形態がこれらに限定されるわけではない。

１．原理
先ず、本実施形態における現在位置の測位の原理について説明する。本実施形態では、
図１に示すように、カーナビゲーション装置１を備えた移動体である自動車の進行方向を
「ｙ軸」（直進方向を正方向）、ｙ軸正方向に向かって左右方向を「ｘ軸」（右方向を正
方向）、ｙ軸に対する高さ方向を「ｚ軸」（上方向を正方向）とする直交３軸の移動体座
標系を定義する。

カーナビゲーション装置１は、慣性航法用センサとして、それぞれ３軸の加速度センサ
及びジャイロセンサを備えている。以下実施形態においては、これらのセンサの３軸は、
上述した移動体座標系の３軸にそれぞれ対応するように定められているとする。尚、セン
サの３軸は、必ずしも移動体座標系の３軸に対応させる必要はない。なぜならば、例えば
センサの出力値に対して公知の行列演算を行うことで、移動体座標系の３軸の値に変換（
座標変換）することができるからである。但し、以下の実施形態においては、説明を簡明
化するために、センサが検出する３軸は、移動体座標系の３軸と同じとして説明する。

カーナビゲーション装置１は、慣性航法用センサの検出結果に基づいて公知の慣性航法
演算処理を行って、自動車の位置、速度及び姿勢を演算する。そして、カルマンフィルタ
処理（以下、適宜「ＫＦ（Kalman Filter）処理」と称す。）を行うことで、慣性航法演
算処理の演算結果に含まれる誤差と、慣性航法用センサのバイアスとを推定し、推定した
誤差を用いて慣性航法演算処理の演算結果を補正するとともに、推定したバイアスを用い
て慣性航法用センサのバイアス補正を行う。

カルマンフィルタは、誤差を含む観測値を利用して、時々刻々と変化する状態量を推定
する確率理論に基づく推定方法である。カルマンフィルタでは、状態ベクトルと呼ばれる
ベクトルを用いて推定演算を行う。尚、カルマンフィルタそのものは公知の技術であるた
め、推定演算の詳細については説明を省略する。

本実施形態では、次式（１）で表わされる状態ベクトル「Ｘ」を用いて推定演算を行う
。

ここで、「δｐ」は慣性航法演算処理により求められた位置に含まれる誤差、「δｖ」
は慣性航法演算処理により求められた速度に含まれる誤差、「ε」は慣性航法演算処理に
より求められた姿勢に含まれる誤差、「ｂ_a」は加速度センサのバイアス、「ｂ_g」はジャ
イロセンサのバイアスをそれぞれ示している。これら５つの成分は、それぞれ３次元のベ
クトルで表わされるため、状態ベクトル「Ｘ」は全部で１５次元のベクトルとなる。

カーナビゲーション装置１は、ジャイロセンサの検出結果に基づいて、自動車のｚ軸回
りの回転（いわゆるヨー角。）の有無を判定する。また、カーナビゲーション装置１は気
圧センサを備えており、この気圧センサにより検出された自動車の外気圧に基づいて、自
動車の相対高度を第２高度変化として算出する。そして、ｚ軸回りの回転の有無と、第２
高度変化の有無とに応じて、カルマンフィルタに入力する観測値を可変に設定して推定演
算を行い、求められた状態ベクトル「Ｘ＝（δｐ，δｖ，ε，ｂ_a，ｂ_g）」の各成分を用
いて、慣性航法演算処理の演算結果の補正及び慣性航法用センサのバイアス補正を行う。
以下、その詳細について説明する。

１−１．ｚ軸回りの回転が有りで、第２高度変化が有りの場合
図２は、ｚ軸回りの回転が有りで、第２高度変化も有りの場合における誤差推定の流れ
を示す図である。この場合は、カーナビゲーション装置１は、慣性航法用センサであるジ
ャイロセンサ及び加速度センサそれぞれから出力される角速度「ω」及び加速度「ａ」を
用いて慣性航法演算処理を行い、自動車の位置「ｐⁱ」、速度「ｖⁱ」及び姿勢「Ａⁱ」を
演算する。

また、カーナビゲーション装置１は、慣性航法演算処理において、現在の時刻と１時刻
前（測位演算は所定時間間隔で行っており、各測位タイミングを「時刻」という。「１時
刻前」とは前回の測位タイミングのことである。）との自動車の相対高度を第１高度変化
「Δｈⁱ」として算出し、第１高度変化「Δｈⁱ」と第２高度変化「Δｈ^b」との差「δｈ
＝Δｈⁱ−Δｈ^b」を観測値とする第１ＫＦ処理を行って、状態ベクトル「Ｘ＝（δｐ，δ
ｖ，ε，ｂ_a，ｂ_g）」を演算する。

そして、慣性航法演算処理の演算結果「（ｐⁱ，ｖⁱ，Ａⁱ）」に、推定した誤差「（δ
ｐ，δｖ，ε）」を加算することで、慣性航法演算処理の演算結果を補正する。また、推
定した誤差「（δｐ，δｖ，ε）」をフィードバックして次の時刻（次の測位タイミング
）における慣性航法演算処理に反映させるとともに、バイアス「（ｂ_a，ｂ_g）」を慣性航
法用センサにフィードバックして、慣性航法用センサのバイアス補正を行う。

１−２．ｚ軸回りの回転が無しで、第２高度変化が有りの場合
図３は、ｚ軸回りの回転が無しで、第２高度変化が有りの場合における誤差推定の流れ
を示す図である。この場合は、速度制約条件としてｘ軸方向（自動車の直進方向に対する
左右方向）への速度「ｖ^r _x」を“０”に設定し（ｖ^r _x＝０）、慣性航法演算処理により求
めたｘ軸方向への速度「ｖⁱ _x」と「ｖ^r _x＝０」との差「δｖ_x＝ｖⁱ _x−ｖ^r _x＝ｖⁱ _x」と、
第１高度変化と第２高度変化との差「δｈ」とを観測値とする第２ＫＦ処理を行って、状
態ベクトル「Ｘ＝（δｐ，δｖ，ε，ｂ_a，ｂ_g）」を演算する。そして、慣性航法演算処
理の演算結果「（ｐⁱ，ｖⁱ，Ａⁱ）」の補正及び慣性航法用センサのバイアス補正を行う
。

１−３．ｚ軸回りの回転が有りで、第２高度変化が無しの場合
図４は、ｚ軸回りの回転が有りで、第２高度変化が無しの場合における誤差推定の流れ
を示す図である。この場合は、速度制約条件としてｚ軸方向（自動車の直進方向に対する
上下方向）への速度「ｖ^r _z」を“０”に設定し（ｖ^r _z＝０）、慣性航法演算処理により求
めたｚ軸方向への速度「ｖⁱ _z」と「ｖ^r _z＝０」との差「δｖ_z＝ｖⁱ _z−ｖ^r _z＝ｖⁱ _z」と、
第１高度変化と第２高度変化との差「δｈ」とを観測値とする第３ＫＦ処理を行って、状
態ベクトル「Ｘ＝（δｐ，δｖ，ε，ｂ_a，ｂ_g）」を演算する。そして、慣性航法演算処
理の演算結果「（ｐⁱ，ｖⁱ，Ａⁱ）」の補正及び慣性航法用センサのバイアス補正を行う
。

１−４．ｚ軸回りの回転が無しで、第２高度変化が無しの場合
図５は、ｚ軸回りの回転が無しで、第２高度変化も無しの場合における誤差推定の流れ
を示す図である。この場合は、速度制約条件として、ｘ軸方向への速度「ｖ^r _x」と、ｚ軸
方向への速度「ｖ^r _z」とをそれぞれ“０”に設定し（ｖ^r _x＝ｖ^r _z＝０）、「δｖ_x＝ｖⁱ _x
−ｖ^r _x＝ｖⁱ _x」と、「δｖ_z＝ｖⁱ _z−ｖ^r _z＝ｖⁱ _z」と、「δｈ」との３つを観測値とする
第４ＫＦ処理を行って、状態ベクトル「Ｘ＝（δｐ，δｖ，ε，ｂ_a，ｂ_g）」を演算する
。そして、慣性航法演算処理の演算結果「（ｐⁱ，ｖⁱ，Ａⁱ）」の補正及び慣性航法用セ
ンサのバイアス補正を行う。

２．機能構成
図６は、本実施形態におけるカーナビゲーション装置１の機能構成を示すブロック図で
ある。カーナビゲーション装置１は、ＧＰＳアンテナ１０と、ＧＰＳ受信部２０と、ホス
トＣＰＵ（Central Processing Unit）３０と、操作部４０と、表示部５０と、加速度セ
ンサ６０と、ジャイロセンサ７０と、気圧センサ８０と、温度センサ９０と、ＲＯＭ（Re
ad Only Memory）１００と、ＲＡＭ（Random Access Memory）１１０とを備えて構成され
る。

ＧＰＳアンテナ１０は、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を含むＲＦ（Ra
dio Frequency）信号を受信するアンテナであり、受信した信号をＧＰＳ受信部２０に出
力する。尚、ＧＰＳ衛星信号は、衛星毎に異なる拡散符号の一種であるＰＲＮ（Pseudo R
andom Noise）コードで直接スペクトラム拡散方式により変調された１．５７５４２［Ｇ
Ｈｚ］の通信信号である。ＰＲＮコードは、コード長１０２３チップを１ＰＮフレームと
する繰返し周期１ｍｓの擬似ランダム雑音符号である。

ＧＰＳ受信部２０は、ＧＰＳアンテナ１０から出力された信号に基づいてカーナビゲー
ション装置１の現在位置を測位する測位回路であり、いわゆるＧＰＳ受信機に相当する機
能ブロックである。ＧＰＳ受信部２０は、ＲＦ（Radio Frequency）受信回路部２１と、
ベースバンド処理回路部２３とを備えて構成される。尚、ＲＦ受信回路部２１と、ベース
バンド処理回路部２３とは、それぞれ別のＬＳＩ（Large Scale Integration）として製
造することも、１チップとして製造することも可能である。

ＲＦ受信回路部２１は、ＲＦ信号の処理回路ブロックであり、所定の局部発振信号を分
周或いは逓倍することで、ＲＦ信号乗算用の発振信号を生成する。そして、生成した発振
信号を、ＧＰＳアンテナ１０から出力されたＲＦ信号に乗算することで、ＲＦ信号を中間
周波数の信号（以下、「ＩＦ（Intermediate Frequency）信号」と称す。）にダウンコン
バートし、ＩＦ信号を増幅等した後、Ａ／Ｄ変換器でデジタル信号に変換して、ベースバ
ンド処理回路部２３に出力する。

ベースバンド処理回路部２３は、ＲＦ受信回路部２１から出力されたＩＦ信号に対して
相関処理等を行ってＧＰＳ衛星信号を捕捉・抽出し、データを復号して航法メッセージや
時刻情報等を取り出して測位演算を行う回路部である。ベースバンド処理回路部２３は、
プロセッサとしてのＣＰＵと、メモリとしてのＲＯＭ及びＲＡＭを備えて構成される。尚
、測位演算としては、例えば最小二乗法を用いた測位演算等の公知の手法を適用すること
ができる。

ホストＣＰＵ３０は、ＲＯＭ１００に記憶されているシステムプログラム等の各種プロ
グラムに従ってカーナビゲーション装置１の各部を統括的に制御するプロセッサである。
ホストＣＰＵ３０は、測位プログラム１０３に従って、ナビゲーション画面に表示させる
位置である出力位置を決定する。そして、ナビゲーションプログラム１０１に従って、出
力位置をプロットしたナビゲーション画面を生成して、表示部５０に表示させる。

操作部４０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であ
り、押下されたアイコンやボタンの信号をホストＣＰＵ３０に出力する。この操作部４０
の操作により、目的地の入力や、ナビゲーション画面の表示要求等の各種指示入力がなさ
れる。

表示部５０は、ＬＣＤ（Liquid Crystal Display）等により構成され、ホストＣＰＵ３
０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部５０には、
ナビゲーション画面等が表示される。

加速度センサ６０は、加速度を検出して自動車の移動状態を検出する３軸のセンサであ
り、検出した３軸の加速度をホストＣＰＵ３０に出力する。加速度センサ６０は、慣性航
法用センサの一種である。

ジャイロセンサ７０は、角速度を検出して自動車の回転を検出する３軸のセンサ（角速
度センサ）であり、検出した３軸の角速度をホストＣＰＵ３０に出力する。ジャイロセン
サ７０も、慣性航法用センサの一種である。

ジャイロセンサ７０の出力には、いわゆるゼロ点バイアスが含まれている。ゼロ点バイ
アスは、角速度が無い状態（例えば自動車の完全停止時（静止時））におけるジャイロセ
ンサ７０の出力値であり、温度変化によって変動する温度依存性がある。温度「Ｔ」にお
けるゼロ点バイアス「Ｂ_T」は、例えば次式（２）に示すような２次の温度補償モデル式
で表すことができる。

但し、「Ｔ₀」は基準温度、「Ｂ_T0」は基準温度Ｔ₀におけるゼロ点バイアス、「ｋ₁」
は１次の温度係数、「ｋ₂」は２次の温度係数をそれぞれ示している。

ホストＣＰＵ３０は、測位処理において、自動車が停止（静止）していると判定した場
合に、フィールドキャリブレーションを行って、上述した２次の温度補償モデル式を算出
する。そして、算出した温度補償モデル式を用いて、ジャイロセンサ７０のゼロ点バイア
スの補正を行う。

気圧センサ８０は、自動車の外気圧を検出するセンサであり、その検出結果をホストＣ
ＰＵ３０に出力する。

温度センサ９０は、外界の気温を検出する接触式又は非接触式のセンサであり、その検
出結果をホストＣＰＵ３０に出力する。

ＲＯＭ１００は、読み出し専用の記憶装置であり、ホストＣＰＵ３０がカーナビゲーシ
ョン装置１を制御するためのシステムプログラムや、ナビゲーション機能を実現するため
の各種プログラムやデータ等を記憶している。

ＲＡＭ１１０は、読み書き可能な記憶装置であり、ホストＣＰＵ３０により実行される
システムプログラム、各種処理プログラム、各種処理の処理中データ、処理結果などを一
時的に記憶するワークエリアを形成している。

図７は、ＲＯＭ１００に格納されたデータの一例を示す図である。ＲＯＭ１００には、
ホストＣＰＵ３０により読み出され、ナビゲーション処理として実行されるナビゲーショ
ンプログラム１０１と、測位処理（図１０及び図１１参照）として実行される測位プログ
ラム１０３と、ＫＦ誤差推定処理として実行されるＫＦ誤差推定プログラム１０５と、ナ
ビゲーション画面を生成するための地図情報のデータである地図データ１０７とが記憶さ
れている。

また、ＫＦ誤差推定プログラム１０５には、第１ＫＦ処理、第２ＫＦ処理、第３ＫＦ処
理及び第４ＫＦ処理として実行される第１ＫＦ処理プログラム１０５１、第２ＫＦ処理プ
ログラム１０５２、第３ＫＦ処理プログラム１０５３及び第４ＫＦ処理プログラム１０５
４が、サブルーチンとして含まれている。

ナビゲーション処理とは、ホストＣＰＵ３０が、地図データ１０７に記憶されている地
図情報を用いて、測位処理により決定した出力位置を道路上に補正するマップマッチング
処理を行うとともに、補正後の位置をプロットしたナビゲーション画面を生成して、表示
部５０に表示させる処理である。尚、マップマッチング処理については公知であるため、
詳細な説明を省略する。

測位処理とは、ホストＣＰＵ３０が、慣性航法演算処理を行って、自動車の位置、速度
及び姿勢を演算するとともに、自動車のｚ軸回りの回転の有無及び第２高度変化の有無に
応じて観測値を可変に設定してＫＦ処理を行うことで、慣性航法演算処理の演算結果に含
まれる誤差を推定する処理である。そして、ホストＣＰＵ３０は、推定した誤差を用いて
慣性航法演算処理の演算結果を補正する。

また、ホストＣＰＵ３０は、ＧＰＳ受信部２０から測位結果を取得した場合は、当該測
位結果であるＧＰＳ測位位置を出力位置に決定し、ＧＰＳ受信部２０から測位結果を取得
しなかった場合は、補正によって得られた慣性航法演算処理の演算結果に含まれる慣性航
法演算位置を出力位置に決定する。測位処理については、フローチャートを用いて詳細に
後述する。

ＫＦ誤差推定処理とは、ホストＣＰＵ３０が、高度変化の差「δｈ」や、ｘ軸及びｚ軸
に対する速度の差「δｖ_x」及び「δｖ_z」を観測値とするＫＦ処理を行って、慣性航法演
算処理の演算結果に含まれる誤差「（δｐ，δｖ，ε）」及び慣性航法用センサのバイア
ス「（ｂ_a，ｂ_g）」を推定する処理である。

図８は、ＲＡＭ１１０に格納されるデータの一例を示す図である。ＲＡＭ１１０には、
計測履歴データ１１１と、ゼロ点バイアス参照データ１１３と、温度補償モデル式データ
１１５とが記憶される。

図９は、計測履歴データ１１１のデータ構成の一例を示す図である。計測履歴データ１
１１には、各時刻１１１１毎に、加速度センサ６０により検出された３軸の加速度１１１
２と、ジャイロセンサ７０により検出された３軸の角速度１１１３と、気圧センサ８０に
より検出された外気圧１１１４と、温度センサ９０により検出された温度１１１５と、Ｇ
ＰＳ受信部２０により測位されたＧＰＳ測位位置１１１６と、慣性航法演算処理により求
められた慣性航法演算位置１１１７と、慣性航法演算処理により求められた速度１１１８
及び姿勢１１１９とが対応付けて記憶される。計測履歴データ１１１は、測位処理におい
てホストＣＰＵ３０により随時更新される。

ゼロ点バイアス参照データ１１３は、自動車の停止時において温度センサ９０により検
出された温度と、当該温度におけるゼロ点バイアスとが対応付けて蓄積記憶されたデータ
である。測位処理において、ホストＣＰＵ３０は、ゼロ点バイアス参照データ１１３に記
憶されたデータを基に、例えば最小二乗法を用いて１次の温度係数「ｋ₁」及び２次の温
度係数「ｋ₂」を求めることで、式（２）で表わされる２次の温度補償モデル式を算出す
る。

温度補償モデル式データ１１５は、ホストＣＰＵ３０により算出された温度補償モデル
式が記憶されたデータである。

３．処理の流れ
図１０及び図１１は、ホストＣＰＵ３０によりＲＯＭ１００に記憶されている測位プロ
グラム１０３が読み出されて実行されることで、カーナビゲーション装置１において実行
される測位処理の流れを示すフローチャートである。

測位処理は、ＲＦ受信回路部２１によるＧＰＳ衛星信号の受信と併せて、ホストＣＰＵ
３０が、操作部４０に測位開始指示の操作がなされたことを検出した場合に実行を開始す
る処理である。尚、カーナビゲーション装置１の電源のＯＮ／ＯＦＦとＧＰＳの起動／停
止とを連動させ、カーナビゲーション装置１の電源投入操作を検出した場合に測位処理の
実行を開始させることにしてもよい。

また、特に説明しないが、以下の測位処理の実行中は、ＧＰＳアンテナ１０によるＲＦ
信号の受信や、ＲＦ受信回路部２１によるＩＦ信号へのダウンコンバート、ベースバンド
処理回路部２３によるＧＰＳ衛星信号の捕捉・追尾、疑似距離の演算や測位演算等が随時
行われている状態にあるものとする。また、加速度センサ６０、ジャイロセンサ７０、気
圧センサ８０及び温度センサ９０の検出結果に従って、ホストＣＰＵ３０により、ＲＡＭ
１１０の計測履歴データ１１１は随時更新されるものとする。

先ず、ホストＣＰＵ３０は、例えば加速度センサ６０の検出結果に基づいて、自動車が
停止しているか否かを判定し（ステップＡ１）、停止していないと判定した場合は（ステ
ップＡ１；Ｎｏ）、ステップＡ１３へと処理を移行する。また、停止していると判定した
場合は（ステップＡ１；Ｙｅｓ）、温度センサ９０から現在の温度を取得する（ステップ
Ａ３）。

次いで、ホストＣＰＵ３０は、ジャイロセンサ７０からのオリジナルの出力値である原
出力値を取得し（ステップＡ５）、ステップＡ３で取得した温度と、ステップＡ５で取得
した原出力値とを対応付けて、ＲＡＭ１１０のゼロ点バイアス参照データ１１３に蓄積記
憶させる（ステップＡ７）。

その後、ホストＣＰＵ３０は、ゼロ点バイアス参照データ１１３に記憶されている複数
の温度及び原出力値のデータを基に、例えば最小二乗法を用いて１次の温度係数「ｋ₁」
及び２次の温度係数「ｋ₂」を求め、式（２）で表わされる２次の温度補償モデル式を算
出する（ステップＡ９）。そして、算出した温度補償モデル式で、ＲＡＭ１１０の温度補
償モデル式データ１１５を更新する（ステップＡ１１）。

次いで、ホストＣＰＵ３０は、ＲＡＭ１１０の温度補償モデル式データ１１５に記憶さ
れている温度補償モデル式を用いて、温度センサ９０により検出された現在の温度におけ
るゼロ点バイアスを算出し、算出したゼロ点バイアスを用いて、ジャイロセンサ７０のゼ
ロ点バイアス補正を行う（ステップＡ１３）。

その後、ホストＣＰＵ３０は、１時刻前（前回測位時）のＫＦ誤差推定処理で求められ
た加速度センサ６０及びジャイロセンサ７０のバイアス「ｂ_a」及び「ｂ_g」を用いて、慣
性航法用センサのバイアス補正を行う（ステップＡ１５）。

次いで、ホストＣＰＵ３０は、慣性航法用センサの検出結果を用いて公知の慣性航法演
算処理を行って、自動車の位置「ｐⁱ」、速度「ｖⁱ」及び姿勢「Ａⁱ」を演算するととと
もに、自動車の相対高度を演算して第１高度変化「Δｈⁱ」とする（ステップＡ１７）。

その後、ホストＣＰＵ３０は、気圧センサ８０の検出結果に基づいて、第２高度変化を
算出する（ステップＡ１９）。具体的には、気圧センサ８０により検出された大気圧の変
化量から相対高度を算出して、第２高度変化「Δｈ^b」とする。

次いで、ホストＣＰＵ３０は、ジャイロセンサ６０の検出結果に基づいて、ｚ軸回りの
回転が有るか否かを判定する（ステップＡ２１）。具体的には、ジャイロセンサ６０によ
り検出されたｚ軸回りの角速度が所定の閾値以上であるか否かを判定し、閾値以上である
と判定した場合は、ｚ軸回りの回転が有るものと判定する。尚、所定の閾値は、ジャイロ
センサ６０の検出誤差等を加味して、自動車がｚ軸回りに回転していない（左右方向に回
転していない）とみなせる値に予め定められる。

そして、ｚ軸回りの回転が有ると判定した場合は（ステップＡ２１；Ｙｅｓ）、ホスト
ＣＰＵ３０は、ステップＡ２５へと処理を移行する。また、ｚ軸回りの回転が無いと判定
した場合は（ステップＡ２１；Ｎｏ）、ステップＡ１７の慣性航法演算処理で求めたｘ軸
方向への速度「ｖⁱ _x」と、速度制約条件として設定したｘ軸方向への速度「ｖ^r _x＝０」と
の差「δｖ_x＝ｖⁱ _x−ｖ^r _x＝ｖⁱ _x」を、第１観測値として設定する（ステップＡ２３）。

その後、ホストＣＰＵ３０は、第２高度変化が有るか否かを判定し（ステップＡ２５）
、有ると判定した場合は（ステップＡ２５；Ｙｅｓ）、ステップＡ２９へと処理を移行す
る。また、第２高度変化が無いと判定した場合は（ステップＡ２５；Ｎｏ）、ステップＡ
１７の慣性航法演算処理で求めたｚ軸方向への速度「ｖⁱ _z」と、速度制約条件として設定
したｚ軸方向への速度「ｖ^r _z＝０」との差「δｖ_z＝ｖⁱ _z−ｖ^r _z＝ｖⁱ _z」を、第２観測値
として設定する（ステップＡ２７）。

また、ホストＣＰＵ３０は、ステップＡ１７の慣性航法演算処理で求めた第１高度変化
「Δｈⁱ」と、ステップＡ１９で求めた第２高度変化「Δｈ^b」との差「δｈ＝Δｈⁱ−Δ
ｈ^b」を、第３観測値として設定する（ステップＡ２９）。

そして、ホストＣＰＵ３０は、ＲＯＭ１００に記憶されている第１ＫＦ処理プログラム
１０５１〜第４ＫＦ処理プログラム１０５４の中から、設定した観測値に応じたＫＦ処理
プログラムを選択する（ステップＡ３１）。ＫＦ処理プログラムの選択は、図２〜図５で
それぞれ説明したＫＦ処理に対応するプログラムを選択することで行う。

その後、ホストＣＰＵ３０は、ステップＡ３１で選択したＫＦ処理プログラムを読み出
して実行することでＫＦ誤差推定処理を行い、慣性航法演算処理の演算結果に含まれる誤
差「（δｐ，δｖ，ε）」及び慣性航法用センサのバイアス「（ｂ_a，ｂ_g）」を推定する
（ステップＡ３３）。そして、推定した誤差を用いて、慣性航法演算処理の演算結果「（
ｐⁱ，ｖⁱ，Ａⁱ）」を補正する（ステップＡ３５）。

次いで、ホストＣＰＵ３０は、ＧＰＳ受信部２０からＧＰＳ測位結果を取得したか否か
を判定し（ステップＡ３７）、取得したと判定した場合は（ステップＡ３７；Ｙｅｓ）、
取得したＧＰＳ測位位置を出力位置に決定する（ステップＡ３９）。また、ＧＰＳ測位結
果を取得しなかったと判定した場合は（ステップＡ３７；Ｎｏ）、ステップＡ３５の補正
結果の慣性航法演算位置「ｐⁱ」を出力位置に決定する（ステップＡ４１）。

出力位置を決定した後、ホストＣＰＵ３０は、操作部４０を介してユーザにより測位終
了指示がなされたか否かを判定し（ステップＡ４３）、なされなかったと判定した場合は
（ステップＡ４３；Ｎｏ）、ステップＡ１に戻る。また、測位終了指示がなされたと判定
した場合は（ステップＡ４３；Ｙｅｓ）、測位処理を終了する。

４．作用効果
本実施形態によれば、慣性航法演算処理により随時求められる相対高度である第１高度
変化「Δｈⁱ」と、移動体である自動車の外気圧を検出する気圧センサの検出結果に基づ
いて算出される相対高度である第２高度変化「Δｈ^b」との差「δｈ＝Δｈⁱ−Δｈ^b」が
算出される。そして、算出された高度変化の差「δｈ」、慣性航法演算処理により求めら
れる自動車の直進方向に対する左右方向の速度「ｖⁱ _x」、及び、慣性航法演算処理により
求められる自動車の高度方向の速度「ｖⁱ _z」に応じた観測値を用いたカルマンフィルタ処
理が行われて、慣性航法演算処理の演算結果「（ｐⁱ，ｖⁱ，Ａⁱ）」に含まれる誤差「（
δｐ，δｖ，ε）」が推定され、推定された誤差を用いて慣性航法演算処理の演算結果が
補正されて、自動車の現在位置が測位される。

本実施形態では、自動車が直進方向に対する左右方向に回転していることが検出された
場合は、左右方向の速度「ｖⁱ _x」を観測値として用いず、自動車が高度方向に変位してい
ることが検出された場合は、高度方向の速度「ｖⁱ _z」を観測値として用いずにカルマンフ
ィルタ処理を行うようにしている。これにより、現実的な自動車の移動を想定した適切な
誤差推定を実現することができ、ひいては、自動車の現在位置をより正確に測位すること
が可能となる。

５．変形例
５−１．電子機器
本発明は、測位装置を備えた電子機器であれば何れの電子機器にも適用可能である。例
えば、ノート型パソコンやＰＤＡ（Personal Digital Assistant）等についても同様に適
用可能である。但し、移動体に設置される際には、移動体に対する電子機器の相対的な姿
勢を固定する必要がある。すなわち、電子機器の慣性航法用センサが検出する座標系と移
動体座標系との相対的関係を常に一定とし、移動体座標系の３軸の値を検出する必要があ
る。換言すれば、移動体座標系の３軸の値を検出できれば、移動体に対する電子機器の相
対的な姿勢は任意である。

５−２．移動体
また、移動体は必ずしも自動車に限られるわけではなく、バスや電車、人間等の移動体
についても同様に適用可能である。

５−３．衛星測位システム
上述した実施形態では、衛星測位システムとしてＧＰＳを例に挙げて説明したが、ＷＡ
ＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）
、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛
星測位システムであってもよい。

５−４．処理の分化
ホストＣＰＵ３０が行う処理の一部又は全部を、ベースバンド処理回路部２３のＣＰＵ
が行うことにしてもよい。具体的には、例えばベースバンド処理回路部のＣＰＵが測位処
理を行う。そして、ホストＣＰＵ３０が、測位処理により求められた出力位置に対してマ
ップマッチング処理等を行ってナビゲーション画面を生成し、生成したナビゲーション画
面を表示部５０に表示させるナビゲーション処理を行うようにする。

５−５．上下方向の回転有無の判定
上述した実施形態では、気圧センサ８０の検出結果に基づいて算出された第２高度変化
が無しの場合に、ｚ軸方向への速度制約条件を用いてカルマンフィルタ処理を実行するも
のとして説明したが、移動体の直進方向に対する上下方向への回転（ｘ軸回りの回転。い
わゆるピッチ角。）の有無を判定し、回転が無いと判定された場合に限り、ｚ軸方向への
速度制約条件を用いてカルマンフィルタ処理を実行することとしてもよい。

具体的には、ジャイロセンサ７０により検出されたｘ軸回りのピッチ角と移動体の初期
水平状態のピッチ角との差分が所定の閾値未満であるか否かを判定し、閾値以上であると
判定した場合は、ｘ軸回りの回転（上下方向への回転）が有るものと判定する。そして、
この場合において、ｚ軸回りの回転（左右方向への回転）も有ると判定した場合は、第１
高度変化と第２高度変化との差「δｈ＝Δｈⁱ−Δｈ^b」のみを観測値とする第１ＫＦ処理
を行い、ｚ軸回りの回転が無いと判定した場合は、「δｈ」と、ｘ軸方向に対する速度の
差「δｖ_x＝ｖⁱ _x−ｖ^r _x＝ｖⁱ _x」とを観測値とする第２ＫＦ処理を行って、状態ベクトル
「Ｘ＝（δｐ，δｖ，ε，ｂ_a，ｂ_g）」を演算するようにする。

また、ｘ軸回りのピッチ角と移動体の初期水平状態のピッチ角との差分が所定の閾値未
満であると判定した場合は、ｘ軸回りの回転（上限方向への回転）が無いものと判定する
。そして、この場合において、ｚ軸回りの回転（左右方向への回転）が有ると判定した場
合は、「δｈ」と、ｚ軸方向に対する速度の差「δｖ_z＝ｖⁱ _z−ｖ^r _z＝ｖⁱ _z」とを観測値
とする第３ＫＦ処理を行い、ｚ軸回りの回転も無いと判定した場合は、「δｈ」と、「δ
ｖ_x」と、「δｖ_z」とを観測値とする第４ＫＦ処理を行って、状態ベクトル「Ｘ」を演算
するようにする。

５−６．回転の検出
また、上述した実施形態では、ジャイロセンサ７０により検出された角速度を用いて移
動体の回転の有無を判定するものとして説明したが、角速度を時間微分することで得られ
る角加速度を用いて移動体の回転の有無を判定することとしてもよい。

具体的には、各時刻それぞれにおいて、当該時刻にジャイロセンサ７０により検出され
た角速度と、１時刻前にジャイロセンサ７０により検出された角速度との差分を算出する
。そして、算出した差分が所定の閾値以上である場合は、移動体の回転が有りと判定し、
閾値未満である場合は、移動体の回転が無しと判定する。尚、この場合は、ジャイロセン
サ７０の原出力値の差分を算出しているため、ジャイロセンサ７０のバイアス補正を行わ
ずとも、回転の有無を適切に判定することができる。

５−７．慣性航法用センサのバイアス補正
上述した実施形態では、ジャイロセンサ７０のゼロ点バイアスの補正を行う場合を例に
挙げて説明したが、加速度センサ６０のゼロ点バイアスについても同様に温度補正するこ
とが可能である。

５−８．温度補償モデル式
また、２次の温度補償モデル式を用いて慣性航法用センサのゼロ点バイアスの補正を行
うのではなく、３次以上の温度補償モデル式を用いてゼロ点バイアスの補正を行うことと
してもよい。この場合は、図１０の測位処理のステップＡ９において温度補償モデル式を
算出する際に、例えば最小二乗法を用いて、１次及び２次の温度係数「ｋ₁」及び「ｋ₂」
に加えて、３次以上の温度係数「ｋ₃，・・・」も求めるようにすればよい。

移動体座標系の一例を示す図。 ｚ軸回りの回転有りで、第２高度変化有りの場合の誤差推定の流れを示す図。 ｚ軸回りの回転無しで、第２高度変化有りの場合の誤差推定の流れを示す図。 ｚ軸回りの回転有りで、第２高度変化無しの場合の誤差推定の流れを示す図。 ｚ軸回りの回転無しで、第２高度変化無しの場合の誤差推定の流れを示す図。 カーナビゲーション装置の機能構成を示すブロック図。 ＲＯＭに格納されたデータの一例を示す図。 ＲＡＭに格納されたデータの一例を示す図。 計測履歴データのデータ構成の一例を示す図。 測位処理の流れを示すフローチャート。 測位処理の流れを示すフローチャート。

符号の説明

１ カーナビゲーション装置、 １０ ＧＰＳアンテナ、 ２０ ＧＰＳ受信部、
２１ ＲＦ受信回路部、 ２３ ベースバンド処理回路部、 ３０ ホストＣＰＵ、
４０ 操作部、 ５０ 表示部、 ６０ 加速度センサ、 ７０ ジャイロセンサ、
８０ 気圧センサ、 ９０ 温度センサ、 １００ ＲＯＭ、 １１０ ＲＡＭ

Claims (6)

1. 動体に搭載される測位装置が 所定の慣性航法用センサを用いた所定の慣性航法演算処理を実行して前記移動体の現在位置を測位する測位方法であって、
   前記慣性航法用センサの検出結果から、前記移動体の直進方向に対する左右方向の回転有無を判定することと、
   前記慣性航法演算処理により随時求められる相対高度を第１高度変化として算出することと、
   前記移動体の外気圧を検出する気圧センサの検出結果に基づいて、相対高度を第２高度変化として算出することと、
   前記第１高度変化と前記第２高度変化との差を算出することと、
   前記高度変化の差、前記慣性航法演算処理により求められる前記移動体の直進方向に対する左右方向の速度、及び、前記慣性航法演算処理により求められる前記移動体の高度方向の速度に応じた観測値を用いたカルマンフィルタ処理を行い、
   前記カルマンフィルタ処理は前記左右方向の回転が無いと判定された場合には、前記左右方向の速度及び前記高度変化の差それぞれに応じた観測値を少なくとも用いたカルマンフィルタ処理を行い、
   前記左右方向の回転が有ると判定された場合には、前記左右方向の速度に応じた観測値を用いずに、前記高度変化の差に応じた観測値を少なくとも用いたカルマンフィルタ処理を行って、
   前記慣性航法演算処理の演算結果に含まれる誤差を推定することと、
   前記誤差を用いて前記慣性航法演算処理の演算結果を補正して、
   前記移動体の現在位置を測位することと、
   を含む測位方法。
2. 前記慣性航法用センサは角速度を検出するセンサを有して構成され、
   前記左右方向の回転有無を判定することは、前記検出された角速度又は当該角速度の時
   間微分である角加速度が所定の閾値条件を満たすか否かによって判定することである、
   請求項１に記載の測位方法。
3. 前記誤差を推定することは、前記第２高度変化が無しの場合には、前記高度方向の速度
   及び前記高度変化の差それぞれに応じた観測値を少なくとも用いたカルマンフィルタ処理
   を行い、前記第２高度変化が有る場合には、前記高度方向の速度に応じた観測値を用いず
   に、前記高度変化の差に応じた観測値を少なくとも用いたカルマンフィルタ処理を行って
   、前記慣性航法演算処理の演算結果に含まれる誤差を推定することである、
   請求項１〜２の何れか一項に記載の測位方法。
4. 前記移動体が停止したときに、気温を検出する温度センサの検出温度と前記慣性航法用
   センサの検出結果とを対応付けて記憶することと、
   前記記憶された前記温度センサの検出温度と前記慣性航法用センサの検出結果とから、
   任意の気温に対する前記慣性航法用センサの検出誤差を補償する温度補償モデル式を算出
   することと、
   前記温度センサの現在の検出温度に対応する前記慣性航法用センサの検出誤差の補償値
   を前記温度補償モデル式から求め、前記慣性航法用センサの検出結果を補償することと、
   を更に含む請求項１〜３の何れか一項に記載の測位方法。
5. 請求項１〜４の何れか一項に記載された測位方法を、移動体に搭載される測位装置に内
   蔵されたコンピュータに実行させるためのプログラム。
6. 移動体に搭載され、所定の慣性航法用センサを用いた所定の慣性航法演算処理を実行し
   て前記移動体の現在位置を測位する測位装置であって、
   前記慣性航法演算処理により随時求められる相対高度を第１高度変化として算出する第
   １高度変化算出部と、
   前記移動体の外気圧を検出する気圧センサの検出結果に基づいて、相対高度を第２高度
   変化として算出する第２高度変化算出部と、
   前記第１高度変化と前記第２高度変化との差を算出する高度変化差算出部と、
   前記高度変化の差、前記慣性航法演算処理により求められる前記移動体の直進方向に対
   する左右方向の速度、及び、前記慣性航法演算処理により求められる前記移動体の高度方
   向の速度に応じた観測値を用いたカルマンフィルタ処理を行い、前記カルマンフィルタ処理は前記左右方向の回転が無いと判定された場合には、前記左右方向の速度及び前記高度変化の差それぞれに応じた観測値を少なくとも用いたカルマンフィルタ処理を行い、
   前記左右方向の回転が有ると判定された場合には、前記左右方向の速度に応じた観測値を用いずに、前記高度変化の差に応じた観測値を少なくとも用いたカルマンフィルタ処理を行って、前記慣性航法演算処理の演算結果に含まれる誤差を推定するカルマンフィルタ処理部と、
   前記誤差を用いて前記慣性航法演算処理の演算結果を補正して、前記移動体の現在位置
   を測位する測位部と、
   を備えた測位装置。

Images