JP6268945B2 - 慣性装置、方法及びプログラム - Google Patents

慣性装置、方法及びプログラム Download PDF

Info

Publication number
JP6268945B2
JP6268945B2 JP2013230548A JP2013230548A JP6268945B2 JP 6268945 B2 JP6268945 B2 JP 6268945B2 JP 2013230548 A JP2013230548 A JP 2013230548A JP 2013230548 A JP2013230548 A JP 2013230548A JP 6268945 B2 JP6268945 B2 JP 6268945B2
Authority
JP
Japan
Prior art keywords
information
unit
inertial
error
inertial device
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
JP2013230548A
Other languages
English (en)
Other versions
JP2014167461A (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 JP2013230548A priority Critical patent/JP6268945B2/ja
Priority to TW103103400A priority patent/TWI490497B/zh
Priority to PCT/JP2014/052706 priority patent/WO2014119801A1/en
Priority to EP14745398.9A priority patent/EP2951529B1/en
Priority to US14/651,273 priority patent/US20150330791A1/en
Priority to CN201480007053.4A priority patent/CN104969030B/zh
Priority to KR1020157020541A priority patent/KR101732835B1/ko
Publication of JP2014167461A publication Critical patent/JP2014167461A/ja
Application granted granted Critical
Publication of JP6268945B2 publication Critical patent/JP6268945B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Description

本発明は、慣性装置、方法及びプログラムに関する。
屋内や地下のように、GPS(Global Positioning System)を用いた測位が行えない環境において、加速度センサや地磁気センサ等が統合された慣性装置の演算により、歩行者の位置を推定する、慣性航法技術（PDR; Pedestrian Dead Reckoning）が知られている。
慣性航法技術を用いた従来の技術には、歩行推定モデルを用いて、一歩ごとに歩幅と進行方向を推定し、歩行に伴う動線を、実時間または過去の時間に遡って算出する技術が存在する（特許文献１、２）。
また、慣性航法技術を実施する従来の慣性装置は、歩行するユーザの体に固定した状態で使用されるか、あるいは、装着後、ユーザにキャリブレーションを実行させることにより、歩行に伴う正しい歩幅推定を可能としている（特許文献３ないし６）。
しかしながら、特許文献１、２に記載の技術は、歩行するユーザの一歩ごとの移動量又は移動方位を補正して位置推定を行うため、歩行のような進行動作に伴う位置推定の分解能は、「一歩」の単位に律速され、それより高い分解能を実現することは難しい。
また、特許文献３ないし６に記載の技術は、ユーザが慣性装置を腰などに固定し、規定の姿勢で使用する必要があるか、あるいは、姿勢を規定する代わりに、ユーザが装着後にキャリブレーションを行う必要がある。従って、従来の慣性装置は、ユーザにとって使い勝手が悪いという問題点があった。
このように、従来の慣性航法技術を用いる慣性装置は、進行動作に伴う位置推定の分解能、すなわち推定精度が低いものであった。
本発明は、このような問題に鑑みてなされたものであり、位置の推定精度を改善し、利便性を向上させる慣性装置を提供することを目的とする。
上述した課題を解決し目的を達成するため、本発明の一実施形態における慣性装置は、
慣性センサの出力を用いて現在位置を推定する慣性装置であって、
前記出力から求めた、当該慣性装置を有する対象となるユーザが歩行又は走行する進行動作の特徴を表すパラメータから、進行速度の変化を表す波形を生成する波形生成部と、
生成された複数の前記波形を合成する波形合成部と、
合成された前記波形から取得した進行速度情報と、前記対象の実際の進行速度に対する誤差を表す前記進行速度情報の誤差情報とを用いて現在位置を推定する推定部と、
を有する。
また、本発明の一実施形態における方法は、
慣性センサの出力を用いて現在位置をコンピュータが推定する方法であって、
コンピュータが、前記出力から求めた、慣性装置を有する対象となるユーザが歩行又は走行する進行動作の特徴を表すパラメータから、進行速度の変化を表す波形を生成する波形生成段階と、
コンピュータが、生成された複数の前記波形を合成する波形合成段階と、
コンピュータが、合成された前記波形から取得した進行速度情報と、前記対象の実際の進行速度に対する誤差を表す前記進行速度情報の誤差情報とを用いて現在位置を推定する推定段階と、
を有する。
また、本発明の一実施形態におけるプログラムは、コンピュータに、上記方法を実行させる。
本発明によれば、位置の推定精度を改善し、利便性を向上させる慣性装置を提供できる。
本発明の一実施形態における慣性装置の概略を表す図。 本発明の一実施形態における慣性装置のハードウェア構成例を表す図。 本発明の一実施形態における慣性装置の機能ブロックの一部を表す図。 姿勢演算処理を説明する図。 時間発展の手続きにおける姿勢演算（ロール・ピッチ・ヨー角）を説明する図。 時間発展の手続きにおける姿勢演算（ロール・ピッチ・ヨー角）を説明する図。 時間発展の手続きにおける姿勢演算（ロール・ピッチ・ヨー角）を説明する図。 第一の観測更新の手続きにおける姿勢演算（ロール・ピッチ角）を説明する図。 第二の観測更新の手続きにおける姿勢演算（ヨー角）を説明する図。 ピーク検出処理を説明する図。 水平成分移動速度特徴情報の算出処理を説明する図。 水平成分移動速度特徴情報の算出処理を説明する図。 歩行判定処理を説明する図。 左右の移動速度ぶれ幅判定処理を説明する図。 進行方向推定処理を説明する図。 ＴＲＩＡＤアルゴリズムにより姿勢を求める処理を説明する図。 本発明の一実施形態における慣性装置の機能ブロック全体を表す図。 本発明の一実施形態における慣性装置の進行速度推定部の機能ブロック図。 パラメータデータベースの一例を表す図。 進行速度波形の例を表す図。 合成された進行速度波形の例を表す図。 本発明の一実施形態における慣性装置の絶対位置情報入力部、絶対位置推定部及びマップマッチング部の機能ブロック図。 電波強度と測位時間間隔の関係を表す図。 時間発展の手続きにおける現在位置の演算を説明する図。 時間発展の手続きにおける現在位置の演算を説明する図。 時間発展の手続きにおける現在位置の演算を説明する図。 観測更新の手続きにおける現在位置の演算を説明する図。 拡張カルマンフィルタの一般式を説明する図（従来技術）。 時間更新の手続きにおいて用いられる変数を説明する図（従来技術）。 観測更新の手続きにおいて用いられる変数を説明する図（従来技術）。 歩行動作における鉛直方向の運動特性を説明する図。 歩行動作における水平方向の運動特性を説明する図。 歩行時における歩行速度の推定結果を表す図。 歩行時における現在位置の推定結果を表す図。 歩行時における現在位置の推定結果を表す図。 推定位置が他の位置情報により補正される様子を表す図。 歩行時における測位誤差の推定値の変化を表す図。
以下、本発明の実施形態を図面に基づいて説明する。
１． 概要
２． ハードウェア構成
３． 進行方向推定機能
３．１ 座標系変換
拡張カルマンフィルタの一般式
拡張カルマンフィルタの適用
時間発展の手続き
第一の観測更新の手続き
第二の観測項新の手続き
ＴＲＩＡＤアルゴリズムを用いた姿勢情報の計算
３．２ 進行方向推定
４． 現在位置推定機能
４．１ 進行速度推定
４．２ 絶対位置情報入力
４．３ 絶対位置推定
４．４ マップマッチング
５． 進行速度及び現在位置の推定結果
≪ １． 概要 ≫
まず、図１を用いて、本発明の一実施形態における慣性装置の概要を説明する。図１は、慣性装置１を所持するユーザが歩行する様子を表している。本明細書では、ユーザの歩行する方向がｘ軸で表され、ｘ軸と垂直であり、かつ、地面に平行な方向がｙ軸で表され、鉛直方向がｚ軸で表される。
慣性装置１は、例えば、携帯電話、スマートフォン、ＰＤＡ（Personal Digital Assistant）又はノートブックＰＣのように、ユーザが持ち運ぶことのできる小型のデバイスである。慣性装置１は、市販されているスマートフォン等に搭載されているような、慣性センサ（加速度センサ、角速度センサ、地磁気センサ）を備えている。慣性装置１は、各センサを利用して、加速度及び角速度の変化並びに装置の向きを検出することができる。
詳しくは後述するが、本発明の一実施形態における慣性装置１は、まず、各センサを利用して、三つの軸方向の加速度（三軸加速度）と、三つの軸方向の角速度（三軸角速度）と、三つの軸で表される地磁気の強さ（三軸地磁気）とを含むセンサ情報を随時取得する。各測定値は、デバイス座標系によって表現されるため、慣性装置１は、各センサ情報を、絶対座標系に変換する。
慣性装置１は、絶対座標空間における移動加速度（ベクトル）を取得する。具体的には、例えば、慣性装置１は、絶対座標系に変換されたセンサ情報から重力成分を除去した、鉛直方向成分と水平成分の加速度（三軸移動加速度）を取得することができる。そして、慣性装置１は、随時取得されて記憶される三軸移動加速度のうち、鉛直方向の移動加速度の変化を表す波形において、歩行中のユーザの一の足（軸足）が着地してから、他方の足が軸足を追い越す時刻を特定する。当該時刻は、鉛直方向の移動加速度の変化を表す波形が、谷のピークとなる時刻に対応ため、本明細書では、当該時刻のことを、ピーク時刻又はピーク位置と呼ぶ。
次に、慣性装置１は、ピーク位置を中心とした所定の期間内の水平成分移動加速度の積算処理を実行する。ここで、算出された値を、水平成分移動速度特徴ベクトルと呼ぶ。慣性装置１は、算出された水平成分移動速度特徴ベクトルと、測定対象の移動に係る動作の周期と、鉛直方向移動加速度のピーク値（鉛直方向ピーク加速度）を用いて、得られたデータが、測定対象による実際の歩行又は進行動作によるものであるか判定する。以下、測定対象の移動に係る動作の周期のことを、移動周期と呼ぶ。移動周期の一例は、人の歩行周期である。また、測定対象による実際の歩行又は進行動作を、単に歩行動作と記述する（但し、「歩行」以外の動作も含む）。
次に、慣性装置１は、歩行動作によるものであると判定した場合には、前後の水平成分移動速度特徴ベクトルを合成し、一歩踏み出したときの進行方向ベクトルを、随時算出する。その後、最新の進行方向ベクトルと、直近の進行方向ベクトルの合成方向を、進行方向として推定する。
慣性装置１は、さらに、推定された進行方向や、既に取得済みの水平成分移動速度特徴ベクトル等の情報等をもとに、その時点における測定対象の進行速度の特性を表す進行速度波形（時間に対する水平方向の速度成分の変化）を生成する（図２０）。詳しくは後述する。そして、逐次得られた進行速度波形を合成し（図２１、詳しくは後述する）、実際の進行速度を表す速度推定ベクトルを得る。
慣性装置１は、このようにして得た速度推定ベクトルと、別途取得した絶対位置座標の情報をもとに、現在位置を推測することができる。
このように、本発明の一実施形態における慣性装置１は、測定対象の移動によってセンサから逐次得られる情報を用いて、測定対象の移動速度を推定する。そして、これと絶対位置座標を表す情報を組み合わせることにより、一歩未満の分解能で、進行方向と現在位置を推定することができる。また、慣性装置１は、測定対象の本来の移動の動作ではない動作を感知すると、この動作が、進行に係る動作でない（例えば、ユーザによる、歩行とは関係ない、手を振る動作等）ものと判定し、得られたデータを方向推定に利用しない。これによって、例えば、測定対象であるユーザは、慣性装置１を任意の姿勢で保持することができる。この利点は、本慣性装置が、ユーザに対してナビゲーション機能を提供する場合に、特に有利である。
なお、慣性装置１は、上で例示したデバイス以外のデバイス（例えば、音楽プレーヤ、活動量計、腕時計等）であってもよい。また、慣性装置１は、他のデバイス（例えば、歩行ロボットや、動物に装着する首輪等）に内蔵されて提供されてもよい。これにより、鉛直方向に一定の周期で運動する多様な人又は物体の進行方向を推定することが可能となる。
≪ ２． ハードウェア構成 ≫
次に、図２を用いて、本発明の一実施形態における慣性装置１のハードウェア構成を説明する。図２は、慣性装置１が、スマートフォンのような携帯デバイスである場合の、ハードウェア構成例である。
慣性装置１は、ＣＰＵ１１、ＲＡＭ１２、ＲＯＭ１３、加速度センサ１４、角速度センサ１５、地磁気センサ１６、マイク１７、スピーカ１８、通信モジュール１９、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０、ＧＰＳ受信モジュール２１、ディスプレイ２２、タッチパネル２３、電池２４、気圧センサ２５及びバス２６を有する。
ＣＰＵ１１は、慣性装置１の動作制御を行うプログラムを実行する。ＲＡＭ１２は、ＣＰＵ１１のワークエリア等を構成する。ＲＯＭ１３は、ＣＰＵ１１が実行するプログラムや、プログラムの実行に必要なデータを記憶する。加速度センサ１４は、慣性装置１の、デバイス座標系におけるX'、Y'、Z'軸方向の加速度を検出する。角速度センサ１５（又はジャイロセンサ）は、慣性装置１の、デバイス座標系におけるX'、Y'、Z'軸方向の角速度を検出する。地磁気センサ１６は、磁北を表す三次元のベクトルを出力し、慣性装置１の向きを検出する。気圧センサ２５は、気圧を測定し、当該慣性装置１の高度を検出する。
マイク１７は、ユーザの声等の音声を電気信号に変換する。スピーカ１８は、電気信号を音声として出力する。通信モジュール１９は、３Ｇネットワーク及び／又は無線ＬＡＮに接続された他の装置と通信するための装置である。Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０は、Ｂｌｕｅｔｏｏｔｈを用いて通信するための装置である。ＧＰＳ受信モジュール２１は、ＧＰＳ衛星やＩＭＥＳ（Indoor Messaging Service）によって送信される測位信号を受信するための装置である。
ディスプレイ２２は、ユーザに対して画面を提示するための装置である。タッチパネル２３は、ユーザからの入力を受け付ける装置である。電池２４は、慣性装置１を駆動するための電力を供給する装置である。バス２６は、電池２４を除く各装置を相互に接続する。
なお、マイク１７、スピーカ１８、通信モジュール１９、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０、ＧＰＳ受信モジュール２１、ディスプレイ２２及びタッチパネル２３は、任意の構成要素である。例えば、慣性装置１が、表示画面を有しない活動量計のようなデバイスである場合には、これらの装置を備えていなくてもよい。
また、慣性装置１は、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０の代わりに、他の規格に従って無線通信する装置（例えば、ＺｉｇＢｅｅ（登録商標）通信モジュール）を備えていてもよい。
≪ ３．進行方向推定機能 ≫
次に、図３を用いて、本発明の一実施形態における慣性装置１の機能ブロックを説明する。本節では、慣性装置１が、測定対象の進行方向を推定する進行方向推定機能を、「座標系変換」機能と、「進行方向推定」機能の、二つに分けて説明する。
≪ ３．１ 座標系変換 ≫
慣性装置１において座標系変換機能を実行する座標系変換部１００は、加速度取得部１０１と、角速度取得部１０２と、地磁気取得部１０３と、姿勢演算部１０４と、地磁気信頼性評価部１０５と、絶対加速度変換部１０７とを含む。座標系変換部１００は、加速度センサ１４より取得された、センサの座標系（デバイス座標系）における三軸の加速度を、絶対座標系における加速度（絶対加速度）に変換する。
なお、絶対座標系は、複数種類のセンサによって観測された座標値を統一的に扱うために使用される統一的な座標系であり、例えば、ＧＰＳで使用されるＷＧＳ８４経緯度座標系や、ＵＴＭ（ユニバーサル横メルカトル図法）など直交座標系がある。絶対座標系は、ワールド（世界）座標系とも呼ばれる。また、デバイス座標系は、ボディー座標系とも呼ばれ、慣性装置１上の１点を原点として定義し、この原点で互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系のことを表す。
加速度取得部１０１は、加速度センサ１４によって検出された三軸加速度の変化を取得する。
角速度取得部１０２は、角速度センサ１５によって検出された三軸角速度の変化を取得する。ここで取得される角速度は、加速度と同様に、デバイス座標系に固定されている。
地磁気取得部１０３は、地磁気センサ１６によって検出された磁北を表す三次元の地磁気ベクトルを取得し、慣性装置１の向きを表す情報を取得する。ここで取得される向きは、加速度と同様に、デバイス座標系に固定されている。
姿勢演算部１０４は、上述した、加速度取得部１０１、角速度取得部１０２及び地磁気取得部１０３が取得したセンサ情報を用いて、慣性装置１の現在の姿勢を演算し、その姿勢情報（回転行列）から、逆行列計算により、逆回転行列１０６を求める。
ここで、地磁気取得部１０３の取得した情報は、屋内環境における環境磁場の影響により、信頼性が低い場合がある。そこで、姿勢演算部１０４は、後述する、地磁気信頼性評価部１０５により信頼できると判断された場合にのみ、地磁気取得部１０３の取得した情報を使用する。
姿勢演算部１０４は、一般的に知られている拡張カルマンフィルタ（非特許文献１、非特許文献２、図２８−３０）を利用して、慣性装置１の姿勢を表す行列を求め、その逆行列を出力する。以下、その処理について詳細に説明する。
≪ 拡張カルマンフィルタの一般式 ≫
図２８は、一般的に知られている拡張カルマンフィルタの一般式を表す。カルマンフィルタでは、時間ステップをひとつ進めるために、時間発展と観測更新の二つの手続きが実行される。時間発展の手続きでは、前の時刻の推定状態から、現在時刻の推定状態が計算される。また、観測更新の手続では、現在時刻の観測を用い、推定値を補正して、より正確な状態が推定される。これらの手続を逐次繰り返すことによって、最適な状態変数が推定される。
図２９は、拡張カルマンフィルタの時間発展の手続きにおいて使用される各変数を説明する図である。図２８の時間発展の項目に含まれる（１）−（３）の式に対応して、各変数の説明が付されている。ここで、ｋは、離散的なステップ時間を表しており、ｋ−１は、今の時間より１ステップ前の時間を表す。
図３０は、拡張カルマンフィルタの観測更新の手続きにおいて使用される各変数を説明する図である。図２８の観測更新の項目に含まれる（１）−（６）の式に対応して、各変数の説明が付されている。
≪ 拡張カルマンフィルタの適用 ≫
姿勢演算部１０４は、上述した拡張カルマンフィルタにおける「時間発展の手続き」を、角速度センサ１５の出力による姿勢情報の更新に用いる（ロール角、ピッチ角、ヨー角）。また、拡張カルマンフィルタにおける「観測更新の手続き」を、加速度センサ１４の出力による姿勢情報の更新に用いる（ロール角、ピッチ角）（以後、「第一の観測更新の手続き」と呼ぶ）。また、「観測更新の手続き」を、さらに、地磁気センサ１６の出力による姿勢情報の更新にも用いる（ヨー角）（以後、「第二の観測更新の手続き」と呼ぶ）。
このように、姿勢演算部１０４は、７状態の拡張カルマンフィルタを構築する。姿勢演算部１０４は、一つの時間発展の手続きと、二つの観測更新の手続きを、並列にループさせ、姿勢とジャイロゼロ点バイアス値の推定を行う。ここで、姿勢は、以下のようなクオータニオン（ベクトル）によって表される。
クオータニオンベクトルとは、４変数からなるベクトルであり、物体の姿勢を表す。ロール・ピッチ・ヨーの姿勢表現ではシンバルロックという特異点問題がある反面、クオータニオンでは、特異点が無く全ての姿勢が表すことができる。また、ジャイロゼロ点バイアス値は、三軸に対応する三変数（ｂｘk,ｂｙk,ｂｚk）によって表される（ｂは定数）。
以下では、上記の三つの手続き（１）−（３）を、それぞれ説明する。
≪ 時間発展の手続き ≫
まず、図５−図７を用いて、拡張カルマンフィルタにおける時間発展の手続きを説明する。姿勢演算部１０４は、当該手続きを実行し、後述する状態推定モデルにおいて、ジャイロ出力値を入力として、拡張カルマンフィルタの時間発展の手続きに従って時間積分を行う。そして、更新されたクォータニオンベクトルｑと、誤差共分散行列Ｐを得る（ロール・ピッチ・ヨー角）。
図５は、本発明において、拡張カルマンフィルタの一般式における、（１）システムの状態推定モデルに含まれる項目を説明する図である。ここでは、現在時刻の状態推定値を、上述したクォータニオンベクトル及びジャイロゼロ点バイアス値により、図５の式（１）−１のように定義する。
また、入力量ｕkを、角速度センサの出力値（ω0xk,ω0yk,ω0zk）（rad/sec）を用いて、図５の式（１）−４のように定義する（ｂは定数）。
すなわち、（ωxk,ωyk,ωzk）は、ゼロ点の値を減算し、オフセットがない角速度を表す（rad/sec）。そして、システムの状態推定モデルを、図５の式（１）−５のように表す。ここで、Ｃ1、Ｃ2、Ｃ3は、任意の定数である。
図６は、本発明において、拡張カルマンフィルタの一般式における、（２）時間更新における偏微分行列（ヤコビアン）を説明する図である。図５を用いて説明したように、システムの状態推定モデルは、図５の式（１）−５によって表される。式（１）−５の右辺はｆであるため、右辺の偏微分が、時間発展における偏微分行列となる。
図７は、本発明において、拡張カルマンフィルタの一般式における、（３）誤差の共分散行列推定モデルＰk|k-1を説明する図である。プロセスノイズＱkは、システム同定によって求められる定数であり、予め算出される。
上記のプロセスノイズＱkと、１ステップ前の時刻の誤差共分散行列と、時間発展における偏微分行列（ヤコビアン）Ｆk及びその転置行列Ｆk Tにより、現在時刻の誤差誤差共分散行列Ｐk|k-1が求まる（図７の式（３）−５）。なお、現在時刻の誤差誤差共分散行列Ｐk|k-1及びＰk-1|k-1は、７×７の行列となり、行列内の要素は実数となる。
姿勢演算部１０４は、上記のモデル及び変数の定義を用いて、拡張カルマンフィルタにおける時間発展の手続きを実行し、慣性装置１の、絶対座標系における姿勢を求め、その姿勢を表す行列の逆行列（逆回転行列）を求める。
≪ 第一の観測更新の手続き ≫
次に、図８を用いて、拡張カルマンフィルタにおける第一の観測更新の手続きを説明する。姿勢演算部１０４は、当該手続きを実行し、加速度取得部１０１から得られる水平方向の角度情報と、現在のクォータニオンベクトルの水平角度情報とを比較し、その誤差分を補正する処理を行う（ロール・ピッチ角のみ）。
図８は、拡張カルマンフィルタの一般式における、
（１）観測残差
に含まれる項目を説明する図である。まず、１ステップ前の時間の観測値（ベクトル）ｈは、図８の式（１）−３のように表される。
上記の式に含まれる要素は、三次元の回転行列（４×４）に由来するものであり、予め決められているものとする。また、観測値（ベクトル）ｚkは、図８の式（１）−２のように表される。
ここで、（ａx，ａy，ａz）は、加速度取得部１０１が取得した、加速度センサ１４の出力値である。上記のｈ及びｚkにより、
観測残差
が求まる。
また、拡張カルマンフィルタの一般式における、（２）観測更新における偏微分行列（ヤコビアン）Ｈkは、図７の式（１）−３に表される観測値ｈの偏微分を求めることによって求まる。
また、拡張カルマンフィルタの一般式における、（３）残差の共分散Ｓkは、以下の観測ノイズ（行列）Ｒkと、観測更新における偏微分行列Ｈk、その転置行列Ｈk T及び現在時刻の誤差共分散行列Ｐk|k-1を用いて求められる。
ここで、（ｒ1，ｒ2，ｒ3）は、加速度センサ１４のデバイス評価の結果、予め求められた分散値である。
また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫkは、現在時刻の誤差共分散行列Ｐk|k-1と、観測更新における偏微分行列の転置行列Ｈk Tと、残差の共分散の逆行列Ｓk -1から求まる。なお、Ｋkは、７×３の行列であり、実数値を要素に持つ。
同様に、拡張カルマンフィルタの一般式における、（５）更新された状態推定値ｘk|k及び（６）更新された誤差共分散行列Ｐk|kは、これまでに求められた変数を用いて求めることができる。
姿勢演算部１０４は、上記のモデル及び変数を用いて、拡張カルマンフィルタにおける観測更新の手続きを実行し、水平方向の角度情報と、現在のクォータニオンベクトルの水平角度情報とを比較し、その誤差分を補正する（ロール・ピッチ角のみ）。
≪ 第二の観測項新の手続き ≫
次に、図９を用いて、拡張カルマンフィルタにおける第二の観測更新の手続きを説明する。姿勢演算部１０４は、後述する地磁気信頼性評価部１０５から、地磁気取得部１０３の取得した情報が信頼できるものであるかについての通知を受け取る。そして、地磁気取得部１０３からの情報が信頼できる場合に、ＴＲＩＡＤアルゴリズムに従って取得された姿勢情報から算出されるヨー角の情報を用いて、当該手続きを行い、クオータニオンベクトルのヨー角成分の補正を行う。ＴＲＩＡＤアルゴリズムを用いた演算方法については後述する。
図９は、図８と同様に、拡張カルマンフィルタの一般式における、
（１）観測残差
に含まれる項目を説明する図である。図８の場合と同様に、１ステップ前の時間の観測値（ベクトル）ｈは、図９の式（１）−３のように表される。一方、図８の場合と異なり、観測値（ベクトル）ｚkは、図９の式（１）−２のように表される。
上記のベクトルは、ＴＲＩＡＤアルゴリズムによって求められた、ヨー角方向ベクトルである。
また、拡張カルマンフィルタの一般式における、（２）観測更新における偏微分行列（ヤコビアン）Ｈkは、第一の観測更新の手続きと同様に、１ステップ前の時間の観測地ｈの偏微分を求めることによって、求めることができる。
また、拡張カルマンフィルタの一般式における、（３）残差の共分散Ｓkは、以下の観測ノイズ（行列）Ｒkと、観測更新における偏微分行列Ｈk、その転置行列Ｈk T及び現在時刻の誤差共分散行列Ｐk|k-1を用いて求められる。
ここで、（Ｔ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の逆行列（絶対座標系からデバイス座標系への変換のための行列）を求め、これをクオータニオンに変換する。このようにして得られたクオータニオンが、第二の姿勢情報である。
地磁気信頼性評価部１０５は、第一の姿勢情報と、上述した方法によって求められた第二の姿勢情報の、それぞれの微分値を比較し、それぞれが同じ変化をしているか判定する。地磁気信頼性評価部１０５は、その変化が同じ（すなわち、微分値の差が所定の範囲内）であれば、地磁気センサ１６から取得された地磁気は信頼できるものと判定する。
また、地磁気信頼性評価部１０５は、以下の判断基準を用いて、地磁気ベクトルの信頼性を評価することにより、その評価の精度を高めることができる。
−地磁気ベクトルの絶対値が規定の範囲内か
−第一の姿勢情報と地磁気ベクトルから得られる伏角値が規定の範囲内か
−第二の姿勢情報と地磁気ベクトルから得られる伏角値が規定の範囲内か
ここで、規定の範囲は、国土地理院が発行している、伏角情報と、地磁気ベクトルの大きさの情報を用いて、設定することができる。
そして、地磁気が信頼できるとき、第二の姿勢情報から算出されるヨー角を用いて、第二の観測更新の手続きが実行される。一方で、地磁気が信頼できない場合には、第二の観測更新の手続きは実行されない。
絶対加速度変換部１０７は、加速度取得部１０１が取得した加速度に、姿勢演算部１０４によって求められた逆回転行列１０６を乗算し、絶対座標系での三軸加速度（絶対加速度）を算出する。
≪ ３．２ 進行方向推定 ≫
慣性装置１において進行方向推定機能を実行する進行方向推定部２００は、バンドパスフィルタ２０１と、ピーク検出部２０４と、ピーク位置記憶部２０５と、移動加速度記憶部２０６と、水平成分移動速度特徴情報管理部２０７と、鉛直成分ピーク移動加速度取得部２０８と、水平成分移動速度特徴情報取得部２０９と、周期取得部２１０と、判定部２１１と、進行方向算出部２１２とを含む。進行方向推定部２００は、座標系変換部１００によって得られた、絶対座標系における加速度から、ユーザの一歩ごとの進行方向を求める。
バンドパスフィルタ２０１は、座標系変換部１００より出力された三軸絶対加速度から、重力成分を除去する。通過帯域は、例えば、一般的な歩行周波数である１−３Ｈｚ程度とする。なお、通過帯域は、慣性装置１の測定対象の歩行又は移動に係る周波数に応じて、適宜変更され得る。ここでは、バンドパスフィルタ２０１が出力する、重力成分が除去された絶対加速度を、移動加速度２０２と呼ぶ。移動加速度２０２は、後述する、移動加速度記憶部２０６に記憶される。また、移動加速度の鉛直成分である、鉛直成分移動加速度２０３は、後述する、ピーク検出部２０４に渡される。
ピーク検出部２０４は、バンドパスフィルタ２０１から出力される移動加速度２０２のうち、鉛直成分移動加速度２０３の変化（時間変化）を観測し、その波形の谷ピークの位置（ピーク時刻又はピーク位置）を検出する。検出されたピーク位置は、後述する、ピーク位置記憶部２０５に記憶される。以下では、谷ピークの検出方法について説明する。
図１０は、横軸を時間とした、鉛直成分移動加速度２０３（Ｚ）と、移動加速度の水平成分（Ｘ、Ｙ）の変化を表す波形である。このように、それぞれの波形は、移動周期（例えば、歩行周期）と整合する周期を持った波形となり、特に、鉛直成分移動加速度２０３は、±１ｍ／ｓ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）の合計値が最も小さいものを選択することができる。類似するエントリを検出するために、任意の方法を用いることができる。進行速度波形生成部３１２は、取得した速度生成係数Ｃａ−Ｃｃを、進行速度波形生成部３１２に渡す。
進行速度波形生成部３１２は、速度生成係数Ｃａ−Ｃｃと、入力された各パラメータＰａ−Ｐｅ３０７−３１１とを用いて、例えば、以下のような式により、時刻０≦ｔ≦Ｐｃにおける、測定対象の速度変化を表す進行速度波形を生成する。
なお、進行速度波形生成部３１２は、上記の式以外の任意の式を用いて、進行速度波形を生成しても良い。
生成された進行速度波形の例を図２０に示す。ここで、θは、進行方向パラメータＰｅに対応する。数１１によって示される式は、速度パラメータ３０７による推定項(第１項)と、強度パラメータ３０８での推定項(第２項)と、ぶれ幅パラメータ３１０での推定項(第３項)を含む。各項は、それぞれの重みとして、速度生成係数Ｃａ−Ｃｃが乗算されており、複数のパラメータに基づく速度推定を行うことにより、精度の高い歩行速度波形を生成することが可能である。なお、速度生成係数Ｃａ−Ｃｃは、和が１となるように、予め正規化されて、パラメータＤＢ３１３に格納されている。進行速度波形生成部３１２は、生成した進行速度波形を、進行速度波形合成部３１４に渡す。
進行速度波形合成部３１４は、進行速度波形生成部３１２から進行速度波形を取得すると、後述する、進行速度波形記憶部３１５に記憶された、過去に合成された進行速度波形を取得し、これらを合成する。このとき、進行速度波形合成部３１４は、対応する時間位置において、これらの進行速度波形を積算することにより合成する。進行速度波形合成部３１４は、進行速度波形を任意の方法で合成してもよい（例えば、複数の進行速度波形が重なった場合には各々を比較しその最大値を採用する、等）。図２１は、進行速度波形合成部３１４が、複数の進行速度波形を積算して合成した例を表している。進行速度波形合成部３１４は、合成した進行速度波形を、進行速度波形記憶部３１５に記憶する。
進行速度波形記憶部３１５は、進行速度波形合成部３１４により合成された進行速度波形を、時刻情報とともに記憶する。
このようにして、随時取得されるパラメータに基づいて合成された最新の進行速度波形が、進行速度波形記憶部３１５に記憶される。後述する絶対位置推定部５００及びマップマッチング部６００は、現時点における、合成された当該進行速度波形の値を参照することにより、最新の進行速度を表す進行速度情報を取得することができる。進行速度情報は、上述したように、水平方向の二成分によって表されるため、以下では、この進行速度情報を、進行速度推定ベクトル３１６と呼ぶ。
≪４．２ 絶対位置情報入力≫
図２２は、絶対位置情報入力部４００、絶対位置推定部５００及びマップマッチング部６００のそれぞれの内部の詳細な機能ブロック図を表す。
絶対位置情報入力部４００は、第一の絶対位置取得部４０１と、第二の絶対位置取得部４０２と、測位時間計測部４０３と、誤差補正部４０４とを含む。絶対位置情報入力部４００は、慣性装置１の絶対位置を表す情報と、その情報に含まれる誤差の大きさを表す誤差情報とを、絶対位置推定部５００に入力する。
第一の絶対位置取得部４０１は、例えば、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により、
観測残差
が求まる。
また、拡張カルマンフィルタの一般式における（２）観測更新における偏微分行列（ヤコビアン）Ｈkは、図２７の式（１）−３に表される観測値ｈの偏微分を求めることによって求まる。
また、拡張カルマンフィルタの一般式における（３）残差の共分散Ｓkは、第一の絶対位置取得手段４０１の出力した誤差情報である観測ノイズ（行列）Ｒkと、観測更新における偏微分行列Ｈk、その転置行列Ｈk T及び現在時刻の誤差共分散行列Ｐk|k-1を用いて求まる。
なお、ｒ1はＸ軸、ｒ2はＹ軸、ｒ3はＺ軸の成分を表す。
また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫkは、現在時刻の誤差共分散行列Ｐk|k-1と、観測更新における偏微分行列の転置行列Ｈk Tと、残差の共分散の逆行列Ｓk -1から求まる。
同様に、拡張カルマンフィルタの一般式における、（５）更新された状態推定値ｘk|k及び（６）更新された誤差共分散行列Ｐk|kは、これまでに求められた変数を用いて求めることができる。
第二の観測更新処理部５０３は、第一の観測更新処理部５０２と同様に、拡張カルマンフィルタの観測更新の手続きを実行し、慣性装置１の現在位置と誤差情報５０５を算出又は更新する。拡張カルマンフィルタにおける変数は、観測値ｚk及び観測ノイズＲkが、第二の絶対位置取得部４０２によって出力される位置ベクトル及び誤差情報である点を除いて、 第一の観測更新処理部５０２と同様である。
第三の観測更新処理部５０４は、第一の観測更新処理部５０２と同様に、拡張カルマンフィルタの観測更新の手続きを実行し、慣性装置１の現在位置と誤差情報５０５を算出又は更新する。拡張カルマンフィルタにおける変数は、観測値ｚk及び観測ノイズＲkが、後述する、マップマッチング部６００によって出力される位置ベクトル及び誤差情報である点を除いて、 第一の観測更新処理部５０２と同様である。
絶対位置推定部５００は、以上の通り、拡張カルマンフィルタの枠組みにおいて、現在位置と誤差情報５０５を逐次更新することで、現在位置を高精度に推定することができる。慣性装置１に搭載された外部のアプリケーションは、現在位置と誤差情報５０５を参照することにより、現在位置の推定位置だけでなく、慣性装置の向き（ヘディング情報、ヨー角推定値）を取得することができる。
≪４．４ マップマッチング≫
マップマッチング部６００は、マップマッチング処理部６０１を含み、最新の現在位置と誤差情報５０５と、進行速度推定ベクトル３１６を取得し、マップマッチング処理を実行する。
マップマッチング処理部６０１は、最新の現在位置と誤差情報５０５と、進行速度推定ベクトル３１６を取得し、予め用意された地図データベース６０２を参照して、進行可能な領域を表す領域情報を取得する。次に、マップマッチング処理部６０１は、既知の技術であるパーティクルフィルタのアルゴリズムを適用し（例えば、非特許文献６）、マップマッチング処理を実行する。マップマッチング処理部６０１は、現在位置が、マップ上で進行が不可能な場所を示していた場合には、進行可能な領域に現在位置を修正する。また、マップマッチング処理部６０１は、内部処理により、修正された位置を表す位置ベクトルの各成分の誤差量を表す誤差共分散行列σmを算出する。マップマッチング処理部６０１は、修正された現在位置を表す位置ベクトル（Ｘm、Ｙm、Ｚm）及び誤差共分散行列σmを、上述したように、絶対位置推定部５００の第三の観測更新処理部５０４に渡す。
以上のように、本発明の一実施形態における慣性装置１は、進行動作を元に取得された、進行動作の特徴を示す複数のパラメータから、進行速度波形を生成して随時合成し、その波形に基づいて進行速度を算出する。複数のパラメータは、測定対象の特徴と関連付けられてデータベース化されており、進行速度波形は、測定対象とその動作に適するように生成される。また、歩行速度波形を表す式は、複数の項（モデル）によって表現され、各モデルの信頼度となる重みを乗算する。従って、分解能が高く、かつ信頼度の高い、進行速度の推定が可能となる。
また、慣性装置１は、相対値である進行速度及びその誤差情報を、絶対位置情報及びその誤差情報と融合させることにより、現在位置を高精度に求めることができる。また、慣性装置１は、地図データを用いたマップマッチングにより上記現在位置を補正することにより、さらに信頼度の高い現在位置を求めることができる。
≪ ５． 進行速度及び現在位置の推定結果 ≫
以下、図３３−図３７を参照し、本発明の一実施形態における慣性装置１を用いて、進行速度及び現在位置の推定を行った場合に得られた結果を説明する。
まず、本実施形態における慣性装置１は、慣性センサとして、加速度センサ、ジャイロセンサ及び地磁気センサを有し、位置情報を受信する手段（測位手段）として、ＧＰＳ／ＩＭＥＳモジュールおよびＢｌｕｅｔｏｏｔｈ（登録商標）モジュールを有する。慣性装置１は、上記装置を有する、スマートフォン型の携帯端末である。
図３３は、人が慣性装置１を腹部に装着して歩行した場合に得られた、進行速度の推定結果を表す図である。ここで、人は、図３４において破線の矢印で示されるように、約２ｍ直進した後に、９０度左折し、さらに直進する歩行動作を行っている。図３３の横軸は、経過時間（１００ミリ秒）を表し、縦軸は、１００ミリ秒ごとの進行速度（メートル毎秒）を示している。図中に示されている時間領域は、一連の歩行動作中の、約８秒の時間窓について結果を抽出したものである。慣性装置１を所持した人は、この時間窓内で、合計７歩の歩行を実施している。
図３３は、進行方向推定部２００が取得した、一歩ごとの進行方向、周期、ブレ幅、等のパラメータを、進行速度波形生成部３１２が取得し、その後、進行速度波形合成部３１４が合成した進行速度波形を示す。図からわかるように、慣性装置１を用いることにより、複数歩の歩行に伴う進行速度が、１００ミリ秒という高い時間分解能で出力される。
図３４は、人が慣性装置１を腹部に装着して歩行した場合に得られた、現在位置の推定結果を表す図である。上述したように、人は、図３４において破線の矢印で示されるように、約２ｍ直進した後に、９０度左折し、さらに直進する歩行動作を行った。図３４に示される丸印は、慣性装置１が、図３３に示される進行速度波形から得られた現在の速度を元に、絶対位置推定部５００が推定した、現在位置の履歴を示すものである。図３４が示すように、慣性装置１を用いて現在位置を推定することにより、推定される現在位置は、実際の歩行動作とほぼ一致する。
図３５は、人が慣性装置１を片手に保持した状態で、約７５ｍの時計回りの周回歩行（合計９２歩）を行った場合の、現在位置の推定結果を表す図である。ここで、歩行経路上には、ＩＭＥＳによる絶対位置情報（緯度、経度）及びその誤差情報を送信する、２台の位置情報送信機が設置されている。従って、慣性装置１は、加速度情報等による現在位置の推定と、位置情報送信機から送信される情報を用いた現在位置の推定とを行うこととなる。
図３５において、破線で示される線が、実際の歩行経路を示し、丸印が、慣性装置１により取得された現在位置の履歴を示す。図３５に示されるように、慣性装置１を用いることにより、１００ミリ秒ごとの、高い時間分解能を持った現在位置情報が得られ、実際の歩行に対して、精度の高い位置推定が可能となる。なお、時間分解能は、パラメータ設定によって任意に変更することができる。
図３６は、図３５に示した現在位置の推定結果のうち、歩行開始位置（又は歩行終了位置）付近を拡大して示した図である。図３６に示されるように、歩行終了直前（すなわち、位置情報送信機Ａからの位置情報による観測更新処理発生前の位置）では、約１ｍの測位誤差が発生している。これは、進行速度波形合成部３１４から得られる現在の進行速度の推定値に誤差が含まれること、および、該推定値の時間発展処理により現在位置へ測位誤差が蓄積することに起因する。すなわち、実際の測位環境において、想定される範囲の誤差である。
図３７は、図３５を用いて説明した人の歩行動作における、カルマンフィルタが推定する測位誤差推定値の変化を示す。慣性装置１は、進行速度波形合成部３１４の進行速度に含まれる誤差と現在位置の誤差を、カルマンフィルタの枠組みによって誤差共分散行列として、逐次推定する。このため、図３７が示すように、歩行に伴って、カルマンフィルタによって推定される測位誤差推定値（誤差共分散行列の対角成分に対して二乗和平方根を取ったもの）が増加していくのが分かる。
一方で、位置情報送信機から絶対位置情報及び誤差情報を受信できた場合には、慣性装置１は、位置情報送信機の測位手段による測位誤差と、カルマンフィルタによって推定されている現在位置に対する測位誤差の推定値とを、観測更新処理によって比較する。そして、慣性装置１は、誤差共分散行列が小さくなるように、現在位置の推定値に対して補正を実施する。その結果、図３７に示されるように、位置情報送信機Ａ及び位置情報送信機Ｂによる観測更新が実施された場合に、カルマンフィルタが推定する測位誤差推定値が減少する。結果として、慣性装置１は、外部から得た位置情報及び測位誤差を用いて、推定した現在位置を補正し、推定精度を改善することができる。
再び図３６を参照すると、歩行終了直前に、位置情報入力部から入力された正しい位置情報を用い観測更新処理を実行することで、蓄積した測位誤差が解消され、正しい現在位置（観測更新処理終了後の位置）に修正されていることがわかる。
このように、本実施形態における慣性装置１により、進行動作に伴う位置推定の分解能および推定精度が改善され、様々な携行形態に対応する、利便性が高い慣性装置および慣性航法技術が実現される。
１ 慣性装置
１１ ＣＰＵ
１２ ＲＡＭ
１３ ＲＯＭ
１４ 加速度センサ
１５ 角速度センサ（ジャイロセンサ）
１６ 地磁気センサ
１７ マイク
１８ スピーカ
１９ 通信モジュール
２０ Ｂｌｕｅｔｏｏｔｈ通信モジュール
２１ ＧＰＳ通信モジュール
２２ ディスプレイ
２３ タッチパネル
２４ 電池
２５ 気圧センサ
１００ 座標系変換部
１０１ 加速度取得部
１０２ 角速度取得部
１０３ 地磁気取得部
１０４ 姿勢演算部
１０５ 地磁気信頼性評価部
１０７ 絶対加速度変換部
２０１ バンドパスフィルタ
２０４ ピーク検出部
２０５ ピーク位置記憶部
２０６ 移動加速度記憶部
２０７ 水平成分移動速度特徴情報管理部
２０８ 鉛直成分ピーク移動加速度取得部
２０９ 水平成分移動速度特徴情報取得部
２１０ 周期取得部
２１１ 判定部
２１２ 進行方向推定部
３００ 進行速度推定部
３０１ 水平成分速度特徴情報取得部
３０２ 鉛直成分ピーク加速度取得部
３０３ 周期取得部
３０４ ぶれ幅取得部
３０５ 進行方向取得部
３０６ 変換部
３１２ 進行速度波形生成部
３１４ 進行速度波形合成部
３１５ 進行速度波形記憶部
４００ 絶対位置情報入力部
４０１ 第一の絶対位置取得部
４０２ 第二の絶対位置取得部
４０３ 測位時間計測部
４０４ 誤差補正部
５００ 絶対位置推定部
５０１ 時間発展処理部
５０２ 第一の観測更新処理部
５０３ 第二の観測更新処理部
５０４ 第三の観測更新処理部
５０５ 現在位置と誤差情報
６００ マップマッチング部
６０１ マップマッチング処理部
特開２０１２−０８８２５３号広報 特許第４７１４８５３号広報 特開２００３−３０２４１９号公報 特開２０１１−２３７４５２号公報 特開２０００−９７７２２号公報 特開２００２−１３９３４０号公報
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日 Wei Tech Ang et al.、" Kalman filtering for real-time orientation tracking of handheld microsurgical instrument"、Intelligent Robots and Systems, 2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference、2004年9月28日、Volume 3、p.2574-2580 vol.3 Malcolm D. Shuster、"Deterministic Three-Axis Attitude Determination"、The Journal of the Astronautical Sciences、2004年7月、Volume 52、No. 3、p.405-419 "TRIAD Algorithm"、[online]、2011年5月25日、Wikipedia、インターネット<URL:http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm> 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 (10)

  1. 慣性センサの出力を用いて現在位置を推定する慣性装置であって、
    前記出力から求めた、当該慣性装置を有する対象となるユーザが歩行又は走行する進行動作の特徴を表すパラメータから、進行速度の変化を表す波形を生成する波形生成部と、
    生成された複数の前記波形を合成する波形合成部と、
    合成された前記波形から取得した進行速度情報と、前記対象の実際の進行速度に対する誤差を表す前記進行速度情報の誤差情報とを用いて現在位置を推定する推定部と、
    を有する、慣性装置。
  2. 前記波形生成部は、前記パラメータ及び前記対象の属性情報と関連付けられた係数を用いて前記波形を生成する、
    請求項１に記載の慣性装置。
  3. 前記パラメータは、当該慣性装置の進行動作に係る、水平方向の移動速度、鉛直方向の加速度、周期、進行方向、及び、進行方向に対する左右方向のぶれ幅に関するパラメータを含む、
    請求項１又は２に記載の慣性装置。
  4. 位置情報送信装置から送信された情報に基づいて取得した絶対位置情報及び該絶対位置情報の誤差情報を前記推定部に入力する入力部
    をさらに有し、
    前記推定部は、入力された前記絶対位置情報及び該絶対位置情報の誤差情報を用いて、推定された前記現在位置を更新する、
    請求項１ないし３の何れか一項に記載の慣性装置。
  5. 前記絶対位置情報を取得する間隔を計測する計測部と、
    計測された前記間隔に応じて、前記絶対位置情報の誤差情報を補正する補正部と、
    をさらに有する、請求項４に記載の慣性装置。
  6. 前記入力部は、通信方式の異なる複数の前記位置情報送信装置から送信された前記絶対位置情報及び該絶対位置情報の誤差情報を入力する、
    請求項４又は５に記載の慣性装置。
  7. 一の前記位置情報送信装置は、ＩＭＥＳの規格に準ずる絶対位置情報を送信する、
    請求項６に記載の慣性装置。
  8. 推定された前記現在位置と、地図データベースを用いてマップマッチングを行い、前記現在位置を修正するマップマッチング部
    をさらに有し、
    前記推定部は、修正された前記現在位置を表す情報を用いて、推定された前記現在位置を更新する、
    請求項１ないし７の何れか一項に記載の慣性装置。
  9. 慣性センサの出力を用いて現在位置をコンピュータが推定する方法であって、
    コンピュータが、前記出力から求めた、慣性装置を有する対象となるユーザが歩行又は走行する進行動作の特徴を表すパラメータから、進行速度の変化を表す波形を生成する波形生成段階と、
    コンピュータが、生成された複数の前記波形を合成する波形合成段階と、
    コンピュータが、合成された前記波形から取得した進行速度情報と、前記対象の実際の進行速度に対する誤差を表す前記進行速度情報の誤差情報とを用いて現在位置を推定する推定段階と、
    を有する、方法。
  10. コンピュータに、請求項９に記載の方法を実行させるプログラム。
JP2013230548A 2013-02-04 2013-11-06 慣性装置、方法及びプログラム Active JP6268945B2 (ja)

Priority Applications (7)

Application Number Priority Date Filing Date Title
JP2013230548A JP6268945B2 (ja) 2013-02-04 2013-11-06 慣性装置、方法及びプログラム
TW103103400A TWI490497B (zh) 2013-02-04 2014-01-29 慣性裝置、方法以及儲存媒體
EP14745398.9A EP2951529B1 (en) 2013-02-04 2014-01-30 Inertial device, method, and program
US14/651,273 US20150330791A1 (en) 2013-02-04 2014-01-30 Inertial device, method, and program
PCT/JP2014/052706 WO2014119801A1 (en) 2013-02-04 2014-01-30 Inertial device, method, and program
CN201480007053.4A CN104969030B (zh) 2013-02-04 2014-01-30 惯性装置、方法和程序
KR1020157020541A KR101732835B1 (ko) 2013-02-04 2014-01-30 관성 장치, 방법, 및 프로그램

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2013019210 2013-02-04
JP2013019210 2013-02-04
JP2013230548A JP6268945B2 (ja) 2013-02-04 2013-11-06 慣性装置、方法及びプログラム

Publications (2)

Publication Number Publication Date
JP2014167461A JP2014167461A (ja) 2014-09-11
JP6268945B2 true JP6268945B2 (ja) 2018-01-31

Family

ID=51262499

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2013230548A Active JP6268945B2 (ja) 2013-02-04 2013-11-06 慣性装置、方法及びプログラム

Country Status (7)

Country Link
US (1) US20150330791A1 (ja)
EP (1) EP2951529B1 (ja)
JP (1) JP6268945B2 (ja)
KR (1) KR101732835B1 (ja)
CN (1) CN104969030B (ja)
TW (1) TWI490497B (ja)
WO (1) WO2014119801A1 (ja)

Families Citing this family (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10019762B2 (en) 2014-05-15 2018-07-10 State Farm Mutual Automobile Insurance Company System and method for identifying idling times of a vehicle using accelerometer data
US10304138B2 (en) 2014-05-15 2019-05-28 State Farm Mutual Automobile Insurance Company System and method for identifying primary and secondary movement using spectral domain analysis
US9127946B1 (en) * 2014-05-15 2015-09-08 State Farm Mutual Automobile Insurance Company System and method for identifying heading of a moving vehicle using accelerometer data
JP2016033473A (ja) * 2014-07-31 2016-03-10 セイコーエプソン株式会社 位置算出方法及び位置算出装置
JP6476925B2 (ja) * 2015-01-30 2019-03-06 カシオ計算機株式会社 情報処理装置、位置更新方法及びプログラム
WO2016206119A1 (en) * 2015-06-26 2016-12-29 Intel Corporation Technologies for pedestrian dead reckoning
US10408627B2 (en) 2015-11-30 2019-09-10 Ricoh Company, Ltd. Inertial device to estimate position based on corrected movement velocity
JPWO2017150162A1 (ja) * 2016-03-01 2018-12-20 株式会社リコー 位置推定装置、位置推定方法、及びプログラム
JP6804908B2 (ja) * 2016-09-13 2020-12-23 株式会社東芝 推定装置、推定方法及びコンピュータプログラム
US10330479B2 (en) * 2016-09-20 2019-06-25 Trimble Inc. Vehicle navigation by dead reckoning and GNSS-aided map-matching
US10323942B2 (en) * 2016-09-23 2019-06-18 Qualcomm Incorporated User-specific learning for improved pedestrian motion modeling in a mobile device
KR101941009B1 (ko) * 2016-09-28 2019-01-22 광주과학기술원 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치
KR101916908B1 (ko) * 2016-09-28 2018-11-08 광주과학기술원 위치 추정 시스템 및 위치 추정 시스템을 포함하는 무인 이동 장치
CN108537094B (zh) * 2017-03-03 2022-11-22 株式会社理光 图像处理方法、装置和系统
CN107121128B (zh) * 2017-05-27 2022-04-05 中国北方车辆研究所 一种足式机器人地形参数的测量方法及系统
JP6686985B2 (ja) * 2017-08-03 2020-04-22 カシオ計算機株式会社 軌跡推定装置、軌跡推定方法及び軌跡推定プログラム
JP2019148586A (ja) * 2018-02-27 2019-09-05 シャープ株式会社 位置計測装置、位置補正方法、及び位置情報取得システム
JP7059114B2 (ja) * 2018-06-15 2022-04-25 株式会社東芝 位置計測装置および位置計測方法
KR102177663B1 (ko) * 2019-04-02 2020-11-11 주식회사 로터스에코 제로 크로싱 최소화를 위한 쿼터니언 분해 방법 및 그를 위한 장치
CN111174780B (zh) * 2019-12-31 2022-03-08 同济大学 盲人道路惯导定位系统
CN112033439B (zh) * 2020-08-20 2022-08-12 哈尔滨工业大学 一种摇摆基座地球系下重力加速度矢量无纬度构建方法
CN112461237B (zh) * 2020-11-26 2023-03-14 浙江同善人工智能技术有限公司 一种应用于动态变化场景下的多传感器融合定位方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5610847A (en) * 1994-10-28 1997-03-11 Cadence Design Systems, Inc. Ratiometric fourier analyzer
JP3505040B2 (ja) * 1996-07-11 2004-03-08 株式会社リコー 携帯型情報処理装置
JP3775779B2 (ja) 2000-10-30 2006-05-17 株式会社国際電気通信基礎技術研究所 歩行航行装置およびそれを用いたナビゲーションシステム
US6826477B2 (en) * 2001-04-23 2004-11-30 Ecole Polytechnique Federale De Lausanne (Epfl) Pedestrian navigation method and apparatus operative in a dead reckoning mode
JP3801163B2 (ja) * 2003-03-07 2006-07-26 セイコーエプソン株式会社 体動検出装置、ピッチ計、歩数計、腕時計型情報処理装置、制御方法及び制御プログラム
JP2006177749A (ja) 2004-12-22 2006-07-06 Ritsumeikan 周期運動体の移動軌跡算出方法及び装置
JP2009115714A (ja) * 2007-11-08 2009-05-28 Tottori Univ 移動体の速度測定方法および同測定装置
CN100595520C (zh) * 2008-02-21 2010-03-24 上海交通大学 适用于步行者的定位方法
WO2010013745A1 (ja) * 2008-07-29 2010-02-04 京セラ株式会社 携帯電子機器
JP4885922B2 (ja) * 2008-08-27 2012-02-29 京セラ株式会社 携帯電子機器
TWI457793B (zh) * 2008-08-08 2014-10-21 Ind Tech Res Inst 即時動作辨識方法及其慣性感測與軌跡重建裝置
JP2011102707A (ja) * 2009-11-10 2011-05-26 Seiko Epson Corp 測位装置及び測位方法
JP2012037452A (ja) * 2010-08-10 2012-02-23 Panasonic Corp 歩行方位検出装置および歩行方位検出方法
CN102184549B (zh) * 2011-04-29 2012-10-10 闫文闻 一种运动参数确定方法、装置和运动辅助设备
CN102252676B (zh) * 2011-05-06 2014-03-12 微迈森惯性技术开发（北京）有限公司 运动姿态数据获取、人体运动姿态追踪方法及相关设备
TWI435057B (zh) * 2011-06-23 2014-04-21 Automotive Res & Testing Ct Method and apparatus for learning correction of vehicle dynamic inertial sensor
JP5082001B2 (ja) 2011-08-15 2012-11-28 株式会社日立製作所 物体の進行方向検知方法、位置検知方法、進行方向検知装置、位置検知装置、移動動態認識方法及び移動動態認識装置

Also Published As

Publication number Publication date
EP2951529A1 (en) 2015-12-09
KR101732835B1 (ko) 2017-05-04
US20150330791A1 (en) 2015-11-19
WO2014119801A1 (en) 2014-08-07
EP2951529B1 (en) 2021-03-10
KR20150099864A (ko) 2015-09-01
JP2014167461A (ja) 2014-09-11
TW201439542A (zh) 2014-10-16
EP2951529A4 (en) 2016-01-27
CN104969030B (zh) 2017-06-30
TWI490497B (zh) 2015-07-01
CN104969030A (zh) 2015-10-07

Similar Documents

Publication Publication Date Title
JP6268945B2 (ja) 慣性装置、方法及びプログラム
JP6322960B2 (ja) 慣性装置、方法及びプログラム
US10408627B2 (en) Inertial device to estimate position based on corrected movement velocity
JP6852310B2 (ja) 慣性装置、プログラム、測位方法
JP7342864B2 (ja) 測位プログラム、測位方法、及び測位装置
US8473241B2 (en) Navigation trajectory matching
EP2740256B1 (en) Moving direction determination with noisy signals from inertial navigation systems on mobile devices
JP7023234B2 (ja) 歩行者の動きを推定する方法
WO2015146046A1 (ja) 相関係数補正方法、運動解析方法、相関係数補正装置及びプログラム
JP2011503571A (ja) 物体の方位測定
JP2015190850A (ja) 誤差推定方法、運動解析方法、誤差推定装置及びプログラム
Sang et al. A self-developed indoor three-dimensional pedestrian localization platform based on MEMS sensors
Cui et al. Bounding inertial drift with human gait dynamics for personal navigation
JP6259256B2 (ja) 前進運動加速度算出方法及び装置並びにプログラム
Gao et al. Data fusion with two nonlinear constraints on Kalman filtering
JP2019219187A (ja) 位置計測装置および位置計測方法
Huang Self-Contained Pedestrian Tracking With Mems Sensors

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20161021

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20171003

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20171110

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: 20171205

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20171218

R151 Written notification of patent or utility model registration

Ref document number: 6268945

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151