JP6516332B2 - 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 - Google Patents
角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 Download PDFInfo
- Publication number
- JP6516332B2 JP6516332B2 JP2016066935A JP2016066935A JP6516332B2 JP 6516332 B2 JP6516332 B2 JP 6516332B2 JP 2016066935 A JP2016066935 A JP 2016066935A JP 2016066935 A JP2016066935 A JP 2016066935A JP 6516332 B2 JP6516332 B2 JP 6516332B2
- Authority
- JP
- Japan
- Prior art keywords
- matrix
- acc
- angular velocity
- err
- attitude
- 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
Links
Images
Landscapes
- Navigation (AREA)
Description
また、十字通路で進行方向を誤判定しなかったとしても、前方方向は常に地図情報に基づく方向のみを候補として出力する。そのために、例えば直進中に人を避けるといった動作までも検出することは難しい。
正解方向を予め記憶したマップデータ記憶手段と、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列q gyro と、時刻t-1における端末姿勢行列q t-1 とから、時刻tにおける角速度姿勢行列q t gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
加速度センサによって計測された重力ベクトルg acc と、角速度姿勢行列q t gyro から推定された重力ベクトルh(q t gyro )との差分e t acc を用いて、角速度姿勢行列q t gyro から、加速度姿勢行列q t acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
角速度姿勢行列q t gyro 又は加速度姿勢行列q t acc から前方方向を推定する前方方向推定手段と、
正解方向と前方方向との差分となる方向誤差E t を算出する方向誤差算出手段と、
正解方向の姿勢行列q t correct と、加速度姿勢行列q t acc との間の残差e err t を算出し、加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列P t acc を用いて、最適カルマンゲインK t err を算出し、加速度姿勢行列q t acc から、残差e err t 及び最適カルマンゲインK t err の積に基づく誤差姿勢行列q t err を算出し、共分散行列P t acc から、最適カルマンゲインK t err に基づく共分散行列P t err を算出し、更に、P t err を時刻tにおける最終的な端末姿勢行列の共分散行列P t として、時刻t+1における角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
を有することを特徴とする。
前方方向推定手段によって推定された前方方向を、アプリケーションへ出力することも好ましい。
誤差姿勢行列算出手段から出力された誤差姿勢行列qtから、前方方向を推定する付加的前方方向推定手段を更に有し、
付加的前方方向推定手段によって推定された前方方向を、アプリケーションへ出力することも好ましい。
端末姿勢行列であるqt及びqt gyro,qt acc,qt errは、スカラー部及びベクトル部によって表現したクォータニオン(四元数)であることも好ましい。
マップデータ記憶手段の正解方向は、当該携帯端末の位置から通路方向に応じた1つ以上の進行可能な方向であることも好ましい。
加速度センサによって計測された加速度ベクトルaを端末座標系における重力方向とみなすことで、初期の角速度姿勢行列q0を算出し、当該角速度姿勢行列q0を角速度姿勢行列算出手段へ出力する初期姿勢行列算出手段を
更に有することも好ましい。
正解方向を予め記憶したマップデータ記憶手段と、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列q gyro と、時刻t-1における端末姿勢行列q t-1 とから、時刻tにおける角速度姿勢行列q t gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
加速度センサによって計測された重力ベクトルg acc と、角速度姿勢行列q t gyro から推定された重力ベクトルh(q t gyro )との差分e t acc を用いて、角速度姿勢行列q t gyro から、加速度姿勢行列q t acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
角速度姿勢行列q t gyro 又は加速度姿勢行列q t acc から前方方向を推定する前方方向推定手段と、
正解方向と前方方向との差分となる方向誤差E t を算出する方向誤差算出手段と、
正解方向の姿勢行列q t correct と、加速度姿勢行列q t acc との間の残差e err t を算出し、加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列P t acc を用いて、最適カルマンゲインK t err を算出し、加速度姿勢行列q t acc から、残差e err t 及び最適カルマンゲインK t err の積に基づく誤差姿勢行列q t err を算出し、共分散行列P t acc から、最適カルマンゲインK t err に基づく共分散行列P t err を算出し、更に、P t err を時刻tにおける最終的な端末姿勢行列の共分散行列P t として、時刻t+1における角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
してコンピュータを機能させることを特徴とする。
装置は、正解方向を予め記憶したマップデータ記憶部を有し、
装置は、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列q gyro と、時刻t-1における端末姿勢行列q t-1 とから、時刻tにおける角速度姿勢行列q t gyro を、カルマンフィルタを用いて算出すると共に、加速度センサによって計測された重力ベクトルg acc と、角速度姿勢行列q t gyro から推定された重力ベクトルh(q t gyro )との差分e t acc を用いて、角速度姿勢行列q t gyro から、加速度姿勢行列q t acc を、カルマンフィルタを用いて算出する第1のステップと、
角速度姿勢行列q t gyro 又は加速度姿勢行列q t acc から前方方向を推定する第2のステップと、
正解方向と前方方向との差分となる方向誤差E t を算出する第3のステップと、
正解方向の姿勢行列q t correct と、加速度姿勢行列q t acc との間の残差e err t を算出し、前記加速度姿勢行列q t acc を算出する際に用いたカルマンフィルタによって算出された共分散行列P t acc を用いて、最適カルマンゲインK t err を算出し、加速度姿勢行列q t acc から、残差e err t 及び最適カルマンゲインK t err の積に基づく誤差姿勢行列q t err を算出し、共分散行列P t acc から、最適カルマンゲインK t err に基づく共分散行列P t err を算出し、更に、P t err を時刻tにおける最終的な端末姿勢行列の共分散行列P t として、時刻t+1における第1のステップへフィードバックする第4のステップと
を実行することを特徴とする。
図3は、図2における各機能部の算出式を表す説明図である。
角速度センサ101は、基準軸に対する回転運動を検出するセンサであって、角度又は角速度を出力する。携帯端末に搭載可能な角速度センサは、一般に、MEMS(micro electro mechanical systems)で構成されており、物体の運動軸と回転軸との直交方向に働く力を計測することができる。
ωt=(ωx ωy ωz)
角速度ωについて、角速度センサのサンプリングレートfに対する時刻tにおける端末回転量Δθtは、以下のように表される。
Δθt=ωt/f
この回転は、端末座標系について、Δθt/|Δθt|を回転の軸として、|Δθt|の大きさだけ回転させたものである。
加速度センサ102は、端末姿勢を検出するべく、3次元(3軸)の加速度(単位時間当たりの速度の変化)を計測する。
時刻tにおける加速度センサ102から計測される加速度ベクトルは、以下のように表される。
at=(xt,yt,zt)
初期姿勢算出部11は、携帯端末の初期姿勢を決定する。初期姿勢としては、加速度センサ102によって計測された加速度ベクトルaを、端末座標系における重力方向gとみなす。端末姿勢は、重力方向gを鉛直方向下向きとして決定される。
g:重力ベクトル(=(0,0,−1))
a:加速度センサから得られた加速度ベクトル
na=a/|a| (|x|はベクトルxの大きさを表す)
h=g+na/|g+na|
q0=(na・h,na×h) (・は内積、×は外積)
端末姿勢行列であるqt(以下で記述するqt gyro,qt acc,qt errも同様)は、スカラー部及びベクトル部によって表現したクォータニオン(四元数)である(例えば非特許文献4参照)。
角速度姿勢行列算出部12は、角速度センサ101によって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列qgyroと、時刻t-1における端末姿勢行列qt-1とから、時刻tにおける角速度姿勢行列qt gyroを算出する。
qt gyro=qgyro×qt-1
×:クォータニオンの積の演算子
時刻t-1における端末姿勢行列qt-1は、誤差姿勢行列算出部16から入力される。
そして、算出された角速度姿勢行列qt gyroは、加速度姿勢行列算出部13へ出力される。
加速度姿勢行列算出部13は、加速度センサ102によって計測された重力ベクトルgaccと、角速度姿勢行列qt gyroから推定された重力ベクトルh(qt gyro)との差分et accを用いて、角速度姿勢行列qt gyroから、加速度姿勢行列qt accを算出する。
qt acc=qt gyro+Kt accet acc
算出された加速度姿勢行列qt accは、前方方向推定部14及び誤差姿勢行列算出部16へ出力される。qt accは、補正前の端末姿勢に基づく加速度姿勢行列である。
前方方向推定部14は、加速度姿勢行列qt accから前方方向を推定する。例えば、ユーザが携帯端末を把持して、そのディスプレイを見ながら歩行している場合を前提とするならば、単純に、世界座標系における端末のy軸方向を水平面に投影したものを前方方向とみなす。
前方方向推定部14によって推定された前方方向は、アプリケーションと方向誤差算出部15へ出力される。
図4(b)によれば、世界座標系が表されている。世界座標系とは、地表に対するユークリッド座標系で、水平面をxy平面、鉛直方向をz軸としたものである。
尚、水平面について、どの方向がx軸y軸になるかは不定であって、端末の初期姿勢によって決定される。特に、角速度センサと加速度センサのみを用いる場合、水平面上での絶対的な方向(方位角)を定めることはできない。但し、地磁気センサを用いた場合、絶対的な方向を決定することができる。
マップデータ記憶部150は、正解方向を予め記憶したものである。「正解方向」は、当該携帯端末の位置から通路方向に応じた1つ以上の進行可能な方向である。例えば、携帯端末が通行可能な地図情報である。正解方向の候補としては、進行可能な通路に応じて、離散化した方向(0度、90度、180度、270度)となる。
方向誤差算出部15は、前方方向推定部14から前方方向を入力し、マップデータ記憶部150から正解方向を入力する。そして、方向誤差算出部15は、前方方向に対して最も角度差が小さい正解方向を検索する。そして、本発明によれば、その角度差を打ち消す方向に進行方向をずらすこととなる。
方向誤差算出部15は、正解方向と前方方向との差分となる方向誤差Etを算出する。算出された方向誤差Etは、誤差姿勢行列算出部16へ出力される。
尚、他の実施形態として、マップデータを用いて、推定された現在位置と最も近い通路の方向を正解方向として、方向誤差を計算することもできる。
誤差姿勢行列算出部16は、加速度姿勢行列算出部13から加速度姿勢行列qt accを入力し、方向誤差算出部15から方向誤差Etを入力する。
誤差姿勢行列算出部16は、加速度姿勢行列qt accに方向誤差Etで重み付けた誤差から誤差姿勢行列qt errを算出する。そして、当該誤差姿勢行列qt errを時刻tにおける最終的な端末姿勢行列qtとして、時刻t+1における角速度姿勢行列算出部12へフィードバックして入力する。
付加的前方方向推定部17は、誤差姿勢行列算出部16から出力された誤差姿勢行列qtから、前方方向を推定する。付加的前方方向推定部17によって推定された前方方向を、アプリケーションへ出力する。
前方方向推定部14から出力される前方方向と異なって、付加的前方方向推定部17は、姿勢更新後の姿勢(誤差姿勢行列算出部16の誤差姿勢行列qt)に基づいて前方方向を推定する。そのために、現在のタイミングで補正された姿勢に基づく、正確な前方方向を出力することができる。
本発明によれば、前述した角速度姿勢行列算出部12と、加速度姿勢行列算出部13と、誤差姿勢行列算出部16とは、以下のような4つの実施形態で実現することができる。
<第1の実施形態:カルマンフィルタを用いた第1の誤差姿勢行列>
<第2の実施形態:カルマンフィルタを用いた第2の誤差姿勢行列>
<第3の実施形態:加速度姿勢行列の単位補正角度に基づく誤差姿勢行列>
<第4の実施形態:角速度姿勢行列の単位補正角度に基づく誤差姿勢行列>
「カルマンフィルタ」とは、離散的な誤差のある観測から、時々刻々と時間変化する量を推定するために用いられる連続時間線形動的システムである(例えば非特許文献2、3参照)。これは、隠れマルコフモデルに類似したものであって、ガウス分布に従う雑音によって未来の状態変数を統計的に出力することができる。
角速度姿勢行列算出部12は、角速度センサ101によって絶対座標系で表した回転をΔθt Gとすると、時刻t-1における端末姿勢qt-1を用いて、以下のように表すことができる。
Δθt G=qt-1Δθt
q:回転を行列で表したクォータニオン(四元数)
θt=|Δθt G|
nt=Δθt G/|Δθt G|
qgyro=(cos(θt/2) ntsin(θt/2))
角速度センサから計算した時刻tにおける端末姿勢である角速度姿勢行列qt gyroは、時刻t-1における端末姿勢qt-1を、qgyroだけ回転させたものとなる。
qt gyro=qgyro×qt-1
×:クォータニオンの積の演算子
そして、角速度姿勢行列算出部12は、カルマンフィルタの「予測ステップ」を実行する。
Pt gyro=FPt-1FT+GkQkGk T
Ft=∂/∂q(qgyro・q)|qt-1
尚、カルマンフィルタによれば、時変な行列Fk、Gk、Hk、Qk、Rkによって特徴付けられる線形の状態方程式として表される。
加速度姿勢行列算出部13は、カルマンフィルタの「更新ステップ」を実行する。
gacc:加速度ベクトル(=重力ベクトル)
h(q):端末姿勢qから推定される重力ベクトル
et acc=gacc−h(qt gyro)
Ht acc=∂/∂q(h(q))|qt gyro
St acc=Ht accPt gyroHt accT+Rt acc
Kt acc=Pt gyroHt accTSacc-1
qt acc=qt gyro+Kt accet acc
Pt acc=(I−Kt accHt acc)Pt gyro
加速度姿勢行列算出部13は、加速度姿勢行列qt accとカルマンフィルタの共分散行列Pt accとを、前方方向推定部14及び誤差姿勢行列算出部16へ出力する。
誤差姿勢行列算出部16は、以下のように端末姿勢を更新する。
方向誤差Etが0となる端末姿勢qt coreectは、誤差による補正前の端末姿勢qt accを鉛直軸回りに−Etだけ回転させたものである。鉛直軸回りに−Etの回転は、以下のクォータニオンを用いて表される。
qt v=(cos(−Et/2) 0 0 sin(−Et/2))
これを用いることで、方向誤差Etが0となる端末姿勢qt coreectは下記のように計算できる。
qt coreect=qt v×qt acc
(qt accを鉛直方向回りに誤差と同じ大きさで逆方向に回転させた姿勢)
qt coreect:観測値(方向誤差が0となる端末姿勢)
qt acc:内部状態の推定値
正解方向の姿勢行列qt correctと、加速度姿勢行列qt accとの間の(観測)残差eerr tを、以下のように算出する。
et err=qt coreect−qt acc
また、観測残差の共分散は、以下のように計算できる。
St err=Pt acc+Rt err
Rt err:カルマンフィルタに外部から与えるパラメータ(固定値でもよい)
Rt errのパラメータの値を変えることで、1度のステップでの補正量を調整することが可能である。
加速度姿勢行列算出部13のカルマンフィルタによって算出された共分散行列Pt accを用いて、最適カルマンゲインKt errを、以下のように算出する(M-1は、行列Mの逆行列を表す)。
Kt err=Pt accSt err-1
加速度姿勢行列qt accから、残差eerr t及び最適カルマンゲインKt errの積に基づく誤差姿勢行列qt err(更新後の姿勢の推定値)を、以下のように算出する。
qt err=qt acc+KT erret err
共分散行列Pt accから、最適カルマンゲインKt errに基づく共分散行列Ptを、以下のように算出する。
Pt err=(I−Kt err)Pt acc
誤差姿勢行列算出部16は、以下のように端末姿勢を更新する。
クォータニオンq=(w,x,y,z)から、このクォータニオンによる回転と同等の回転を表す行列への変換を表す関数を、以下のように予め定義する。
誤差による補正前の端末のY軸方向yt accは、誤差による補正前の端末姿勢qt accを用いて計算することができる。具体的には、端末座標系でのy軸方向を表すベクトル、即ち、(0,1,0)をqt accを用いて回転させればよい。即ち、以下のように計算することができる(MTは、行列Mの転置行列を表す)。
yt acc=R(qt acc)(0,1,0)T
鉛直軸回りに−Etの回転は、以下のクォータニオンを用いて表される。
qt v=(cos(−Et/2) 0 0 sin(−Et/2))
これを用いて、方向誤差Etが0となる端末のY軸方向yt coreectは、以下のように算出することができる。
yt coreect=R(qt v)yt acc
(yt accを鉛直方向回りに誤差と同じ大きさで逆方向に回転させたベクトル)
yt coreect:観測値(方向誤差が0となる端末のY軸方向)
yt acc:内部状態から推定値した端末のY軸方向
正解方向の姿勢行列yt correctと、加速度姿勢行列yt accとの間の(観測)残差eerr tを、以下のように算出する。
et err=yt coreect−R(qt acc)(0,1,0)T
尚、R(qt acc)(0,1,0)T=yt accである。
また、観測残差の共分散は、以下のように算出できる。
St err=HPt accHT+Rt err
Rt err:カルマンフィルタに外部から与えるパラメータ(固定値でもよい)
H:R(q)(0,1,0)Tのヤコビ行列。
即ち、H=∂/∂q(R(q)(0,1,0)T)|qt acc
Rt errのパラメータの値を変えることで、1度のステップでの補正量を調整することが可能である。
加速度姿勢行列算出部13のカルマンフィルタによって算出された共分散行列Pt accを用いて、最適カルマンゲインKt errを、以下のように算出する(M-1は、行列Mの逆行列を表す)。
Kt err=Pt accSt err-1
加速度姿勢行列qt accから、残差eerr t及び最適カルマンゲインKt errの積に基づく誤差姿勢行列qt(更新後の姿勢の推定値)を、以下のように算出する。
qt err=qt acc+KT erret err
共分散行列Pt accから、最適カルマンゲインKt errに基づく共分散行列Ptを、以下のように算出する。
Pt err=(I−Kt err)Pt acc
誤差姿勢行列算出部16は、所定の単位補正割合rに基づいて、誤差姿勢行列qt errを算出する。また、qt errを時刻tにおける最終的な姿勢行列qtとする。
(1)方向誤差Etに対する所定の単位補正割合rの単位補正角度rEtを算出する。
(2)加速度姿勢行列qt accに、単位補正角度rEtだけ鉛直軸回りへの回転を表す姿勢行列を積算して、誤差姿勢行列qt errを算出する。
補正後の端末姿勢qt errは、誤差による補正前の端末姿勢qt accを、鉛直軸回りに、−rEtだけ回転させたものである。鉛直軸回りに−rEtの回転は、以下のクォータニオンを用いて表される。
qt v=(cos(−rEt/2) 0 0 sin(−rEt/2))
qt err=qt v×qt acc
図5は、本発明における携帯端末の第2の機能構成図である。
(1)方向誤差Etに対する所定の単位補正割合rの単位補正角度rEtを算出する。
(2)角速度姿勢行列qt gyroに、単位補正角度rEtだけ鉛直軸回りへの回転を表す姿勢行列を積算して、誤差姿勢行列qt errを算出する。
101 角速度センサ
102 加速度センサ
11 初期姿勢算出部
12 角速度姿勢行列算出部
13 加速度姿勢行列算出部
14 前方方向推定部
15 方向誤差算出部
150 マップデータ記憶部
16 誤差姿勢行列算出部
17 付加的前方方向推定部
Claims (8)
- 角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する携帯端末であって、
正解方向を予め記憶したマップデータ記憶手段と、
前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列q gyro と、時刻t-1における端末姿勢行列q t-1 とから、時刻tにおける角速度姿勢行列q t gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
前記加速度センサによって計測された重力ベクトルg acc と、角速度姿勢行列q t gyro から推定された重力ベクトルh(q t gyro )との差分e t acc を用いて、角速度姿勢行列q t gyro から、加速度姿勢行列q t acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
前記角速度姿勢行列q t gyro 又は前記加速度姿勢行列q t acc から前方方向を推定する前方方向推定手段と、
前記正解方向と前記前方方向との差分となる方向誤差E t を算出する方向誤差算出手段と、
前記正解方向の姿勢行列q t correct と、前記加速度姿勢行列q t acc との間の残差e err t を算出し、前記加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列P t acc を用いて、最適カルマンゲインK t err を算出し、前記加速度姿勢行列q t acc から、前記残差e err t 及び最適カルマンゲインK t err の積に基づく誤差姿勢行列q t err を算出し、前記共分散行列P t acc から、最適カルマンゲインK t err に基づく共分散行列P t err を算出し、更に、P t err を時刻tにおける最終的な端末姿勢行列の共分散行列P t として、時刻t+1における前記角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
を有することを特徴とする携帯端末。 - 前記前方方向推定手段によって推定された前記前方方向を、アプリケーションへ出力する
ことを特徴とする請求項1に記載の携帯端末。 - 前記誤差姿勢行列算出手段から出力された前記誤差姿勢行列qtから、前方方向を推定する付加的前方方向推定手段を更に有し、
前記付加的前方方向推定手段によって推定された前記前方方向を、アプリケーションへ出力する
ことを特徴とする請求項1に記載の携帯端末。 - 端末姿勢行列であるqt及びqt gyro,qt acc,qt errは、スカラー部及びベクトル部によって表現したクォータニオン(四元数)である
ことを特徴とする請求項1から3のいずれか1項に記載の携帯端末。 - 前記マップデータ記憶手段の前記正解方向は、当該携帯端末の位置から通路方向に応じた1つ以上の進行可能な方向である
ことを特徴とする請求項1から4のいずれか1項に記載の携帯端末。 - 加速度センサによって計測された加速度ベクトルaを端末座標系における重力方向とみなすことで、初期の角速度姿勢行列q0を算出し、当該角速度姿勢行列q0を前記角速度姿勢行列算出手段へ出力する初期姿勢行列算出手段を
更に有することを特徴とする請求項1から5のいずれか1項に記載の携帯端末。 - 角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する装置に搭載されたコンピュータを機能させるプログラムであって、
正解方向を予め記憶したマップデータ記憶手段と、
前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列q gyro と、時刻t-1における端末姿勢行列q t-1 とから、時刻tにおける角速度姿勢行列q t gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
前記加速度センサによって計測された重力ベクトルg acc と、角速度姿勢行列q t gyro から推定された重力ベクトルh(q t gyro )との差分e t acc を用いて、角速度姿勢行列q t gyro から、加速度姿勢行列q t acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
前記角速度姿勢行列q t gyro 又は前記加速度姿勢行列q t acc から前方方向を推定する前方方向推定手段と、
前記正解方向と前記前方方向との差分となる方向誤差E t を算出する方向誤差算出手段と、
前記正解方向の姿勢行列q t correct と、前記加速度姿勢行列q t acc との間の残差e err t を算出し、前記加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列P t acc を用いて、最適カルマンゲインK t err を算出し、前記加速度姿勢行列q t acc から、前記残差e err t 及び最適カルマンゲインK t err の積に基づく誤差姿勢行列q t err を算出し、前記共分散行列P t acc から、最適カルマンゲインK t err に基づく共分散行列P t err を算出し、更に、P t err を時刻tにおける最終的な端末姿勢行列の共分散行列P t として、時刻t+1における前記角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
してコンピュータを機能させることを特徴とするプログラム。 - 角速度センサ及び加速度センサを有する装置の前方方向推定方法であって、
前記装置は、正解方向を予め記憶したマップデータ記憶部を有し、
前記装置は、
前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列q gyro と、時刻t-1における端末姿勢行列q t-1 とから、時刻tにおける角速度姿勢行列q t gyro を、カルマンフィルタを用いて算出すると共に、前記加速度センサによって計測された重力ベクトルg acc と、角速度姿勢行列q t gyro から推定された重力ベクトルh(q t gyro )との差分e t acc を用いて、角速度姿勢行列q t gyro から、加速度姿勢行列q t acc を、カルマンフィルタを用いて算出する第1のステップと、
前記角速度姿勢行列q t gyro 又は前記加速度姿勢行列q t acc から前方方向を推定する第2のステップと、
前記正解方向と前記前方方向との差分となる方向誤差E t を算出する第3のステップと、
前記正解方向の姿勢行列q t correct と、前記加速度姿勢行列q t acc との間の残差e err t を算出し、前記加速度姿勢行列q t acc を算出する際に用いたカルマンフィルタによって算出された共分散行列P t acc を用いて、最適カルマンゲインK t err を算出し、前記加速度姿勢行列q t acc から、前記残差e err t 及び最適カルマンゲインK t err の積に基づく誤差姿勢行列q t err を算出し、前記共分散行列P t acc から、最適カルマンゲインK t err に基づく共分散行列P t err を算出し、更に、P t err を時刻tにおける最終的な端末姿勢行列の共分散行列P t として、時刻t+1における第1のステップへフィードバックする第4のステップと
を実行することを特徴とする装置の前方方向推定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016066935A JP6516332B2 (ja) | 2016-03-29 | 2016-03-29 | 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016066935A JP6516332B2 (ja) | 2016-03-29 | 2016-03-29 | 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2017181218A JP2017181218A (ja) | 2017-10-05 |
JP6516332B2 true JP6516332B2 (ja) | 2019-05-22 |
Family
ID=60004353
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2016066935A Active JP6516332B2 (ja) | 2016-03-29 | 2016-03-29 | 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6516332B2 (ja) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110855797A (zh) * | 2019-11-28 | 2020-02-28 | 河北农业大学 | 一种基于动作识别的绵羊行为监测***及方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2015152559A (ja) * | 2014-02-19 | 2015-08-24 | 株式会社リコー | 慣性装置、制御方法及びプログラム |
JP2015179002A (ja) * | 2014-03-19 | 2015-10-08 | セイコーエプソン株式会社 | 姿勢推定方法、姿勢推定装置及びプログラム |
-
2016
- 2016-03-29 JP JP2016066935A patent/JP6516332B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2017181218A (ja) | 2017-10-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10037028B2 (en) | Systems, devices, and methods for on-board sensing and control of micro aerial vehicles | |
Li et al. | Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems | |
JP6285628B2 (ja) | 移動端末及び位置推定方法並びにコンピュータ読出可能記録媒体 | |
JP5061264B1 (ja) | 小型姿勢センサ | |
JP4876204B2 (ja) | 小型姿勢センサ | |
Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
JP2006126148A (ja) | 方位姿勢検出装置 | |
Allotta et al. | Single axis FOG aided attitude estimation algorithm for mobile robots | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN102654404A (zh) | 一种提高航姿参考***解算精度和***抗干扰能力的方法 | |
JP2004239643A (ja) | ハイブリッド航法装置 | |
CN106813679B (zh) | 运动物体的姿态估计的方法及装置 | |
JP7025215B2 (ja) | 測位システム及び測位方法 | |
JP2012193965A (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
CN108627154A (zh) | 极地区域操作姿态与航向参考*** | |
Troni et al. | Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles | |
JP5511088B2 (ja) | 自律測位に用いる重力ベクトルを補正する携帯装置、プログラム及び方法 | |
EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
JP6516332B2 (ja) | 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 | |
JP2013122384A (ja) | カルマンフィルタ、及び、状態推定装置 | |
Allotta et al. | Underwater vehicles attitude estimation in presence of magnetic disturbances | |
Le et al. | A heading observer for ROVs under roll and pitch oscillations and acceleration disturbances using low-cost sensors | |
Munguia et al. | An attitude and heading reference system (AHRS) based in a dual filter | |
KR101340158B1 (ko) | 고정 표적을 이용한 무인항공기의 표적 위치 보정 방법 및 컴퓨터 판독 가능한 기록매체 | |
CN111492203A (zh) | 状态估计 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20180306 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20181220 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20190108 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190306 |
|
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: 20190411 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20190412 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6516332 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |