JP6852310B2 - 慣性装置、プログラム、測位方法 - Google Patents

慣性装置、プログラム、測位方法 Download PDF

Info

Publication number
JP6852310B2
JP6852310B2 JP2016166207A JP2016166207A JP6852310B2 JP 6852310 B2 JP6852310 B2 JP 6852310B2 JP 2016166207 A JP2016166207 A JP 2016166207A JP 2016166207 A JP2016166207 A JP 2016166207A JP 6852310 B2 JP6852310 B2 JP 6852310B2
Authority
JP
Japan
Prior art keywords
correction
unit
traveling direction
information
estimated
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2016166207A
Other languages
English (en)
Other versions
JP2017106891A (ja
Inventor
裕介 松下
裕介 松下
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ricoh Co Ltd
Original Assignee
Ricoh Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ricoh Co Ltd filed Critical Ricoh Co Ltd
Priority to US15/358,893 priority Critical patent/US10408627B2/en
Publication of JP2017106891A publication Critical patent/JP2017106891A/ja
Application granted granted Critical
Publication of JP6852310B2 publication Critical patent/JP6852310B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Description

本発明は、慣性装置、プログラム及び測位方法に関する。
屋内や地下のようにＧＰＳ(Global Positioning System)を用いた測位が行えない環境で、加速度センサ、ジャイロセンサ及び地磁気センサの検出値を用いて歩行者の位置を推定する慣性航法技術（PDR; Pedestrian Dead Reckoning）が知られている。慣性航法技術では、検出値の演算によって歩行者の進行方向や歩行速度ベクトルを推定し進行方向に対する歩行速度ベクトルを時間積分することによって現在位置を推定する。なお、加速度センサ、ジャイロセンサ及び地磁気センサが統合されたセンサをモーションセンサという。
しかし、歩行者向けの慣性航法技術は、位置推定精度を向上させるために装置を使用する前に歩幅や歩行速度等のキャリブレーション操作をユーザが行なう必要がある。また、ＧＰＳ等の絶対位置情報を取得した際に歩幅や歩行速度推定のためのパラメータを学習する技術が知られているが、屋外でなければ機能しない。
また、慣性航法技術における方位推定においては、地磁気センサ及びジャイロセンサ等によって検出された絶対方位角に基づいて歩行者の進行方向を決定する。つまり、地磁気センサにより絶対方位角が分からないと、ジャイロセンサで方位の変化量が分かっても現在位置を正しく推定できない。
しかし、絶対方位角の検出誤差はビルや地下などの屋内環境で著しく大きくなることが知られている。これは、地磁気センサが鉄筋等の構造物から環境磁場の撹乱を受けるためであり、屋内環境では地磁気センサのみに基づく慣性航法は実用的ではない。
別の問題として、ジャイロセンサには温度変化に伴うゼロ点シフト（一般的にジャイロドリフトと呼ばれる）が発生する。ゼロ点シフトは積算誤差の重畳による方位推定精度の低下を引き起こすため、地磁気センサを用いずに継続的に方位を精度よく検出することは困難である。
地磁気センサとジャイロセンサをハイブリッド化し、地磁気センサが検出する方位を信頼できると判定した場合、地磁気センサで検出された絶対方位角でジャイロセンサにおいて推定された方位角を補正する手法が知られている。しかし、地磁気センサが検出する方位を信頼できない屋内環境においては補正そのものがなされない状況が多くなり、ジャイロセンサのみを使用する場合と共通した課題を抱えることになる。
絶対方位の精度を向上する技術として、マップマッチングを用いる技術が知られている（例えば、特許文献１参照。）。特許文献１には、慣性航法によって直線歩行を検出し、地図情報にマッチングされた２地点間の方位からユーザの方位を補正する技術が開示されている。
しかしながら、特許文献１に記載された方位の補正方法では地図情報が必要であるという問題だけでなく、慣性航法による位置の精度低下によって地図上の誤った位置にマッチングされた場合、正しい方位補正を行なうことができないという問題がある。
本発明は、上記課題に鑑み、屋内の位置精度を向上させる慣性装置を提供することを目的とする。
上記課題に鑑み、本発明は、センサ出力を用いて進行方向と移動速度を推定する進行方向推定部と、前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定する位置推定部と、外部から提供される絶対位置情報を取得する絶対位置情報取得部と、前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成する位置補正部と、前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正する進行方向補正部と、
前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出する補正パラメータ算出部と、を有し、前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、前記絶対位置情報取得部は、前記絶対位置情報が無線で提供されるエリアに慣性装置が進入するごとに前記絶対位置情報を取得し、前記補正パラメータ算出部は、前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正された前記補正位置との第１距離、前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正される前の前記補正位置との第２距離、をそれぞれ算出し、前記第１距離に対する前記第２距離の比率を用いて、前記移動速度を補正する移動速度補正パラメータを算出し、前記移動速度補正パラメータを用いて前記移動速度を補正する移動速度補正部を有し、前記位置推定部は、前記移動速度補正部により補正された前記移動速度で前記推定位置を推定する慣性装置に関する。
屋内の位置精度を向上させる慣性装置を提供することができる。
慣性航法により生じる２点間の距離の誤差と方位誤差について説明する図の一例である。 慣性装置１の座標系を説明する図の一例である。 本発明の一実施形態における慣性装置のハードウェア構成例を表す図である。 本発明の一実施形態における慣性装置の機能ブロックの一部を表す図である。 拡張カルマンフィルタの一般式におけるシステムの状態推定モデルに含まれる項目を説明する図である。 拡張カルマンフィルタの一般式における時間更新における偏微分行列（ヤコビアン）を説明する図である。 拡張カルマンフィルタの一般式における誤差共分散行列Ｐk|k-1を説明する図である。 拡張カルマンフィルタの一般式における観測誤差を説明する図である。 拡張カルマンフィルタの一般式における観測誤差を説明する図である。 姿勢演算部１０４ＴＲＩＡＤアルゴリズムにより第二の姿勢情報を計算する手順のフローチャート図の一例である。 座標変換を説明する図の一例である。 回転行列ＤＣＭ２を説明する図の一例である。 進行方向推定部の機能を説明する図の一例である。 鉛直成分移動加速度（Ｚ）と移動加速度の水平成分（Ｘ、Ｙ）の変化を表す波形を示す図の一例である。 歩行動作における鉛直方向の運動特性を説明する図の一例である。 歩行動作における水平方向の運動特性を説明する図の一例である。 移動加速度の水平成分と、鉛直成分の、時間に対する変化を表す波形を示す図の一例である。 水平成分移動速度特徴ベクトルの一例を示す図である。 各種情報が歩行動作に由来して取得されたデータであるかどうかを判定する手順を示すフローチャート図の一例である。 左右の移動速度のぶれ幅を説明する図の一例である。 進行方向推定処理を説明する図の一例である。 位置推定部が有する機能を説明する図の一例である。 進行速度推定部の内部の詳細な機能ブロック図の一例である。 パラメータデータベースの一例を表す図の一例である。 進行速度波形の例を表す図の一例である。 合成された進行速度波形の例を表す図の一例である。 慣性装置の絶対位置情報入力部、絶対位置推定部及びマップマッチング部の機能ブロック図の一例である。 電波強度と測位時間間隔の関係を表す図の一例である。 時間発展の手続きにおける現在位置の演算を説明する図の一例である。 時間発展の手続きにおける現在位置の演算を説明する図の一例である。 時間発展の手続きにおける現在位置の演算を説明する図の一例である。 観測更新の手続きにおける現在位置の演算を説明する図。 補正後の速度情報を模式的に示す図の一例である。 絶対位置情報の送信機と慣性装置との距離に応じた受信信号強度の変化の一例を示す図である。 歩行速度推定パラメータとヨー角補正値の推定手順を示すフローチャート図の一例である。 ５箇所の受信エリアを移動した際に得られた測位結果と補正の様子を示す図の一例である。 従来技術と本実施形態の技術とで得られた歩行者の移動軌跡を比較する図の一例である。 拡張カルマンフィルタの一般式を説明する図（従来技術）。 時間更新の手続きにおいて用いられる変数を説明する図（従来技術）。 観測更新の手続きにおいて用いられる変数を説明する図（従来技術）。
以下、本発明を実施するための形態について図面を参照しながら説明する。
まず、図１を用いて、慣性航法により生じる２点間の距離の誤差と方位誤差について説明する。図１は、慣性航法により生じる２点間の距離の誤差と方位誤差について説明する図の一例である。
受信エリアＡＲＥＡ１と受信エリアＡＲＥＡ２は慣性装置が進入した場合に絶対位置情報を電波や音波などの無線で受信可能なエリアを示す。Ｐ１、Ｐ２は絶対位置情報に含まれる絶対位置である。絶対位置情報とは、何らかの基準（座標系）において正確であることが確認されている位置（緯度・経度・標高など）であり、天井や通路などに設置された送信機により定期的に送信されている。
Ｐ´１は絶対位置情報の受信エリアＡＲＥＡ１においてパラメトリックな手法（カルマンフィルタなど）により補正された位置を示し、Ｐ´２は絶対位置情報の受信エリアＡＲＥＡ２においてパラメトリックな手法により補正された位置を示す。円Ｅ１とＥ２は、パラメトリックな手法により推定された誤差の推定値（誤差情報）である。
慣性装置１を携帯した歩行者が受信エリアＡＲＥＡ１から受信エリアＡＲＥＡ２に移動する場合を考える。受信エリアＡＲＥＡ１で歩行者の位置はＰ´１に補正されておりこれを始点とする。歩行者が受信エリアＡＲＥＡ２に到達した際に、慣性航法のみによる移動の軌跡１０１２と推定された位置１００１がＰ２と大きく乖離している。乖離に起因するのは距離と方位の推定誤差である。つまり、歩行速度を推定するための歩行速度パラメータが正しくない場合や、慣性装置１が推定した方位に誤差が含まれている場合、慣性航法のみにより推定された位置１００１と、パラメトリックな手法で補正された位置Ｐ´２とは乖離する。図１では、方位の推定誤差（ヨー角補正値）Δψと距離の推定誤差ΔＬが示されている。
これらの推定誤差が発生する原因は、歩行者の歩行速度を推定するための歩行速度推定パラメータが該歩行者にとって適切でないために積算して求められる位置１００１に誤差が重畳するためであること、及び、慣性装置１が推定する方位（ヨー角）の補正が行われないためである。よって、これらを精度良く補正し、より正確な位置推定を行なうことのできる慣性装置１が求められる。
＜本実施形態の慣性装置による測位方法の概略＞
続いて、本実施形態の慣性装置１による測位方法の概略について説明する。本実施形態の測位方法は、慣性航法によって得られる測位結果（少なくとも位置座標が含まれる。慣性航法にパラメトリックな手法が用いられる場合は誤差情報も含まれる。）を絶対位置情報を受信した際にパラメトリックな手法を用いて補正し、位置座標に対する誤差（図１のＥ１、Ｅ２）を推定する。
慣性航法で得られる位置座標は、この補正された位置座標と誤差でリセットされる。そして、後述する「歩行条件」が成立する場合、次の処理を実行する。
１．最新の補正後の位置座標（Ｐ´２）と前回の補正後の位置座標（Ｐ´１）の２点間の距離Ａ及び進行方向（距離Ａの直線で結ぶ第１方位）を計算する。
２．前回の補正後の位置座標（Ｐ´１）と慣性航法のみで得られた位置１００１の距離Ｂと２点間の進行方向（距離Ｂの直線で結ぶ第２方位）を計算する。
３．距離Ａを距離Ｂで除算した結果を、歩行速度補正パラメータとして以後の慣性航法による位置の推定に使用する。
４．１の第１方位と２の第２方位の差分をヨー角補正値（方向補正パラメータ）Δψとして姿勢を補正する。
このような測位方法によれば、パラメトリックな手法で歩行速度補正パラメータ及び方向補正パラメータを算出し、補正された進行方向に補正された歩行速度で以降の慣性航法による測位を行うため、慣性航法により位置を精度良く推定することができる。
なお、パラメトリックな手法とは、母集団の特性を規定する母数について仮説を設けて推測する手法であり、例えば、母集団の正規性や等分散性を仮定して真値や誤差を推定する手法を言う。これに対比される言葉としてノンパラメトリックな手法があり、こちらは
母集団の分布型（母数）について一切の仮定を設けないで真値や誤差を推定する手法である。本実施形態ではパラメトリックな手法の一例としてカルマンフィルタを使用する。
＜慣性装置の概略＞
図２は、慣性装置１の座標系を説明する図の一例である。図２（ａ）は後述する絶対座標系であり、図２（ｂ）は慣性装置１に固定されたデバイス座標系を示す。本明細書では、絶対座標系として、ユーザの歩行する平面がＸ軸とＹ軸で表され、Ｘ軸及びＹ軸に対し鉛直方向がＺ軸で表される。Ｘ軸とＹ軸は垂直である。また、デバイス座標系をＸ´軸とＹ´軸とＺ´軸で表す。
慣性装置１は、情報処理装置と称される倍があり、例えば、スマートデバイスと呼ばれる端末、携帯電話、スマートフォン、タブレット端末、ウェアラブルＰＣ、ＰＤＡ（Personal Digital Assistant）又はノートブックＰＣのように、ユーザが持ち運ぶことのできる小型のデバイスである。慣性装置１は、市販されているスマートフォン等に搭載されているような、慣性センサ（三軸の加速度センサ、三軸の角速度センサ）及び三軸の地磁気センサを備えている。慣性装置１は、各センサを利用して、加速度及び角速度の変化並びに向きを検出することができる。地磁気センサは地磁気を検出して絶対値としての方位を検出する。加速度センサは加速度の変化を検出する。角速度センサは角速度の変化を検出する。また、慣性センサとは慣性の変化を検出して変化の大きさに応じた信号を出力するセンサの総称である。
なお、慣性装置１は、上で例示したデバイス以外のデバイス（例えば、音楽プレーヤ、活動量計、腕時計等）であってもよい。また、慣性装置１は、他のデバイス（例えば、歩行ロボットや、動物に装着する首輪等）に内蔵されて提供されてもよい。これにより、鉛直方向に一定の周期で運動する多様な人又は物体の進行方向を推定することが可能となる。
＜ハードウェア構成＞
次に、図３を用いて、本発明の一実施形態における慣性装置１のハードウェア構成を説明する。図３は、慣性装置１が、スマートフォンのようなスマートデバイスである場合の、ハードウェア構成例である。
慣性装置１は、ＣＰＵ１１、ＲＡＭ１２、ＲＯＭ１３、加速度センサ１４、角速度センサ１５、地磁気センサ１６、マイク１７、スピーカ１８、通信モジュール１９、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０、ＧＰＳ受信モジュール２１、ディスプレイ２２、タッチパネル２３、電池２４、気圧センサ２５及びバス２６を有する。
ＣＰＵ１１は、慣性装置１の動作制御を行うプログラムを実行する。ＲＡＭ１２は、ＣＰＵ１１のワークエリア等を構成する。ＲＯＭ１３は、ＣＰＵ１１が実行するプログラム
や、プログラムの実行に必要なデータを記憶する。加速度センサ１４は、慣性装置１のデバイス座標系におけるＸ'、Ｙ'、Ｚ'軸方向の加速度（センサ出力）を検出する。角速度センサ１５（又はジャイロセンサ）は、慣性装置１の、デバイス座標系におけるＸ'、Ｙ'、Ｚ'軸方向の角速度（センサ出力）を検出する。地磁気センサ１６は、磁北を表す三次元のベクトル（センサ出力）を出力し、慣性装置１の向きを検出する。気圧センサ２５は、気圧を測定し、当該慣性装置１の高度を検出する。
マイク１７は、ユーザの声等の音声を電気信号に変換する。スピーカ１８は、電気信号を音声として出力する。通信モジュール１９は、３Ｇネットワーク及び／又は無線ＬＡＮに接続された他の装置と通信するための装置である。Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０は、Ｂｌｕｅｔｏｏｔｈ（登録商標）を用いて通信するための装置である。ＧＰＳ受信モジュール２１は、ＧＰＳ衛星やＩＭＥＳ（Indoor Messaging Service）によって送信される測位信号を受信するための装置である。
ディスプレイ２２は、ユーザに対して画面を提示するための装置である。タッチパネル２３は、ユーザからの入力を受け付ける装置である。電池２４は、慣性装置１を駆動するための電力を供給する装置である。バス２６は、電池２４を除く各装置を相互に接続する。
なお、マイク１７、スピーカ１８、通信モジュール１９、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０、ＧＰＳ受信モジュール２１、ディスプレイ２２及びタッチパネル２３は、任意の構成要素である。例えば、慣性装置１が、表示画面を有しない活動量計のようなデバイスである場合には、これらの装置を備えていなくてもよい。
また、慣性装置１は、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０の代わりに、他の規格に従って無線通信する装置（例えば、ＺｉｇＢｅｅ（登録商標）通信モジュール）を備えていてもよい。
＜慣性装置の全体構成＞
図４は、慣性装置１の機能ブロック図の一例である。慣性装置１は、姿勢情報変換処理部１００、進行方向推定部２００、補正パラメータ推定処理部７００、位置推定部８００、姿勢情報推定部１１０、デバイス座標系三軸加速度取得部１２０、及び、測位誤差・座標推定部３３０を有している。慣性装置１が有するこれら各部は、図３に示したＣＰＵ１１がＲＡＭ１２からＲＯＭ１３に展開されたプログラムを実行し、図３に示す各部を制御することで実現される機能又は手段である。
補正パラメータ推定処理部７００は、補正パラメータ（ヨー角補正値７０４と歩行速度推定パラメータ７０５）を算出する。詳しくは後述される。
姿勢情報推定部１１０は、加速度、角速度、及び地磁気の検出値を取得して後述するクォータニオン（後述する方向余弦行列又は回転行列ＤＣＭ１と等価の情報を有している）を作成する。クォータニオンは姿勢情報変換処理部１００に通知される。
姿勢情報変換処理部１００は、方向余弦行列（又は回転行列ＤＣＭ１）によりデバイス座標系の加速度を絶対座標系に変換する。また、姿勢情報変換処理部１００は、ヨー角補正値７０４を取得して、進行方向を補正する。このようにして進行方向が補正された絶対座標系加速度ベクトル１０７が進行方向推定部２００に通知される。
進行方向推定部２００は、絶対座標系に変換された絶対座標系加速度ベクトル１０７を用いて歩行者の進行方向２１３を作成する。詳細は後述される。
位置推定部８００は、進行方向推定部２００が推定した進行方向２１３から進行方向速度ベクトルを算出し、カルマンフィルタなどのパラメトリックな手法を用いて絶対位置と誤差情報を算出する。この絶対位置で慣性航法のみによる速度と位置が求まる。また、位置推定部８００は、絶対位置情報を取得するとカルマンフィルタなどのパラメトリックな手法を用いて位置を補正し誤差情報を算出する。この時、絶対位置補正フラグがＴＲＵＥとなる。絶対位置、誤差情報及び絶対位置補正フラグ（７０６）は補正パラメータ推定処理部７００に通知される。
また、位置推定部８００は、補正パラメータ推定処理部７００から歩行速度推定パラメータ７０５を取得し、進行速度ベクトルを補正する。
デバイス座標系三軸加速度取得部１２０は、三軸の加速度センサからデバイス座標系の加速度を取得する。デバイス座標系の三軸加速度は座標系変換処理部１０６に通知される。
測位誤差・座標推定部３３０は送信機３４０などから絶対位置情報（位置を補正するための絶対位置情報なので絶対位置補正情報と称してもよい）を取得して絶対位置を抽出すると共に、測位誤差を推定する。測位誤差と絶対位置は位置推定部８００に通知される。
<<姿勢情報推定部１１０>>
姿勢情報推定部１１０の機能について説明する。姿勢情報推定部１１０は、加速度取得部１０１と、角速度取得部１０２と、地磁気取得部１０３と、姿勢演算部１０４と、を含む。姿勢情報推定部１１０は、慣性装置の姿勢を示す回転行列ＤＣＭ１（又は方向余弦行列、クォータニオン）を出力する。
なお、絶対座標系は、複数種類のセンサによって観測された座標値を統一的に扱うために使用される統一的な座標系であり、例えば、ＧＰＳで使用されるＷＧＳ８４経緯度座標系や、ＵＴＭ（ユニバーサル横メルカトル図法）など直交座標系がある。絶対座標系は、ワールド（世界）座標系とも呼ばれる。また、デバイス座標系は、ボディー座標系とも呼ばれ、慣性装置１上の１点を原点として定義し、この原点で互いに直交する三軸をそれぞれＸ´軸、Ｙ´軸、Ｚ´軸として定義した座標系のことを表す。
加速度取得部１０１は、加速度センサ１４によって検出された三軸加速度の変化を取得する。角速度取得部１０２は、角速度センサ１５によって検出された三軸角速度の変化を取得する。ここで取得される角速度は、加速度と同様に、デバイス座標系に固定されている。地磁気取得部１０３は、地磁気センサ１６によって検出された磁北を表す三次元の地磁気ベクトルを取得し、慣性装置１の向きを表す情報を取得する。ここで取得される向きは、加速度と同様に、デバイス座標系に固定されている。
姿勢演算部１０４は、上記した、加速度取得部１０１、角速度取得部１０２及び地磁気取得部１０３が取得したセンサ情報を用いて、慣性装置１の現在の姿勢を演算し、その姿勢情報（回転行列）から、逆行列計算により逆回転行列を求める。この逆回転行列が後述する回転行列ＤＣＭ１である。
姿勢演算部１０４は、一般的に知られている拡張カルマンフィルタ（非特許文献１、非特許文献２、図３８−４０）を利用して、慣性装置１の姿勢を表す行列を求め、その逆行列を出力する。以下、その処理について詳細に説明する。
（（拡張カルマンフィルタの一般式））
図３８は、一般的に知られている拡張カルマンフィルタの一般式を表す。カルマンフィルタでは、時間ステップをひとつ進めるために、時間発展と観測更新の二つの手続きが実行される。時間発展の手続きでは、前の時刻の推定状態から、現在時刻の推定状態が計算される。また、観測更新の手続では、現在時刻の観測を用い、推定値を補正して、より正確な状態が推定される。これらの手続を逐次繰り返すことによって、最適な状態変数が推定される。
図３９は、拡張カルマンフィルタの時間発展の手続きにおいて使用される各変数を説明する図である。図３８の時間発展の項目に含まれる（１）−（３）の式に対応して、各変数の説明が付されている。ここで、ｋは、離散的なステップ時間を表しており、ｋ−１は、今の時間より１ステップ前の時間を表す。
図４０は、拡張カルマンフィルタの観測更新の手続きにおいて使用される各変数を説明する図である。図３８の観測更新の項目に含まれる（１）−（６）の式に対応して、各変数の説明が付されている。
((拡張カルマンフィルタの適用))
姿勢演算部１０４は、上記した拡張カルマンフィルタにおける「時間発展の手続き」を、角速度センサ１５の出力による姿勢情報の更新に用いる（ロール角、ピッチ角、ヨー角）。また、拡張カルマンフィルタにおける「観測更新の手続き」を、加速度センサ１４の出力による姿勢情報の更新に用いる（ロール角、ピッチ角）（以後、「第一の観測更新の手続き」と呼ぶ）。また、「観測更新の手続き」を、更に、地磁気センサ１６の出力による姿勢情報の更新にも用いる（ヨー角）（以後、「第二の観測更新の手続き」と呼ぶ）。
このように、姿勢演算部１０４は、７状態の拡張カルマンフィルタを構築する。姿勢演算部１０４は、一つの時間発展の手続きと、二つの観測更新の手続きを、並列にループさせ、姿勢とジャイロゼロ点バイアス値の推定を行う。ここで、姿勢は、以下のようなクォータニオン（以下では、クォータニオンベクトルという）によって表される。
Figure 0006852310
 クォータニオンベクトルとは、４変数からなるベクトルであり、物体の姿勢を表す。ロール・ピッチ・ヨーの姿勢表現ではシンバルロックという特異点問題がある反面、クォータニオンでは、特異点が無く全ての姿勢を表すことができる。また、ジャイロゼロ点バイアス値は、三軸に対応する三変数（ｂｘk,ｂｙk,ｂｚk）によって表される（ｂは定数）。
以下では、上記の三つの手続き（１）−（３）を、それぞれ説明する。
（（時間発展の手続き））
まず、図５−図７を用いて、拡張カルマンフィルタにおける時間発展の手続きを説明する。姿勢演算部１０４は、当該手続きを実行し、後述する状態推定モデルにおいて、ジャイロ出力値を入力として、拡張カルマンフィルタの時間発展の手続きに従って時間積分を行う。そして、更新されたクォータニオンベクトルｑと、誤差共分散行列Ｐを得る（ロール・ピッチ・ヨー角）。
図５は、本実施形態において、拡張カルマンフィルタの一般式における、（１）システムの状態推定モデルに含まれる項目を説明する図である。ここでは、現在時刻の状態推定値を、上記したクォータニオンベクトル及びジャイロゼロ点バイアス値により、図５の式（１）−１のように定義する。
Figure 0006852310
 また、入力量ｕkを、角速度センサの出力値（ω0xk,ω0yk,ω0zk）（rad/sec）を用いて、図５の式（１）−４のように定義する（ｂは定数）。
Figure 0006852310
 すなわち、（ωxk,ωyk,ωzk）は、ゼロ点の値を減算し、オフセットがない角速度を表す（rad/sec）。そして、システムの状態推定モデルを、図５の式（１）−５のように表す。ここで、Ｃ1、Ｃ2、Ｃ3は、任意の定数である。
Figure 0006852310
 図６は、本発明において、拡張カルマンフィルタの一般式における、（２）時間更新における偏微分行列（ヤコビアン）を説明する図である。図５を用いて説明したように、システムの状態推定モデルは、図５の式（１）−５によって表される。式（１）−５の右辺はｆであるため、右辺の偏微分が、時間発展における偏微分行列となる。
図７は、本発明において、拡張カルマンフィルタの一般式における、（３）誤差共分散行列Ｐk|k-1を説明する図である。プロセスノイズＱkは、システム同定によって求められる定数であり、予め算出される。
Figure 0006852310
 上記のプロセスノイズＱkと、１ステップ前の時刻の誤差共分散行列と、時間発展における偏微分行列（ヤコビアン）Ｆk及びその転置行列Ｆk Tにより、現在時刻の誤差共分散行列Ｐk|k-1が求まる（図７の式（３）−５）。なお、現在時刻の誤差共分散行列Ｐk|k-1及びＰk-1|k-1は、７×７の行列となり、行列内の要素は実数となる。
姿勢演算部１０４は、上記のモデル及び変数の定義を用いて、拡張カルマンフィルタにおける時間発展の手続きを実行し、慣性装置１の、絶対座標系における姿勢を求め、その姿勢を表す行列の逆行列（逆回転行列）を求める。
（（第一の観測更新の手続き））
次に、図８を用いて、拡張カルマンフィルタにおける第一の観測更新の手続きを説明する。姿勢演算部１０４は、当該手続きを実行し、加速度取得部１０１から得られる水平方向の角度情報と、現在のクォータニオンベクトルの水平角度情報とを比較し、その誤差分を補正する処理を行う（ロール・ピッチ角のみ）。
図８は、拡張カルマンフィルタの一般式における、
（１）観測残差
Figure 0006852310
 に含まれる項目を説明する図である。まず、１ステップ前の時間の観測値（ベクトル）ｈは、図８の式（１）−３のように表される。
Figure 0006852310
 上記の式に含まれる要素は、三次元の回転行列（４×４）に由来するものであり、予め決められているものとする。また、観測値（ベクトル）ｚkは、図８の式（１）−２のように表される。
Figure 0006852310
 ここで、（ａx，ａy，ａz）は、加速度取得部１０１が取得した、加速度センサ１４の出力値である。上記のｈ及びｚkにより、観測残差
Figure 0006852310
 が求まる。
また、拡張カルマンフィルタの一般式における、（２）観測更新における偏微分行列（ヤコビアン）Ｈkは、図８の式（１）−３に表される観測値ｈの偏微分を求めることによって求まる。
また、拡張カルマンフィルタの一般式における、（３）残差の共分散Ｓkは、以下の観測ノイズ（行列）Ｒkと、観測更新における偏微分行列Ｈk、その転置行列Ｈk T及び現在時刻の誤差共分散行列Ｐk|k-1を用いて求められる。
Figure 0006852310
 ここで、（ｒ1，ｒ2，ｒ3）は、加速度センサ１４のデバイス評価の結果、予め求められた分散値である。
また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫkは、現在時刻の誤差共分散行列Ｐk|k-1と、観測更新における偏微分行列の転置行列Ｈk Tと、残差の共分散の逆行列Ｓk-1から求まる。なお、Ｋkは、７×３の行列であり、実数値を要素に持つ。
同様に、拡張カルマンフィルタの一般式における、（５）更新された状態推定値ｘk|k及び（６）更新された誤差共分散行列Ｐk|kは、これまでに求められた変数を用いて求めることができる。
姿勢演算部１０４は、上記のモデル及び変数を用いて、拡張カルマンフィルタにおける観測更新の手続きを実行し、水平方向の角度情報と、現在のクォータニオンベクトルの水平角度情報とを比較し、その誤差分を補正する（ロール・ピッチ角のみ）。
（（第二の観測更新の手続き））
次に、図９を用いて、拡張カルマンフィルタにおける第二の観測更新の手続きを説明する。姿勢演算部１０４は、ＴＲＩＡＤアルゴリズムに従って取得された姿勢情報から算出されるヨー角の情報を用いて、当該手続きを行い、クォータニオンベクトルのヨー角成分の補正を行う。ＴＲＩＡＤアルゴリズムを用いた演算方法については後述する。
図９は、図８と同様に、拡張カルマンフィルタの一般式における、
（１）観測残差
Figure 0006852310
 に含まれる項目を説明する図である。図８の場合と同様に、１ステップ前の時間の観測値（ベクトル）ｈは、図９の式（１）−３のように表される。一方、図８の場合と異なり、観測値（ベクトル）ｚkは、図９の式（１）−２のように表される。
Figure 0006852310
 上記のベクトルは、ＴＲＩＡＤアルゴリズムによって求められた、ヨー角方向ベクトルである。
また、拡張カルマンフィルタの一般式における、（２）観測更新における偏微分行列（ヤコビアン）Ｈkは、第一の観測更新の手続きと同様に、１ステップ前の時間の観測値ｈの偏微分を求めることによって、求めることができる。
また、拡張カルマンフィルタの一般式における、（３）残差の共分散Ｓkは、以下の観測ノイズ（行列）Ｒkと、観測更新における偏微分行列Ｈk、その転置行列Ｈk T及び現在時刻の誤差共分散行列Ｐk|k-1を用いて求められる。
Figure 0006852310
 ここで、（Ｔ1，Ｔ2，Ｔ3）は、地磁気センサ１６のデバイス評価の結果、予め求められ
た分散値である。
また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫk、（５）更新された状態推定値ｘk|k及び（６）更新された誤差共分散行列Ｐk|kは、第一の観測更新の手続きと同様に、求めることができる。
<<ＴＲＩＡＤアルゴリズムを用いた姿勢情報の計算>>
ここで、図１０を用いて、姿勢演算部１０４が、既知のＴＲＩＡＤアルゴリズムにより、第二の姿勢情報を計算する例を説明する。
まず、ステップＳ１０において、工場出荷時、あるいはユーザによる設定時に、初期化処理が実行され、加速度の鉛直下向き成分を表すベクトルと、地磁気ベクトルのリファレンスベクトルが、AccRef及びMagRefとして、それぞれ記憶される。ここで、加速度の鉛直下向き成分を表すベクトルは、以下のステップＳ２０で説明されるように、クォータニオンから変換して求められる。また、地磁気ベクトルは、地磁気センサから入力された、磁北を表すベクトルである。当該ステップは、工場出荷時、あるいはユーザによる設定時にのみ実行される。すなわち、リファレンスベクトルは、初期化処理が実行されない限り、同じ値が保持される。
ステップＳ３０において、AccFrameとMagFrameを用いて、３×３の行列であるMagFrameMを計算する。
ステップＳ３２において、AccFrameとMagFrameの外積を求め、その結果を正規化することにより、AccCrossMagを求める。
ステップＳ３４において、AccFrameとAccCrossMagの外積を求め、その結果を正規化することにより、AccCrossAcMを求める。
ステップＳ３６において、AccFrameと、ステップＳ３２で求めたAccCrossMagと、ステップＳ３４で求めたAccCrossAcM（いずれも１×３の行列（ベクトル））とを組み合わせて、３ｘ３の行列であるMagFrameMを生成する。
一方、ステップＳ４０において、AccRefとMagRefを用いて、３×３の行列であるMagRefMを計算する。
ステップＳ４２において、AccRefとMagRefの外積を求め、その結果を正規化することにより、MagCrossAccを求める。
ステップＳ４４において、AccRefとMagCrossAccの外積を求め、その結果を正規化することにより、MagCrossを求める。
ステップＳ４６において、AccRefと、ステップＳ４２で求めたMagCrossAccと、ステップＳ４４で求めたMagCross（いずれも１×３の行列（ベクトル））とを組み合わせて、３ｘ３の行列であるMagRefMを生成する。
ここで、上記ステップＳ４０（Ｓ４２−Ｓ４６）は、初期化処理が実行され、AccRef及びMagRefが変更されたときに実行され、以後、再度の初期化処理が実行されるまで、求めたMagRefMの値を保持して利用しても良い。
ステップＳ５０において、MagFrameとMagRefMの内積を求める。求められた行列をmag_triad（３×３）と呼ぶ。行列mag_triadは、デバイス座標系から絶対座標系へ変換するための行列である。ＴＲＩＡＤアルゴリズムにおいて、三つの列を、それぞれ、ＴＲＩＡＤ１、ＴＲＩＡＤ２、ＴＲＩＡＤ３と呼ぶ。
ステップＳ６０において、mag_triadの逆行列（絶対座標系からデバイス座標系への変換のための行列）を求め、これをクォータニオンに変換する。このようにして得られたクォータニオンが、第二の姿勢情報である。また、mag_triadの逆行列が回転行列ＤＣＭ１である。
＜姿勢情報変換処理部１００＞
図１１を用いて図４の姿勢情報変換処理部１００について説明する。姿勢情報変換処理部１００は姿勢情報取得部１０５と座標系変換処理部１０６を有している。姿勢情報取得部１０５は、姿勢情報推定部１１０からＤＣＭ１を取得し、補正パラメータ推定処理部７００からヨー角補正値を取得する。
そして、座標系変換処理部１０６は、デバイス座標系三軸加速度取得部１２０が取得したデバイス座標系加速度に、回転行列ＤＣＭ１を乗算し、絶対座標系での三軸加速度（絶対加速度）を算出する。また、絶対座標系における三軸加速度ベクトルを回転行列ＤＣＭ２で座標変換する。
図１１に示すように、座標系変換処理部１０６は、回転行列ＤＣＭ１を用いてデバイス座標系における三軸加速度ベクトルを、絶対座標系における加速度ベクトルへと座標変換する。
回転行列ＤＣＭ２は方向余弦行列であり、回転行列ＤＣＭ２の各要素が方向余弦と称される。回転行列ＤＣＭ２は絶対座標系における三軸加速度ベクトルを、ヨー角補正値で進行方向の補正を加えた絶対座標系加速度ベクトル１０７へと座標変換する行列である。回転行列（ＤＣＭ１とＤＣＭ２）は同じ3×3の行列である。
図１２は、回転行列ＤＣＭ２を説明する図の一例である。回転行列ＤＣＭ２ はオイラー角を用い図１２のように表わされる。図２に示したように、Ｘ軸を中心とする回転角をロール角φ、Ｙ軸を中心とする回転角をピッチ角θ、Ｚ軸を中心とする回転角をヨー角ψと称する。
ヨー角補正値によりヨー角ψを補正するので、座標系変換処理部１０６は、回転行列ＤＣＭ２のロール角φとピッチ角θにゼロ(＝０)を代入し、ヨー角ψに、補正パラメータ推定処理部７００から通知され取得されたヨー角補正値(deg)を代入する。
これにより、回転行列ＤＣＭ１とＤＣＭ２を用いた行列の積演算によって、絶対座標系において方位角に対して補正が加えられた（（方位のオフセットがキャンセルされた）絶対座標系加速度ベクトル１０７を進行方向推定部２００が取得できる。なお、ヨー角補正値の求め方については図３５等において後述する。
＜進行方向推定部＞
図１３は、進行方向推定部２００の機能を説明する図の一例である。慣性装置１において進行方向推定機能を実行する進行方向推定部２００は、バンドパスフィルタ２０１と、ピーク検出部２０４と、ピーク位置記憶部２０５と、移動加速度記憶部２０６と、水平成分移動速度特徴情報管理部２０７と、鉛直成分ピーク移動加速度取得部２０８と、水平成分移動速度特徴情報取得部２０９と、周期取得部２１０と、判定部２１１と、進行方向算出部２１２とを含む。進行方向推定部２００は、姿勢情報変換処理部１００によって得られた、絶対座標系加速度ベクトル１０７から、ユーザの一歩ごとの進行方向を求める。
バンドパスフィルタ２０１は、姿勢情報変換処理部１００より出力された絶対座標系加速度ベクトル１０７から重力成分を除去する。通過帯域は、例えば、一般的な歩行周波数である１−３Ｈｚ程度とする。なお、通過帯域は、慣性装置１の測定対象の歩行又は移動に係る周波数に応じて、適宜変更され得る。ここでは、バンドパスフィルタ２０１が出力する、重力成分が除去された絶対加速度を、移動加速度２０２と呼ぶ。移動加速度２０２は、後述する、移動加速度記憶部２０６に記憶される。また、移動加速度の鉛直成分である、鉛直成分移動加速度２０３は、後述する、ピーク検出部２０４に渡される。
ピーク検出部２０４は、バンドパスフィルタ２０１から出力される移動加速度２０２のうち、鉛直成分移動加速度２０３の変化（時間変化）を観測し、その波形の谷ピークの位置（ピーク時刻又はピーク位置）を検出する。検出されたピーク位置は、後述する、ピーク位置記憶部２０５に記憶される。以下では、谷ピークの検出方法について説明する。
図１４は、横軸を時間とした、鉛直成分移動加速度２０３（Ｚ）と、移動加速度の水平成分（Ｘ、Ｙ）の変化を表す波形である。このように、それぞれの波形は、移動周期（例えば、歩行周期）と整合する周期を持った波形となり、特に、鉛直成分移動加速度２０３は、±１ｍ／ｓ2程度の、水平成分に比べて大きな振幅波形が出力される。山ピークは、足が地面に着地した際に現れ、谷ピークは、片方の軸足をもう一方の足が追い越す瞬間に現れる。
図１５は、歩行動作における鉛直方向の運動特性を説明する図である。歩行動作は、下肢の動作により、一般に、立脚相と遊脚相とに分類される。立脚相（Stance Phase）は、一側下肢の踵が接地して、同側足先が離れるまでの時期である。また、遊脚相（Swing Phase）は、一側下肢の足趾が地面から離れてから、同側足の踵が地面に接するまでの時期である。更に、歩行動作は、両脚支持期の有無によって特徴付けられる。一般に、歩行が緩徐になると、歩行の全体に占める両脚支持期の割合が増加し、歩行速度が速くなると少なくなることが知られている。また、両脚支持期は、走行状態になると、消失することも知られている。また、体の上下（鉛直方向）移動量及び左右（水平面方向）移動量は、立脚中期に最大となることが知られている。
立脚中期の前半は、蹴り出した片方の足が軸足追い抜く（蹴り出した片方の足が体幹直下を通り過ぎる）動作を含む。このとき、体は鉛直上方向に移動し、鉛直上向きの移動加速度が発生する。一方、立脚中期の後半は、蹴り出した片方の足の踵が接地するまでの動作を含む。このとき、体は鉛直下方向に移動し、鉛直下向きの移動加速度が発生する。
図１６は、歩行動作における水平方向の運動特性を説明する図である。立脚中期の前半の移動加速度の水平方向成分は、目標となる地点への体幹の移動のために片足を蹴り出した際の加速度と、体の重心移動に伴う横揺れに起因した加速度の影響を含む。一方、立脚中期の後半の移動加速度の水平方向成分は、蹴り出した足を踵接地地点まで移動させることに伴う体幹の移動と、進行方向に対する体の横揺れで発生する加速度と、踵を接地した際の振動に伴う加速度の影響を含む。つまり、立脚中期の後半には、蹴り出しの際に必要となる純粋な移動加速度を包含していないということになる。
従って、本願では、体幹の移動のために片足を蹴り出した際の加速度が大きく反映される、立脚中期の前半の移動加速度を用いて、進行方向の推定を行う。
そこで、鉛直成分移動加速度２０３に着目し、シグナルの閾値判定により、谷ピークを検出することで、一歩を計測する。なお、谷ピークを用いて一歩を検出する理由は、足が着地する山ピーク位置での水平方向の移動加速度は、足が着地することに伴う振動やノイズを含む可能性があるためである。谷ピーク位置での水平方向の移動加速度は、足の着地に伴う影響を減少させ、歩行に伴う実際の加速度をより的確に反映する。
ピーク検出部２０４は、鉛直成分移動加速度２０３が、所定の閾値Ｔｈを下回り、再度、閾値Ｔｈを越える瞬間を捉えることにより、ピーク検出を行なう。ここで、所定の閾値Ｔｈを下回ったときの時刻ｔaと、閾値Ｔｈを越えたときの時刻ｔbとの中間の時刻を、ピーク位置とすることができる。例えば、閾値Ｔｈは、実際に歩行した際の、鉛直方向の移動加速度の半分程度の値を設定するのが望ましい。なお、ピーク検出のために、上記以外の任意の方法を用いても良い。
また、過去のピーク位置を記録しておくことにより、現在のピーク位置と、過去のピーク位置との間の時間を表すピーク間隔を求めることができる。
ピーク位置記憶部２０５は、ピーク検出部２０４の検出したピーク位置を記憶する。ピーク位置記憶部２０５は、例えばリングバッファにより、ピーク位置（時間）を、最新のものから、過去の時間領域に相当するものまでを記憶する。ピーク位置記憶部２０５は、少なくとも、最新のピーク位置と一つ過去のピーク位置を記憶し、後に得られたピーク位置によって随時更新される。記憶されるピーク位置の数は、慣性装置１の記憶容量に応じて適宜変更されてもよい。
移動加速度記憶部２０６は、バンドパスフィルタ２０１から出力された移動加速度２０２を、観測された時刻情報を付加し時系列データとして記憶する。
水平成分移動速度特徴情報管理部２０７は、ピーク検出部２０４によるピーク位置の検出に応じて、そのピーク位置を中心とする所定の期間(τ)内の、水平成分の移動加速度を、各成分（Ｘ、Ｙ）ごとに積算処理し、水平速度を算出する。この水平速度を、水平成分移動速度特徴情報と呼ぶ。水平成分移動速度特徴情報は、速さの方向と大きさの相対値を示すベクトルである。水平成分移動速度特徴情報管理部２０７は、水平成分速度特徴情報を、時刻情報tと共に記憶する。すなわち、水平成分移動速度特徴情報管理部２０７は、水平成分移動速度特徴情報を算出する算出部としての機能と、水平成分移動速度特徴情報を記憶する記憶部としての機能を有する。
図１７は、図１４に対応する、移動加速度の水平成分と、鉛直成分の、時間に対する変化を表す波形を示す。この例では、移動加速度の鉛直成分の波形から求められるピーク位置ｔ1、ｔ2、ｔ3を中心とする、τの期間の、水平成分の移動加速度が、時間積分され、水平成分移動速度特徴ベクトルＶ1、Ｖ2、Ｖ3が算出される。
期間(τ)は、例えば、（ｔb−ta）時間以内の期間であることが望ましい。これは、全時間領域において積算を実行すると、進行方向に対する体の横揺れで発生する加速度と、踵を接地した際の振動に伴う加速度の影響を大きく含んだまま積算してしまい、進行方向の推定が正しく行なえない可能性があるためである。
水平成分移動速度特徴情報は、前述のピーク検出処理と、その後の一連の処理によって、一歩足を踏み出し、片足が軸足を追い越す瞬間に生成される。生成される特徴情報は、大きさと方向を特徴として持った水平成分移動速度特徴ベクトルVである。図１８に例示されるように、水平成分移動速度特徴ベクトルは、片足が軸足を追い越す瞬間に歩行者の体が移動している方向（進行方向）と強度を表している。
鉛直成分ピーク移動加速度取得部２０８は、時刻tにおけるピーク位置（時間）に対応する、鉛直成分移動加速度２０３における移動加速度（鉛直成分ピーク移動加速度）を取得し、この加速度を、後述する、判定部２１１に渡す。
水平成分移動速度特徴情報取得部２０９は、水平成分移動速度特徴情報管理部２０７から、最新の水平成分移動速度特徴情報及び過去の水平成分移動速度特徴情報を取得し、取得した水平成分移動速度特徴情報を、後述する、判定部２１１に渡す。
周期取得部２１０は、ピーク位置記憶部２０５から複数のピーク位置を取得し、測定対象の移動周期（例えば、歩行周期）への換算処理を実行する。また、周期取得部２１０は、複数のピーク位置の差分を順次計算することによって、最新の移動周期及び過去の移動周期を取得することができる。周期取得部２１０は、取得した複数の移動周期を、後述する、判定部２１１に渡す。
判定部２１１は、図１９に示される手順を実行することにより、これまでに検出された各種情報が歩行動作に由来して取得されたデータであるかどうかを判定する。歩行動作は、歩行や走行等を含んだ、測定対象による移動動作である。一方、歩行動作でない非歩行動作は、慣性装置を無作為又は意図的に振る動作や、外部環境から加速度を受けた場合（例えば、外部の移動物体により移動させられている場合）のように、測定対象による移動動作のみに起因しない動作である。以下、図１９に示されるステップに沿って説明する。
まず、ステップＳ１００において、鉛直成分ピーク移動加速度取得部２０８から取得した、鉛直成分ピーク移動加速度が、所定の範囲内である場合に、ステップＳ２００に進む。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。なお、鉛直成分ピーク移動加速度についての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。
次に、ステップＳ２００において、水平成分移動速度特徴情報管理部２０７から取得した、水平成分移動速度特徴情報（ベクトル）の大きさが、所定の範囲内である場合に、ステップＳ３００に進む。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。なお、水平成分移動速度特徴情報（ベクトル）の大きさについての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。
次に、ステップＳ３００において、周期取得部２１０から取得した、移動周期が、所定の範囲内である場合に、ステップＳ４００に進む。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。なお、移動周期についての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。
次に、ステップＳ４００において、左右方向の移動速度のぶれ幅が、所定の範囲内である場合に、ステップＳ５００に進み、判定部２１１は、歩行動作に起因すると判定する。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。
ここで、左右の移動速度のぶれ幅について、図２０を用いて説明する。人の歩行では、右足を踏み出すと、右方向に移動速度ベクトルが発生し、左足を踏み出すと、左方向に移動速度ベクトルが発生する。本判定を行うために、水平成分移動速度特徴ベクトルがこの特性に適合するかどうか判定する。
判定部２１１は、まず、水平成分移動速度特徴ベクトルの始点と終点を、図２０（ｂ）のように結合する。次に、判定部２１１は、各Ｖnベクトルの中点を結ぶ直線と、各Ｖnベクトルの終点との距離ｄnを算出する。そして、判定部２１１は、ｄnが所定の範囲内であるか判定を行い、範囲内であれば、歩行動作に起因すると判定する。なお、ｄnについての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。
ステップＳ５００では、判定部２１１は、歩行動作に起因すると判定する。
ステップＳ６００では、判定部２１１は、非歩行動作に起因すると判定する。
なお、上記したステップＳ１００−Ｓ４００における判定のうち、一部の判定が省略されてもよい。しかしながら、全ての判定がなされることで、より精度の高い進行方向の推定が可能となる。
進行方向算出部２１２は、上記した判定部２１１が、「歩行動作に起因する」であると判定した場合に、一歩ごとの進行方向２１３を求めるために、以下の処理を行う。
進行方向算出部２１２は、ユーザが、ゼロ歩目から一歩目に移るとき、水平成分移動速度特徴情報取得部２０９より、水平成分移動速度特徴ベクトルＶ0を取得する（図２１（ａ））。また、進行方向算出部２１２は、ユーザが、一歩目から二歩目に移るとき、水平成分移動速度特徴情報取得部２０９より、水平成分移動速度特徴ベクトルＶ1を取得する（図２１（ａ））。
ここで、進行方向算出部２１２は、ベクトルＶ0とベクトルＶ1とを規格化し、ベクトルＶ0'とベクトルＶ1'を得る（図２１（ｂ））。そして、進行方向算出部２１２は、それらの合成ベクトル（Ｖ0'＋Ｖ1'）を求め、合成ベクトルの向きを、一歩ごとの進行方向２１３と推定する（（図２１（ｃ）））。上記処理は、ユーザが一歩踏み出すごとに、逐次実行される。
このように、慣性装置１は、一歩ごとの進行方向を推定するために、絶対座標系における加速度の、鉛直方向の加速度の谷ピーク位置を基点とし、その前後の一定の時間枠を用いて算出水平方向の速度ベクトルを用いる。これにより、歩行者の着地の際に生ずる振動等による影響を少なくすることができ、推定の精度が向上する。
更に、慣性装置１は、地磁気センサによるセンサ情報が信頼できるかどうかを評価し、センサ情報が信頼できる場合に、そのセンサ情報を、慣性装置１の姿勢を表すベクトルを補正（ヨー角成分）するために使用する。これにより、姿勢を表すベクトルを、地磁気センサ情報を用いて、より高い精度で補正することが可能となる。
＜位置推定部＞
図２２は、位置推定部８００が有する機能を説明する図の一例である。位置推定部８００は現在位置を推定する。位置推定部８００は、進行速度推定部３００、絶対位置情報入力部４００、絶対位置推定部５００、速度補正部３５０及びマップマッチング部６００を含む。以下、進行速度推定部３００と、絶対位置情報入力部４００と、絶対位置推定部５００と、マップマッチング部６００と、速度補正部３５０とが提供する機能について、それぞれ説明する。
<<進行速度推定部>>
図２３は、進行速度推定部３００の内部の詳細な機能ブロック図を表す。進行速度推定部３００は、水平成分速度特徴情報取得部３０１と、鉛直成分ピーク加速度取得部３０２と、第２周期取得部３０３と、ぶれ幅取得部３０４と、進行方向取得部３０５と、変換部３０６と、進行速度波形生成部３１２と、進行速度波形合成部３１４と、進行速度波形記憶部３１５とを有する。
進行速度推定部３００は、進行方向推定部２００が算出する、水平成分速度特徴情報２１４、鉛直成分ピーク加速度２１５、周期２１６、ぶれ幅２１７及び進行方向２１３に基づき、測定対象の実際の進行速度を表す進行速度推定ベクトルを算出する。以下，その処理内容について説明する。
水平成分速度特徴情報取得部３０１は、水平成分移動速度特徴情報管理部２０７から水平成分速度特徴情報２１４（水平成分速度特徴ベクトル）を取得し、この情報を変換部３０６に入力する。
鉛直成分ピーク加速度取得部３０２は、移動加速度記憶部２０６から、鉛直成分ピーク加速度２１５を取得し、この情報を変換部３０６に入力する。
第２周期取得部３０３は、ピーク位置記憶部２０５に記憶された情報を用いて、測定対象の移動に係る周期２１６を取得し、この情報を変換部３０６に入力する。
ぶれ幅取得部３０４は、水平成分速度特徴情報２１４を用いて、左右の移動速度のぶれ幅（以後、ぶれ幅２１７とする）を取得し、この情報を変換部３０６に入力する。ぶれ幅２１７の算出方法は、図１４等を用いて説明した通りである。
進行方向取得部３０５は、進行方向推定部２００の進行方向算出部２１２が出力した進行方向２１３を取得し、この情報を変換部３０６に入力する。
変換部３０６は、入力された水平成分速度特徴情報２１４、鉛直成分ピーク加速度２１５、周期２１６、ぶれ幅２１７及び進行方向２１３を、それぞれ、速度パラメータＰａ３０７、強度パラメータＰｂ３０８、周期パラメータＰｃ３０９、ぶれ幅パラメータＰｄ３１０及び進行方向パラメータＰｅ３１１に変換する。当該変換は、例えば、符号２１４−２１７の各情報を、所定の規則に従って正規化する。変換部３０６は、入力された各種情報を、所定の範囲のパラメータに変換するために、任意の方法を用いることができる。
進行速度波形生成部３１２は、入力された各パラメータ３０７−３１１と、予め登録された測定対象の属性情報とをキーとして、パラメータＤＢ３１３（例えば、図２３）を参照し、対応する速度生成係数Ｃａ−Ｃｃを取得する。次に、進行速度波形生成部３１２は、入力された各パラメータと、取得した速度生成係数Ｃａ−Ｃｃを用いて、後述する式により、進行速度波形を生成する。
ここで、測定対象の属性情報は、例えば、ユーザの性別、年齢、身長等を含む。なお、測定対象の属性情報は、測定対象を特定するための任意の情報であってもよい。例えば、その他の属性情報の例として、測定対象の種別（人、動物、二足歩行ロボット等）、識別番号、型番、性質（高速、低速）等が挙げられるが、この限りではない。
図２４に例示されるパラメータＤＢ３１３は、測定対象の属性情報である、性別、年齢及び身長を、パラメータ３０７−３１０及び速度生成係数Ｃａ−Ｃｃと関連付けている。図２４の例では、簡単のため、速度生成係数Ｃａ−Ｃｃを取得するために，進行方向パラメータＰｅ３１１を用いていない（すなわち、速度生成係数は、進行方向に拠らず決定される）。しかしながら、進行方向パラメータＰｅ３１１を用いて、パラメータＤＢ３１３を構築しても良い。
パラメータＤＢ３１３は、慣性装置１の提供者等が、属性情報によって特定される測定対象の集団に対して、パラメータ３０７−３１１で特定される動作を行わせることにより、取得したデータをもとに、予め構築するものである。なお、速度パラメータＰａについては、そのノルムの値がテーブルに登録される。
進行速度波形生成部３１２は、入力された各種パラメータと一致するエントリが、パラメータＤＢ３１３に存在しない場合には、パラメータの組み合わせと類似するエントリの速度生成係数Ｃａ−Ｃｃを取得する。例えば、進行速度波形生成部３１２は、類似するエントリを検出するために、各パラメータの二乗平均平方根（Root mean Square）の合計値が最も小さいものを選択することができる。類似するエントリを検出するために、任意の方法を用いることができる。進行速度波形生成部３１２は、取得した速度生成係数Ｃａ−Ｃｃを、進行速度波形生成部３１２に渡す。
進行速度波形生成部３１２は、速度生成係数Ｃａ−Ｃｃと、入力された各パラメータＰａ３０７−Ｐｅ３１１とを用いて、例えば、以下のような式により、時刻０≦ｔ≦Ｐｃにおける、測定対象の速度変化を表す進行速度波形を生成する。
Figure 0006852310
 なお、進行速度波形生成部３１２は、上記の式以外の任意の式を用いて、進行速度波形を生成しても良い。
生成された進行速度波形の例を図２５に示す。ここで、θは、進行方向パラメータＰｅに対応する。数１１によって示される式は、速度パラメータＰａ３０７による推定項(第１項)と、強度パラメータＰｂ３０８での推定項(第２項)と、ぶれ幅パラメータＰｄ３１０での推定項(第３項)を含む。各項は、それぞれの重みとして、速度生成係数Ｃａ−Ｃｃが乗算されており、複数のパラメータに基づく速度推定を行うことにより、精度の高い歩行速度波形を生成することが可能である。なお、速度生成係数Ｃａ−Ｃｃは、和が１となるように、予め正規化されて、パラメータＤＢ３１３に格納されている。進行速度波形生成部３１２は、生成した進行速度波形を、進行速度波形合成部３１４に渡す。
進行速度波形合成部３１４は、進行速度波形生成部３１２から進行速度波形を取得すると、後述する、進行速度波形記憶部３１５に記憶された、過去に合成された進行速度波形を取得し、これらを合成する。このとき、進行速度波形合成部３１４は、対応する時間位置において、これらの進行速度波形を積算することにより合成する。進行速度波形合成部３１４は、進行速度波形を任意の方法で合成してもよい（例えば、複数の進行速度波形が重なった場合には各々を比較しその最大値を採用する、等）。
図２６は、進行速度波形合成部３１４が、複数の進行速度波形を積算して合成した例を表している。進行速度波形合成部３１４は、合成した進行速度波形を、進行速度波形記憶部３１５に記憶する。
進行速度波形記憶部３１５は、進行速度波形合成部３１４により合成された進行速度波形を、時刻情報と共に記憶する。
このようにして、随時取得されるパラメータに基づいて合成された最新の進行速度波形が、進行速度波形記憶部３１５に記憶される。後述する絶対位置推定部５００及びマップマッチング部６００は、現時点における、合成された当該進行速度波形の値を参照することにより、最新の進行速度を表す進行速度情報を取得することができる。進行速度情報は、上記したように、水平方向の二成分によって表されるため、以下では、この進行速度情報を、進行速度推定ベクトル３１６と呼ぶ。
<<絶対位置情報入力部>>
図２７は、絶対位置情報入力部４００、絶対位置推定部５００及びマップマッチング部６００のそれぞれの内部の詳細な機能ブロック図を表す。
絶対位置情報入力部４００は、第一の絶対位置取得部４０１と、第二の絶対位置取得部４０２と、測位時間計測部４０３と、誤差補正部４０４とを含む。絶対位置情報入力部４００は、慣性装置１の絶対位置を表す情報と、その情報に含まれる誤差の大きさを表す誤差情報とを絶対位置推定部５００に入力する。
第一の絶対位置取得部４０１は、例えば、Bluetooth（登録商標）通信により、外部に設けられた位置情報送信装置と通信し、慣性装置１の絶対位置を表す絶対位置情報を取得する。絶対位置情報は、例えば、緯度、経度、高度によって表される位置ベクトルＸ1、Ｙ1、Ｚ1であってもよいし、決まった地点を基点とする、位置を表す任意のベクトルであってもよい。また、第一の絶対位置取得部４０１は、位置情報送信装置から、上記情報の誤差を表す誤差情報σ1も取得する。誤差情報σ1は、上記の位置情報送信装置から取得される、絶対位置についての誤差共分散行列である。誤差情報σ1は、例えば、位置情報送信装置と慣性装置１との通信に係る電波強度に応じて決定される誤差値を含む。例えば、電波強度が低い値を示す場合には、より誤差の大きな値を有する誤差共分散行列が取得される。誤差情報は、予め慣性装置１に記憶されていてもよいし、位置情報送信装置から送信されてもよい。取得した位置ベクトル及び誤差情報は、後述する、絶対位置推定部５００の第一の観測更新処理部５０２に渡される。
第二の絶対位置取得部４０２は、第一の絶対位置取得部４０１と異なる通信手段（例えば、ＧＰＳ又はＩＭＥＳ（Indoor Messaging System））により、絶対位置情報Ｘ2、Ｙ2、Ｚ2と誤差情報σ2を取得する。なお、絶対位置取得部は、少なくとも一つ存在していればよく、その数は任意である。取得した位置ベクトル及び誤差情報は、後述する、絶対位置推定部５００の第二の観測更新処理部５０３に渡される。
第一の絶対位置取得部４０１と第二の絶対位置取得部４０２が取得する絶対位置情報と誤差情報を図２２では絶対位置情報４０５、誤差情報４０６で示した。
測位時間計測部４０３は、第一の絶対位置取得部４０１及び第二の絶対位置取得部４０２が、それぞれ絶対位置情報を取得する間隔を計測し、その間隔を、後述する誤差補正部４０４に渡す。
誤差補正部４０４は、測位時間計測部４０３から受け取った間隔が、所定の間隔であるかどうかを判定し、その間隔の大きさに応じて、各絶対位置取得部から出力される誤差共分散行列（σ1又はσ2）の各分散値が大きくなるよう補正をかける。このために、誤差補正部４０４は、予め用意した、間隔［秒］と補正量（分散値に乗算する値）とを関連付けるテーブルを利用することができる。あるいは、誤差補正部４０４は、間隔の大きさの閾値を設け、閾値を超えると、誤差共分散行列に対して所定の補正をかけてもよい。
特に、ＧＰＳやＩＭＥＳの誤差情報は、マルチパスの影響を考慮して生成されないため、誤差情報に対する信頼度が、電波状況によって変化する可能性がある。信号量と雑音量の比率を表すＳ／Ｎ比と、測位間隔の大きさとの間には、図２８に示すような相関関係がある。そこで、誤差補正部４０４は、測位間隔が所定の間隔を超える場合には、測位誤差がより大きくなるよう補正をかけることにより、誤差情報の信頼度の変化を吸収することができる。誤差情報が直接、送信されない場合については後述する。
なお、外部の位置情報送信装置は、上記の通信方式のほか、赤外線、無線ＬＡＮ、可視光通信、カメラを用いた測位手段等の手段により、絶対位置情報及び誤差情報を送信してもよい。慣性装置１は、対応する受信手段により、絶対位置情報及び誤差情報を受信することができる。受信した絶対位置情報及び誤差情報は、上記した絶対位置取得部と同様に、絶対位置推定部５００の観測更新処理部に入力される。絶対位置取得部及び観測更新処理部の数は、任意である。
<<絶対位置推定部>>
絶対位置推定部５００は、時間発展処理部５０１と、第一の観測更新処理部５０２と、第二の観測更新処理部５０３と、第三の観測更新処理部５０４とを含む。
絶対位置推定部５００は、進行速度推定ベクトル３１６と、進行速度推定ベクトル３１６と共に提供される進行速度測定誤差情報３１７とを用いて、現在位置と、その誤差を表す誤差情報（現在位置と誤差情報５０５）を求める。進行速度測定誤差情報３１７は、進行速度推定ベクトル３１６の誤差を表す誤差共分散行列σvであり、システム同定によって求めた固定のデータが使用される。あるいは、進行速度測定誤差情報３１７は、進行速度に応じて用意された、複数のデータが使用されてもよい。
また、絶対位置推定部５００は、絶対位置情報入力部４００が出力した絶対位置情報（位置ベクトル）及び誤差情報（誤差共分散行列）とを用いて、現在位置と誤差情報５０５を更新する。更に、絶対位置推定部５００は、マップマッチング部６００の出力する位置情報（位置ベクトル）と誤差情報（誤差共分散行列）とを用いて、現在位置と誤差情報５０５を更新する。
拡張カルマンフィルタを用いて、現在位置と誤差情報５０５を算出又は更新する。ここでは、一つの時間発展の手続き（時間発展処理部５０１）と、三つの観測更新の手続き（第一の観測更新処理部５０２、第二の観測更新処理部５０３、第三の観測更新処理部５０４）が、並列に実行される。各手続きにおいて用いられる変数及びモデルを以下で説明する。
時間発展処理部５０１は、図２９−図３１に示す変数及びモデルの定義に従って、拡張カルマンフィルタの時間発展の手続きを実行し、慣性装置１の現在位置と誤差情報５０５を算出又は更新する。ここで、拡張カルマンフィルタにおける変数とモデルを、図２９に示すように定義する。すなわち、今の時刻の状態推定値は、図２９の式（１）−１のように、三次元の位置ベクトルによって定義される。また、入力量は、図２９の式（１）−４のように、進行速度推定部３００によって出力された、進行速度推定ベクトル３１６によって定義される。また、システムの状態推定モデルは、図２９の式（１）−４のように定義される。
また、図３０に示す通り、時間発展における偏微分行列（ヤコビアン）は、システムの状態推定モデルにおける右辺の偏微分が、そのまま偏微分行列となる。
また、図３１に示す通り、プロセスノイズＱkは、システム同定によって予め求められる、進行速度測定誤差情報３１７（σv）であり、予め算出された定数である。なお、現在時刻の誤差共分散行列Ｐk|k-1及びＰk-1|k-1は、３×３の行列となり、行列内の要素は実数となる。
第一の観測更新処理部５０２は、拡張カルマンフィルタの観測更新の手続きを実行し、慣性装置１の現在位置と誤差情報５０５を算出又は更新する。ここで、拡張カルマンフィルタにおける変数を、図３２を用いて説明する。すなわち、１ステップ前の時間の観測値は、図３２の式（１）−３のように、三次元の位置ベクトルとなる。また、観測値は、図３２の式（１）−２のように、第一の絶対位置取得部４０１が出力する位置ベクトル（絶対位置情報）である。上記のｈ及びｚkにより、観測残差
Figure 0006852310
 が求まる。
また、拡張カルマンフィルタの一般式における（２）観測更新における偏微分行列（ヤコビアン）Ｈkは、図３２の式（１）−３に表される観測値ｈの偏微分を求めることによって求まる。
また、拡張カルマンフィルタの一般式における（３）残差の共分散Ｓkは、第一の絶対位置取得部４０１の出力した誤差情報である観測ノイズ（行列）Ｒkと、観測更新における偏微分行列Ｈk、その転置行列ＨkT及び現在時刻の誤差共分散行列Ｐk|k-1を用いて求まる。
Figure 0006852310
 なお、ｒ1はＸ軸、ｒ2はＹ軸、ｒ3はＺ軸の成分を表す。
また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫkは、現在時刻の誤差共分散行列Ｐk|k-1と、観測更新における偏微分行列の転置行列ＨkTと、残差の共分散の逆行列Ｓk-1から求まる。
同様に、拡張カルマンフィルタの一般式における、（５）更新された状態推定値ｘk|k及び（６）更新された誤差共分散行列Ｐk|kは、これまでに求められた変数を用いて求めることができる。
第二の観測更新処理部５０３は、第一の観測更新処理部５０２と同様に、拡張カルマンフィルタの観測更新の手続きを実行し、慣性装置１の現在位置と誤差情報５０５を算出又は更新する。拡張カルマンフィルタにおける変数は、観測値ｚk及び観測ノイズＲkが、第二の絶対位置取得部４０２によって出力される位置ベクトル及び誤差情報である点を除いて、第一の観測更新処理部５０２と同様である。
第三の観測更新処理部５０４は、第一の観測更新処理部５０２と同様に、拡張カルマンフィルタの観測更新の手続きを実行し、慣性装置１の現在位置と誤差情報５０５を算出又は更新する。拡張カルマンフィルタにおける変数は、観測値ｚk及び観測ノイズＲkが、後述する、マップマッチング部６００によって出力される位置ベクトル及び誤差情報である点を除いて、 第一の観測更新処理部５０２と同様である。
絶対位置推定部５００は、以上の通り、拡張カルマンフィルタの枠組みにおいて、現在位置と誤差情報５０５を逐次更新することで、現在位置を高精度に推定することができる。慣性装置１に搭載された外部のアプリケーションは、現在位置と誤差情報５０５を参照することにより、現在位置の推定位置だけでなく、慣性装置の向き（ヘディング情報、ヨー角推定値）を取得することができる。
なお、第一の観測更新処理部５０２又は第二の観測更新処理部５０３が慣性装置１の現在位置と誤差情報５０５を算出又は更新すると、絶対位置補正フラグ３１９がＴＲＵＥとなる。ＴＲＵＥとなった絶対位置補正フラグ３１９は補正パラメータ推定処理部７００に通知され、これによりＦＡＬＳＥとなる（次ステップの絶対位置・誤差情報が通知される前にクリアされる）。
<<マップマッチング>>
マップマッチング部６００は、マップマッチング処理部６０１を含み、最新の現在位置と誤差情報５０５と、進行速度推定ベクトル３１６を取得し、マップマッチング処理を実行する。
マップマッチング部６００は、マップマッチング処理部６０１を含み、最新の現在位置と誤差情報５０５と、進行速度推定ベクトル３１６を取得し、マップマッチング処理を実行する。
マップマッチング処理部６０１は、最新の現在位置と誤差情報５０５と、進行速度推定ベクトル３１６を取得し、予め用意された地図データベース６０２を参照して、進行可能な領域を表す領域情報を取得する。次に、マップマッチング処理部６０１は、既知の技術であるパーティクルフィルタのアルゴリズムを適用し（例えば、非特許文献３）、マップマッチング処理を実行する。マップマッチング処理部６０１は、現在位置が、マップ上で進行が不可能な場所を示していた場合には、進行可能な領域に現在位置を修正する。また、マップマッチング処理部６０１は、内部処理により、修正された位置を表す位置ベクトルの各成分の誤差量を表す誤差共分散行列σmを算出する。マップマッチング処理部６０１は、修正された現在位置を表す位置ベクトル（Ｘm、Ｙm、Ｚm）及び誤差共分散行列σmを、上記したように、絶対位置推定部５００の第三の観測更新処理部５０４に渡す。
＜速度補正部＞
速度補正部３５０は、補正パラメータ推定処理部７００より通知される歩行速度推定パラメータに基づいて進行速度推定ベクトル３１６を補正する処理を実行する。補正の方法は、例えば、補正速度推定パラメータを進行速度推定ベクトル３１６に乗算することなどでよい。あるいは、ルックアップテーブルに補正速度推定パラメータと補正値を対応付けておき、補正速度推定パラメータに対応付けられた補正値をルックアップテーブルから取得して進行速度推定ベクトル３１６に乗算してもよい。この場合、非線形な補正が可能になる。なお、補正方法はこれらに限定はされない。
図３３は、補正後の速度情報を模式的に示す。波形合成後の速度情報が補正速度推定パラメータにより補正され、補正後の速度情報３１６Ａが得られる。なお、図３３では速度情報が大きくなるように補正されているが、速度情報が小さく補正される場合もある。
＜測位誤差・座標推定部＞
測位誤差・座標推定部３３０は、絶対位置座標に対する誤差情報を推定する処理を実行する。例えばＢｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０などの電波で絶対位置情報を慣性装置１が受信することができるが、絶対位置情報は絶対位置情報を送信する送信機３４０の位置を示しており、慣性装置１の位置を示していない。しかし、この場合、受信信号強度の大きさから誤差を推定することができる。
Figure 0006852310
 表１は図１で受信エリアＡＲＥＡ１と受信エリアＡＲＥＡ２を形成する送信機３４０が送信した絶対位置情報を示す。絶対位置情報ＩＤに対応付けて緯度と経度が周囲の慣性装置１に送信されている。慣性装置１はこの絶対位置情報を受信した際の受信信号強度を測定している。測位誤差・座標推定部３３０はこの受信信号強度を用いることで誤差情報を推定できる。
図３４は絶対位置情報の送信機３４０と慣性装置１との距離に応じた受信信号強度の変化の一例を示す図である。このような波形が既知であれば、測位誤差・座標推定部３３０は慣性装置１が絶対位置情報を受信した受信信号強度により送信機３４０までの距離を推定できる。具体的には、受信信号強度を距離に変換する関数やテーブルを設置者等が作っておき、測位誤差・座標推定部３３０はこれらから得られる距離を誤差情報として推定する。このようにして得られた誤差情報は図２２の誤差情報４０６として使用される。
なお、送信機３４０が音波（音声や非可聴音）を用いて絶対位置情報を送信する場合、音声や非可聴音が到達するまでの時間や受信音量を元に誤差を推定することもできる。
また、送信機３４０は絶対位置情報そのものでなく絶対位置情報ＩＤだけを送信する場合がある。この場合、慣性装置１は予め用意された絶対位置情報ＩＤと絶対位置情報の対応テーブルを参照して絶対位置情報を取得する。
＜補正パラメータ推定処理部７００＞
図４に示した補正パラメータ算出部７０１は、補正パラメータ算出部７０１、移動状態検出部７０２、及び、測位履歴記憶部７０３を有している。まず、測位履歴記憶部７０３には、位置推定部８００より通知される絶対位置と誤差情報が時刻情報に対応付けて記憶される。なお、測位履歴記憶部７０３はＲＡＭ１２やＲＯＭ１３に構築される。
測位履歴記憶部７０３には、パラメトリックな手法で第一の観測更新処理部５０２又は第二の観測更新処理部５０３により補正された絶対位置を記憶する。また、時間発展処理部５０１が慣性航法で取得した位置も記憶する。下記の移動状態の判定を行わない場合、補正された絶対位置だけを記憶してもよい。
移動状態検出部７０２は、慣性装置１によって検出される移動が人の歩行や走行によるものか、又は、絶対位置補正によるものかを検出する。具体的には、絶対位置補正フラグのＴＲＵＥを検出した時点から、次に絶対位置補正フラグがＴＲＵＥとなるまでの間の絶対位置を測位履歴記憶部７０３に記憶しておく。絶対位置補正フラグがＴＲＵＥとなってから次に絶対位置補正フラグがＴＲＵＥとなるまでに検出される絶対位置の変化は慣性航法のみにより得られた移動距離であるので、これが所定距離以上であれば、歩行又は走行による移動と判定することができる。この所定距離は適宜決定されるものとする。例えば、数メートルから数十メートルしてよいがこれに限られるものではない。
図１を用いて補正パラメータ算出部７０１の処理を説明する。最後に絶対位置補正フラグがＴＲＵＥとなった際に絶対位置情報を用いてパラメトリックな手法で補正された慣性装置１の絶対位置がＰ´１である。軌跡１０１２を構成する絶対位置、位置１００１、及びＰ´１は測位履歴記憶部７０３に記憶されている。また、絶対位置情報を用いてパラメトリックな手法で慣性装置１の絶対位置が補正されるとＰ´２も記憶される。Ｐ´１、Ｐ´２には絶対位置補正フラグ（ＴＲＵＥ）が付与されている。
次に、絶対位置補正フラグがＴＲＵＥとなる前に、慣性装置１は慣性航法のみにより推定された位置１００１にいる。一方、位置１００１は受信エリアＡＲＥＡ２で受信される絶対位置情報で位置Ｐ´２に補正される。
従って、受信エリアＡＲＥＡ１から受信エリアＡＲＥＡ２に慣性装置１が移動する過程で距離と方位にずれが生じたことになる。補正パラメータ算出部７０１はこのずれを補正する補正パラメータを作成する。具体的には、Ｐ'１と位置１００１の距離Ｂと、Ｐ´１とＰ´２の距離Ａとを算出する。慣性航法が算出する位置は「距離Ａ／距離Ｂ」だけ大きい傾向にあるので、「距離Ａ／距離Ｂ」を歩行速度推定パラメータとする。
また、Ｐ'１とＰ'２を結ぶ方向と、Ｐ'１と位置１００１を結ぶ方向の差が、慣性装置１の正しい方向に対する誤差なので、この差をヨー角補正値Δψとすることができる。Ｐ'１と、Ｐ'２又は位置１００１を結ぶ方向でなくても、例えばＰ'１から歩行者が若干歩行した後の位置とＰ'２又は位置１００１を結ぶ方向を用いてもヨー角補正値Δψを得ることができる。従って、方位の算出の起点となるのはＰ'１そのものでなくてもよく、測位履歴記憶部７０３に記憶されている過去の絶対位置でもよい場合がある。ただし、絶対位置補正フラグ（ＴＲＵＥ）が付与された後の絶対位置のうち絶対位置補正フラグ（ＴＲＵＥ）が付与された絶対位置に近いものほど好ましい。
上記した方向の差をそのままヨー角補正値Δψとするのでなく、補正パラメータ算出部７０１はパラメトリックな手法で位置が補正された際に算出される誤差情報の大きさに応じて方向の差を修正してもよい。例えば、誤差情報を予め定めた閾値で除算して、結果が１以下であればそのまま方向の差をそのままヨー角補正値Δψに決定する。除算した結果が１より大きい場合、除算した結果を方向の差に乗じた値をヨー角補正値Δψに決定する。これにより、誤差情報が大きい場合に進行方向を大きく補正することができる。
上記のように、姿勢情報変換処理部１００はヨー角補正値で慣性装置１の方向（ヨー角）を補正するので慣性装置１の方向は絶対位置情報を取得する毎に正しい方向に補正される。また、位置推定部８００は歩行速度推定パラメータで進行速度推定ベクトルを補正するので、慣性航法により検出される絶対位置を適切に補正できる。
＜慣性装置の全体的な動作＞
図３５は、歩行速度推定パラメータとヨー角補正値の推定手順を示すフローチャート図の一例である。
位置推定部８００は、進行方向推定部２００が推定した進行方向２１３を用いて慣性航法による人の歩行速度と現在位置と推定する(Ｓ１０)。
次に、慣性装置１が受信エリアに進入することで、絶対位置情報入力部４００が送信機３４０から絶対位置情報を取得すると（Ｓ２０のＹｅｓ）、位置推定部８００は現在位置をパラメトリックな手法で補正し、位置座標と該位置座標に対応する誤差情報を推定する（Ｓ３０）。
次に、位置推定部８００は、絶対位置情報を用いて補正された位置座標と誤差情報を、慣性航法による現在の位置に設定する（Ｓ４０）。すなわち、慣性航法が歩行速度に基づいて位置座標を累積するための初期位置が、絶対位置情報を用いて補正された位置座標と誤差情報にリセットされる。なお、ＴＲＵＥの絶対位置補正フラグが補正パラメータ推定処理部７００に送信される。
次に、補正パラメータ算出部７０１は、補正条件が満たされるか否かを判定する（Ｓ５０）。補正条件は、慣性航法のみによる位置座標の履歴が所定距離以上の任意の２点間を歩行したことを示したこと、かつ、絶対位置情報を用いたパラメトリックな手法で得られた位置座標の補正後の誤差情報がある閾値以下であることである。前者により絶対位置情報の受信により位置が補正されたものでないと判定でき（慣性装置１によって検出される移動が人の歩行や走行によるものであると判定でき）、後者により慣性航法による測位結果又は絶対位置情報になんらかのエラーがあると判定できる。なお、前者と後者の両方を判定することで好ましい状況で補正パラメータを作成できるが、前者と後者の一方のうちどちらのみを判定してもよいし、どちらも判定しなくてもよい。
ステップＳ５０の判定がＹｅｓの場合、補正パラメータ算出部７０１は、今回、補正された最新の位置座標と前回の補正後の位置座標との２点間の座標差（距離）と方向を算出する（Ｓ６０）。
次に、補正パラメータ算出部７０１は慣性航法のみで得られた座標差（距離）と２点間の方向を算出する（Ｓ７０）。慣性航法のみで得られた２点の座標のうちの始点は上記のようにリセットされた値である。
そして、補正パラメータ算出部７０１は、ステップＳ６０の座標差をＳ７０の座標差で除算した結果を、歩行速度補正パラメータに決定し位置推定部８００の速度補正部３５０に通知する（Ｓ８０）。
また、補正パラメータ算出部７０１は、ステップＳ６０の方向とＳ７０の方向の差分をヨー角補正値として姿勢情報変換処理部１００へフィードバックする（Ｓ９０）。以降、ステップＳ１０〜Ｓ９０が繰り返し実行される。
＜測位結果＞
図３６は、５箇所の受信エリアを移動した際に得られた測位結果と補正の様子を示す図の一例である。Ｐ１〜Ｐ５は送信機３４０が送信する絶対位置情報であり、Ｐ´１〜Ｐ´５は絶対位置情報を用いてパラメトリックな手法で補正された位置である。また、位置１００２〜１００５は慣性航法のみにより推定された位置である。ＡＲＥＡ１〜５は送信機３４０からの絶対位置情報を受信可能な範囲である。Ｅ１〜Ｅ５はパラメトリックな手法で位置が補正された際に算出された誤差情報である。
受信エリアＡＲＥＡ２では慣性航法により測位された位置１００２と位置Ｐ２とが大きくずれているが、絶対位置情報により位置Ｐ´２に補正されている。その後、受信エリアＡＲＥＡ３〜５では慣性航法により測位された位置１００３〜１００５と位置Ｐ´３〜５とが大きくずれていないが、各受信エリアで位置１００２〜１００５が補正されていることが分かる。
Figure 0006852310
 表２は、測定機が出力する絶対位置情報と、パラメトリックな手法で補正された位置とを数値で示す。
図３６や表２から分かるように、パラメトリックな手法を適用することで、絶対位置情報を受信した際の誤差情報が加味されて絶対位置が更新される。このため、誤差が限りなく小さい場合にはＰ１〜Ｐ５の絶対位置情報が示す位置座標の値に補正される位置が近くなり、誤差が大きい場合には位置座標を中心として含まれる誤差分だけ外側にオフセットした位置に現在位置が更新される。
これはパラメトリックな手法を適用することで、誤差の大きさを元に推定値を収束させる信頼度（誤差共分散行列）が決定され、補正前後で位置座標の補間がなされるためである。よって、パラメトリックな手法を適用することによって、絶対位置情報を受信した場合に受信した位置座標に一致させる補正する手法に比べ、より正確な位置推定を行うことができる。
仮に、絶対位置情報を受信した場合に受信した位置座標そのものに一致させる補正手法を元に図３５のステップＳ５０〜Ｓ９０が実行された場合、絶対位置情報の誤差が大きい場合でも該ステップが実行されてしまう。このため、歩行速度補正パラメータや、ヨー角補正値の推定精度が低下する。あるいは誤った推定を行ってしまう。
本実施形態では、パラメトリックな手法を適用した上でステップＳ５０〜Ｓ９０を実行することで、より精度の高い歩行速度補正パラメータや、ヨー角補正値の推定が可能である。
また、本実施形態では２点の座標から距離を求めるので、２点間を歩行者がどのように歩行したかによって歩行速度補正パラメータが影響を受けない。特許文献２には、ユーザの絶対位置を取得し所定距離移動したことをトリガとして、歩行テンポと、ユーザの歩幅又は移動速度を示すパラメータを対応させて学習する方法が開示されている。しかしながら、この方法では、絶対位置の２点間を蛇行して繋ぐように歩行した場合、歩数カウント数と歩行テンポを元にしていることから距離関係が崩れ、正しい学習結果を得ることはできない。
以上から、歩行速度補正パラメータやヨー角補正値を推定しフィードバックすることによる補正と、パラメトリックな手法を適用することによる歩行速度補正パラメータやヨー角補正値の推定精度の向上の双方によって、慣性航法における位置の推定精度を高めることができる。
図３７は、従来技術と本実施形態の技術とで得られた歩行者の移動軌跡を比較する図の一例である。図３７（ａ）は従来技術による移動軌跡を示し、図３７（ｂ）は本実施形態の技術による移動軌跡を示している。図３７（ａ）では、方位角のずれと歩行速度の推定誤差が、慣性航法が推定する現在位置に重畳し位置の精度が低下している。図３７（ｂ）では、絶対位置情報により歩行速度パラメータとヨー角が補正されるので、位置の精度が向上している。
従来技術では、現在位置を絶対位置に一致させる補正が行われるが、ヨー角の誤差と歩行速度の推定誤差が慣性航法の際に重畳し位置精度が低下するため、測位精度を継続的に維持することが困難であった。本実施形態の技術では、絶対位置情報を観測したタイミングでヨー角と歩行速度推定パラメータが同時に更新されるため、測位精度を向上させることが可能になる。
＜その他の適用例＞
以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
例えば、本実施形態ではパラメトリックな手法としてカルマンフィルタを用いて位置推定部８００が慣性航法により位置を推定し、また、この位置を絶対位置情報で補正した。しかし、パラメトリックな手法として、パーティクルフィルタ、α−βフィルター等の推定手法を用いてもよい。また、これらが変形又は拡張されたフィルターが使用されてもよい。また、ＬＭＳ（least-mean-square（LMS））や最小最急勾配法などで位置を推定してもよい。
また、図４などの構成例は、慣性装置１による処理の理解を容易にするために、主な機能に応じて分割したものである。処理単位の分割の仕方や名称によって本願発明が制限されることはない。慣性装置１の処理は、処理内容に応じて更に多くの処理単位に分割することもできる。また、１つの処理単位が更に多くの処理を含むように分割することもできる。
また、本実施形態では慣性装置１が全ての処理を行ったが、慣性装置１が加速度、角速度及び地磁気を検出してそれらをサーバに送信してもよい。この場合、サーバは慣性装置１の位置を推定して慣性装置１に送信するので、慣性装置１の処理負荷を低減できる。
なお、図１の位置座標（Ｐ´１）は所定位置の一例であり、第一の観測更新処理部５０２又は第二の観測更新処理部５０３は位置補正部の一例であり、補正パラメータ推定処理部７００は補正パラメータ算出部の一例であり、歩行速度補正パラメータは移動速度補正パラメータの一例であり、座標系変換処理部１０６は進行方向補正部の一例である。測位履歴記憶部７０３は記憶装置の一例であり、移動状態検出部７０２は記憶部の一例である。距離Ａは第１距離の一例であり、距離Ｂは第２距離の一例である。速度補正部３５０は移動速度補正部の一例であり、測位履歴記憶部７０３は位置記録部の一例である。絶対位置情報取得部は絶対位置情報入力部４００の一例である。慣性装置１は自装置の一例であり、位置１００１は推定位置の一例であり、Ｐ´２は補正位置の一例である。
１００ 姿勢情報変換処理部
２００ 進行方向推定部
３００ 進行速度推定部
４００ 絶対位置情報入力部
５００ 絶対位置推定部
７００ 補正パラメータ推定処理部
８００ 位置推定部
特許５０５９９３３号公報 特開２０１３‐０５０３０７号公報
Greg Welch and Gary Bishop、"An Introduction to the Kalman Filter"、Department of Computer Science, University of North Carolina at Chapel Hill、2006年7月24日 片山徹、「新版応用カルマンフィルタ」、朝倉書店、2000年1月20日 I. M. Rekleitis、"A particle filter tutorial for mobile robot localization"、Technical Report TR-CIM-04-02、Centre for Intelligent Machines、McGill University、2004年

Claims (12)

  1. センサ出力を用いて進行方向と移動速度を推定する進行方向推定部と、
    前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定する位置推定部と、
    外部から提供される絶対位置情報を取得する絶対位置情報取得部と、
    前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成する位置補正部と、
    前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正する進行方向補正部と、
    前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出する補正パラメータ算出部と、を有し、
    前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、
    前記絶対位置情報取得部は、前記絶対位置情報が無線で提供されるエリアに慣性装置が進入するごとに前記絶対位置情報を取得し、
    前記補正パラメータ算出部は、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正された前記補正位置との第１距離、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正される前の前記補正位置との第２距離、をそれぞれ算出し、
    前記第１距離に対する前記第２距離の比率を用いて、前記移動速度を補正する移動速度補正パラメータを算出し、
    前記移動速度補正パラメータを用いて前記移動速度を補正する移動速度補正部を有し、
    前記位置推定部は、前記移動速度補正部により補正された前記移動速度で前記推定位置を推定する慣性装置。
  2. 前記補正パラメータ算出部は、前記第１距離に対する前記第２距離の前記比率を算出し、前記移動速度補正部は、前記比率を前記移動速度に乗じることで前記移動速度を補正する請求項１に記載の慣性装置。
  3. 前記補正パラメータ算出部は、前記第１距離に対する前記第２距離の前記比率を算出し、
    前記移動速度補正部は、前記比率に補正値が対応付けられたテーブルから前記補正値を取得し、前記補正値を前記移動速度に乗じることで前記移動速度を補正する請求項１に記載の慣性装置。
  4. 前記位置補正部は、パラメトリックな手法を用いて前記推定位置を補正する際に前記推定位置の誤差情報を推定するものであり、
    前記補正パラメータ算出部は、前記誤差情報の大きさに応じて前記方向補正パラメータの大きさを変化させる請求項１〜３のいずれか１項に記載の慣性装置。
  5. 前記絶対位置情報取得部は、前記絶対位置情報が無線で提供されるエリアに前記自装置が進入するごとに前記絶対位置情報を取得し、
    前記位置推定部が推定した前記推定位置が時系列に記憶された位置記録部を有し、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置から、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正されるまでに前記位置記録部に記録されている前記推定位置に基づく移動量が所定距離以上の場合に、前記補正パラメータ算出部は、前記方向補正パラメータを算出する請求項１〜４のいずれか１項に記載の慣性装置。
  6. 前記位置補正部は、パラメトリックな手法を用いて前記推定位置を補正する際に前記推定位置の誤差情報を推定するものであり、
    前記補正パラメータ算出部は、前記誤差情報が閾値以下である場合に前記方向補正パラメータを算出する請求項１〜５のいずれか１項に記載の慣性装置。
  7. 前記進行方向補正部は、方向余弦行列の前記進行方向を表す方向余弦に前記方向補正パラメータを設定し、前記方向余弦行列を前記センサ出力に乗じることで前記進行方向を補正する請求項１〜６のいずれか１項に記載の慣性装置。
  8. センサ出力を用いて進行方向と移動速度を推定する進行方向推定部と、
    前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定する位置推定部と、
    外部から提供される絶対位置情報を取得する絶対位置情報取得部と、
    前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成する位置補正部と、
    前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正する進行方向補正部と、
    前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出する補正パラメータ算出部と、を有し、
    前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、
    前記位置補正部は、パラメトリックな手法を用いて前記推定位置を補正する際に前記推定位置の誤差情報を推定するものであり、
    前記補正パラメータ算出部は、前記誤差情報の大きさに応じて前記方向補正パラメータの大きさを変化させる慣性装置。
  9. 情報処理装置を、
    センサ出力を用いて進行方向と移動速度を推定する進行方向推定部と、
    前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定する位置推定部と、
    外部から提供される絶対位置情報を取得する絶対位置情報取得部と、
    前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成する位置補正部と、
    前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正する進行方向補正部と、
    前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出する補正パラメータ算出部、として機能させ、
    前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、
    前記絶対位置情報取得部は、前記絶対位置情報が無線で提供されるエリアに慣性装置が進入するごとに前記絶対位置情報を取得し、
    前記補正パラメータ算出部は、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正された前記補正位置との第１距離、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正される前の前記補正位置との第２距離、をそれぞれ算出し、
    前記第１距離に対する前記第２距離の比率を用いて、前記移動速度を補正する移動速度補正パラメータを算出し、
    前記移動速度補正パラメータを用いて前記移動速度を補正する移動速度補正部を有し、
    前記位置推定部は、前記移動速度補正部により補正された前記移動速度で前記推定位置を推定するプログラム。
  10. 情報処理装置を、
    センサ出力を用いて進行方向と移動速度を推定する進行方向推定部と、
    前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定する位置推定部と、
    外部から提供される絶対位置情報を取得する絶対位置情報取得部と、
    前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成する位置補正部と、
    前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正する進行方向補正部と、
    前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出する補正パラメータ算出部、として機能させ、
    前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、
    前記位置補正部は、パラメトリックな手法を用いて前記推定位置を補正する際に前記推定位置の誤差情報を推定するものであり、
    前記補正パラメータ算出部は、前記誤差情報の大きさに応じて前記方向補正パラメータの大きさを変化させるプログラム。
  11. 進行方向推定部が、センサ出力を用いて進行方向と移動速度を推定するステップと、
    位置推定部が、前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定するステップと、
    絶対位置情報取得部が、外部から提供される絶対位置情報を取得するステップと、
    位置補正部が、前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成するステップと、
    進行方向補正部が、前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正するステップと、
    補正パラメータ算出部が、前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出するステップと、を有し、
    前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、
    前記絶対位置情報取得部は、前記絶対位置情報が無線で提供されるエリアに慣性装置が進入するごとに前記絶対位置情報を取得し、
    前記補正パラメータ算出部は、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正された前記補正位置との第１距離、
    前回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部が補正した前記補正位置と、今回の前記エリアで前記絶対位置情報が取得された際に前記位置補正部により補正される前の前記補正位置との第２距離、をそれぞれ算出し、
    前記第１距離に対する前記第２距離の比率を用いて、前記移動速度を補正する移動速度補正パラメータを算出し、
    前記移動速度補正パラメータを用いて前記移動速度を補正する移動速度補正部を有し、
    前記位置推定部は、前記移動速度補正部により補正された前記移動速度で前記推定位置を推定する測位方法。
  12. 進行方向推定部が、センサ出力を用いて進行方向と移動速度を推定するステップと、
    位置推定部が、前記進行方向推定部が推定した前記進行方向と前記移動速度を慣性航法に適用して自装置の推定位置を推定するステップと、
    絶対位置情報取得部が、外部から提供される絶対位置情報を取得するステップと、
    位置補正部が、前記絶対位置情報を取得した場合に、前記推定位置を前記絶対位置情報で補正して補正位置を生成するステップと、
    進行方向補正部が、前記推定位置と前記補正位置のそれぞれから所定位置に対する方向の差に基づいて進行方向を補正するステップと、
    補正パラメータ算出部が、前記推定位置と前記所定位置を結ぶ第１方位と、前記補正位置と前記所定位置を結ぶ第２方位の差に基づいて前記進行方向を補正する方向補正パラメータを算出するステップと、を有し、
    前記進行方向補正部は、前記方向補正パラメータを用いて前記進行方向を補正し、
    前記位置補正部は、パラメトリックな手法を用いて前記推定位置を補正する際に前記推定位置の誤差情報を推定するものであり、
    前記補正パラメータ算出部は、前記誤差情報の大きさに応じて前記方向補正パラメータの大きさを変化させる測位方法。
JP2016166207A 2015-11-30 2016-08-26 慣性装置、プログラム、測位方法 Active JP6852310B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US15/358,893 US10408627B2 (en) 2015-11-30 2016-11-22 Inertial device to estimate position based on corrected movement velocity

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2015233496 2015-11-30
JP2015233496 2015-11-30

Publications (2)

Publication Number Publication Date
JP2017106891A JP2017106891A (ja) 2017-06-15
JP6852310B2 true JP6852310B2 (ja) 2021-03-31

Family

ID=59060649

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2016166207A Active JP6852310B2 (ja) 2015-11-30 2016-08-26 慣性装置、プログラム、測位方法

Country Status (1)

Country Link
JP (1) JP6852310B2 (ja)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2019148586A (ja) * 2018-02-27 2019-09-05 シャープ株式会社 位置計測装置、位置補正方法、及び位置情報取得システム
CN112041635B (zh) 2018-04-03 2024-04-09 三菱电机株式会社 移动机、地图管理装置、定位系统、控制电路及存储介质
US10852143B2 (en) * 2018-06-27 2020-12-01 Rohit Seth Motion sensor with drift correction
CN110793514B (zh) * 2018-08-02 2024-03-01 菜鸟智能物流控股有限公司 位置测量方法和位置测量装置
JP7272910B2 (ja) * 2019-09-03 2023-05-12 株式会社ゼンリンデータコム 車載装置、状態推定方法及びプログラム
EP4194881A1 (en) * 2020-11-13 2023-06-14 Naver Corporation Deep smartphone sensors fusion for indoor positioning and tracking
CN113055598B (zh) * 2021-03-25 2022-08-05 浙江商汤科技开发有限公司 朝向数据补偿方法、装置、电子设备、可读存储介质
JP7292359B2 (ja) * 2021-11-17 2023-06-16 三菱電機株式会社 位置推定装置、位置推定方法及び自動運転システム

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5644317A (en) * 1995-03-27 1997-07-01 Motorola, Inc. Dual positioning location system
JPH11230772A (ja) * 1998-02-12 1999-08-27 Kenwood Corp 車載用ナビゲーション装置
US6594617B2 (en) * 2000-08-18 2003-07-15 Applanix Corporation Pedometer navigator system
JP3984112B2 (ja) * 2002-01-18 2007-10-03 アルパイン株式会社 車両位置修正装置および距離しきい値設定方法
JP4825620B2 (ja) * 2006-08-11 2011-11-30 パイオニア株式会社 ナビゲーション装置、ナビゲーション方法、ナビゲーションプログラムおよびコンピュータに読み取り可能な記録媒体
JP5066367B2 (ja) * 2007-01-11 2012-11-07 株式会社日立製作所 位置検知方法
WO2011128970A1 (ja) * 2010-04-13 2011-10-20 アイチ・マイクロ・インテリジェント株式会社 携帯ナビゲーション装置
JP6171944B2 (ja) * 2014-01-16 2017-08-02 ソニー株式会社 情報処理装置、情報処理方法、及びプログラム

Also Published As

Publication number Publication date
JP2017106891A (ja) 2017-06-15

Similar Documents

Publication Publication Date Title
JP6852310B2 (ja) 慣性装置、プログラム、測位方法
JP6268945B2 (ja) 慣性装置、方法及びプログラム
US10408627B2 (en) Inertial device to estimate position based on corrected movement velocity
JP6322960B2 (ja) 慣性装置、方法及びプログラム
US20210247189A1 (en) Positioning apparatus, positioning method, and program
US8473241B2 (en) Navigation trajectory matching
JP7176563B2 (ja) プログラム、情報処理装置、及び情報処理方法
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
US11041725B2 (en) Systems and methods for estimating the motion of an object
JP7023234B2 (ja) 歩行者の動きを推定する方法
WO2015146046A1 (ja) 相関係数補正方法、運動解析方法、相関係数補正装置及びプログラム
US20150185042A1 (en) Dynamic computation of distance of travel on wearable devices
JP2015190850A (ja) 誤差推定方法、運動解析方法、誤差推定装置及びプログラム
JP2015188605A (ja) 誤差推定方法、運動解析方法、誤差推定装置及びプログラム
Dai et al. Radar update (RUPT): A pedestrian navigation system with enhanced trajectory performance
Kim et al. Comparison of step length estimation models using inertial sensor on pelvis
Anacleto et al. Providing location everywhere
CN110579212B (zh) 室内定位方法及装置
JP2019219187A (ja) 位置計測装置および位置計測方法

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20190621

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20200427

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20200609

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20200807

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20210209

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20210222

R151 Written notification of patent or utility model registration

Ref document number: 6852310

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151