東京大学大学院 工学系研究科 社会基盤学専攻

第 2 0 1 2 - 0 4 号

平成 2 6 年 1 1 月

多様な情報を統合した拡張現実のための 標定・3次元復元手法に関する研究

多様な情報を統合した拡張現実のための 標定・3次元復元手法に関する研究

東京大学大学院 工学系研究科 社会基盤学専攻

准教授   布施 孝志

研究助成表紙.indd 2 2014/10/20 15:15

（一財）日本建設情報総合センター研究助成事業

多様な情報を統合した拡張現実のための 標定・３次元復元手法に関する研究

報告書

平成２６年８月

助成研究者紹介

氏

：布施

孝

志

現

：東京大学大学院工学系研究科社会基盤学専攻准教授（博士（工学））

主な研究業績 ：



布施孝志, 大矢紀之: 陸上・海底地形の高さの基準に関する歴史的経緯, 土木学会論文集



中西航

布施孝志: 逐次人物追跡のための事後確率を用いた人物抽出手法, 写真測量とリモー トセンシング, Vol.52, No.5, pp.255-265, 2013.



佐竹健治

堀宗朗編

東日本大震災の科学



日本写真測量学会編: 空間情報による災害の記録 伊勢湾台風から東日本大震災まで

鹿島出 版会, 2012.（分担執筆，編集委員幹事長）



布施孝志

中西航: 歩行者挙動モデルを統合した人物自動追跡手法の構築, 土木学会論文集D3,



清水英範

布施孝志

東京の原景観を探る～現代に蘇る江戸絵図の世界

土木計画学研究・論文 集



清水英範

布施孝志

再現・江戸の景観 －広重・北斎に描かれた江戸、描かれなかった江戸－

鹿島出版会, 2009.



清水英範

布施孝志

江戸の都市景観の再現に関する研究

土木学会論文集D, Vol.64,

No.3, pp.473-492, 2008.

 Yukio Sadahiro, Eihan Shimizu and Takashi Fuse: Visualization of historical data in Tokyo, In Yukio Sadahiro (Eds.): Spatial Data Infrastructure for Urban Regeneration, Springer, 2008.（分担執筆）



村井俊治

布施孝志

改訂版

ワークブック

日本測量協会

 Eihan Shimizu and Takashi Fuse: A method for visualizing the Landscape of old-time cities using GIS, In Atsuyuki Okabe (Eds.): GIS-based Studies in the Humanities and Social Sciences, CRC Press, 2005.（分

担執筆）

 (社)日本写真測量学会,

動体計測研究会編

デジタル写真測量の理論と実践, (社

2004.（分担執筆,

編集代表）



布施孝志

清水英範

高度撮影時系列画像を用いた車両動体認識手法の構築, 土木学会

論文集

1

目次

第

章 序論

1.1

研究の背景と目的

本研究の構成

第

章 画像ベースの標定・3 次元復元手法の整理

調整計算法の整理

2.2

初期値設定の方法

第

章 画像ベース手法の屋外での適用可能性の検証

特徴点抽出・追跡手法と屋外での適用可能性

特徴点追跡手法の改良

3.3

適用

第

章 他センサのデータを付加した手法の開発

提案手法の枠組み

4.2 POS

データのフィルタリング手法

バンドル調整による画像統合手法

適用

第

章 ベクターデータを付加した手法の開発

提案手法の枠組み

5.2

制約条件付きバンドル調整手法の開発

適用

第

章 結論

本研究の成果

今後の課題

参考文献

2

第

章 序論

1.1

研究の背景と目的

建設分野の現場において多様な情報を表現するため、拡張現実感（

は撮影された現実空間の画像上に情報表示するため、現場で自らの視点で情報を知覚できるという点 で優れている。AR においては、カメラの位置決めが中心的な問題の一つである。そのための最も簡 単な方法は、モバイル端末に内蔵された

や電子コンパスからの位置情報を用いることである。

しかし、その精度の問題から、3 次元モデルなどの情報表示には十分ではない。また、マーカー認識 による手法も提案されているが、マーカ自体の設置が困難な屋外での適用は難しく、屋内での適用に 偏向している現状である。屋外においては、磁場センサなどの高価なセンサを設置してユーザの正確 な位置決定も行われているが、適用性は限定的にならざるを得ない。

上記の問題に対し、屋外で簡易なカメラのみを用いて、動画像に対して標定（位置・傾きの推定）

を行い、同時に撮影対象の特徴点の

次元復元を行う研究が試みられている。特に、適用場面の汎用 性から、地上基準点を設置しない方法が望まれるが、未だチャレンジングな問題となっている。また、

現実空間における適用を考えると、実スケールでの推定が必要不可欠であるが、地上基準点を用いな いため、原理的には不可能である。

一方で、通常、簡易デバイスにおいても

た、構造物を表現するために

、CG、

において、実座標での

次元モデルの作成もされてお り、様々な分野で応用されている。これらの情報を最大限活用し、上記の画像をベースとした標定・

3

次元復元手法を構築することができれば、その適用可能性が大きく広がることが期待される。

以上の背景の下、本研究では、簡易なデバイスを前提として、低廉な

などのセンサ情報やベ クタデータである

次元モデルを付加情報として統合し、屋外で利用可能な

手法のための標定・

3

次元復元手法の開発を目的とする。

1.2

本研究の構成

本研究の構成を図

に示す。以下、個別に関して簡潔に説明する。

(1)

画像ベースの標定・

次元復元手法の整理

これまでにも、画像ベースの標定・

次元復元手法は、写真測量およびコンピュータビジョンの分 野で発展してきた。しかし、両者は個別に進展してきた。特に、標定・3 次元復元問題の同時推定に ついては、両分野ともバンドル調整を適用している。写真測量における蓄積に加え、コンピュータビ ジョンにおける適用への展開に着目し、両手法のレビューを行い、理論構成の共通点・相違点を整理 する。

3

図

本研究の構成

両分野におけるバンドル調整適用の比較ポイントとしては、非線形最適化問題における、①初期値 設定方法、および②最適化の解法、が主なものである。

(2)

屋外での適用可能性の検証

本研究では、地上基準点を用いない手法を前提としている。その場合には、動画像から特徴点の抽 出・追跡を行い、逐次、相互標定などによりカメラの移動も追跡し、ある程度の基線長を確保したフ レーム間に対してバンドル調整を適用する。これまでも、適用範囲が狭く、照明環境がコントロール 可能である屋内での適用は多数存在する。本研究では、屋外での適用可能性の評価を行う。ここでは、

簡易デバイスを前提として評価する。

屋外環境においては、天候の影響などから、特に特徴点の抽出・追跡が重要になる。動画像におけ る特徴点抽出・追跡手法は、主に、コンピュータビジョンの分野で開発され、広く利用されてきた。

そのため、コンピュータビジョンにおける特徴点抽出・追跡手法を対象に、屋外での適用可能性を検 証する。

(3)

他センサのデータを付加した手法の開発

基準点を用いない場合、画像ベースの標定・

次元復元手法の適用可能性には限界がある。そのた め、スマートフォンやタブレットなどの、簡易デバイスに搭載された

などのセンサ情報を用いた手法を開発する。

簡易デバイスにおいては、低廉なセンサのため、高精度な情報が得られないことも考慮する。例え ば

の場合には、実スケールを与える情報として相対位置のみを用いて、初期値設定問題に位置 付けた手法の開発を行う。また、加速度計においても、初期値設定への利用を考案する。さらに、複 数のセンサへの拡張が容易となるよう定式化を行う。

(4)

ベクタデータを付加した手法の開発

画像ベース手法に対して、低精度のセンサ情報を付加しても、精度の飛躍的な向上は見込めない。

そこで、地上基準点を用いる代わりに、CAD、

など、実スケールで作成された

次元モデ

画像ベースの標定・3 次元復元手法の整理

(2)

画像ベース手法の屋外での適用可能性の検証

(3)

他センサのデータを 付加した手法の開発

(4)

ベクタデータを 付加した手法の開発

(5)

提案手法の適用可能性の検証

4

ルの統合手法を開発する。

これまでも、距離情報などの追加情報を利用する手法が提案されているが、ここでは、3 次元で作 成されたベクタデータを制約条件として最適化問題として定式化する。既に、バンドル調整が最適化 問題として位置付けられているため、その制約条件として考えることにより、ラグランジュ未定乗数 法が適用可能となる。

(5)

提案手法の適用可能性の検証

提案手法を実空間に適用し、その精度評価を行い、適用可能性の検証を行う。精度評価においては、

別途観測を行ったMMS による取得データや地上型レーザスキャナデータを検証用データとして用い る。なお、本研究の成果は、理想的には通信環境を前提として、サーバによる処理を組み込むことが 考えられる。しかしながら、本研究の主眼は、地上基準点を用いず、その他のセンサ情報やベクタデ ータの導入を行うことであるため、サーバ処理を対象とはしない。

5

第

章 画像ベースの標定・3 次元復元手法の整理

2.1

調整計算法の整理

写真測量およびコンピュータビジョンの両分野において、画像ベースの標定・

次元復元の同時推 定法として、バンドル調整が広く適用されている。バンドル調整とは、複数の画像間の対応点関係と 画像投影座標から、画像の位置と特徴点

次元座標を計算する手法であり、交会残差（再投影誤差）

の最小化により全てのパラメータ

次元座標

まずは、写真測量における調整計算を概観する。いま、共線条件式

が、下記の通りに観測方程式 として得られているとする。

1 1 1 1

1

( , , )

( , , )

m

n m n n

f x x l v

f x x l v

= +

= +







ここで、

は未知数（外部標定要素、特徴点

次元座標）、

は観測値（画像座標）、

1, , _n

v  v

は残差（交会残差）である。観測方程式が非線形であるため、非線形最小二乗法による最適 化が適用される。初期値（近似値）周辺でテイラー展開し、観測方程式を線形化すれば、

0 0 1 1

1 1 1 1 1

1 0 0

0 0

1 1

1 0 0

( , , )

( , , )

m m

m

n n

n m m n n

m

f f

f x x x x l v

x x

f f

f x x x x l v

x x

 

∂  ∂

+∂  ∆ + +∂  ∆ = +

 

∂  ∂

+∂  ∆ + +∂  ∆ = +

 



 

となる。ここで、

は近似値、

は補正値である。また、

、

0 0

0 =(x1, ,x_m)^T

X 

、

、

、

、

1 1

1 0 0

1 0 0

m

n n

m

f f

x x

f f

x x

∂   ∂  

∂  ∂  

    

 

=  

∂   ∂  

    

∂  ∂  

 

A



 



とおくと、線形化された観測方程式群は、

⋅ ∆ = +

A X L V (1.3)

と表現される。残差二乗和

の最小化（未知数

で微分し0 とおく）により、正規方程式は以下 となる（ここでは重み行列を省略する）。

T ∆ = T

(A A) X A L (1.4)

未知数

について解き、収束するまで更新を繰り返す。ここでの最適化手法は、ガウス・ニュート

ン法に基づくものである。

一方、コンピュータビジョンにおいては、共線条件式

に基づくことは写真測量と同じである（表 現法としては、同次座標により射影変換式としての表現があるが、ここでは省略する）。特徴点の画 像への再投影誤差（写真測量では交会残差）e

2 2

( )=

∑

∑

E X X X (1.5)

となる。目的関数の最小化のため、ニュートン法を適用する。初期値（近似値）

周辺で

次の項ま でテイラー展開すると、

0

0 0 0

( ) 1 ( )

2 d T

+ ∆ = + E(X )d ⋅ ∆ + ∆ ⋅ ⋅ ∆

E X X E(X ) X X H X X

X (1.6)

となる。ここで、H(X

2 2

1 1 0 1 0

0

2 2

1 0 0

( )

n

n n n

E E

x x x x

E E

x x x x

 ∂   ∂  

∂ ∂  ∂ ∂  

    

 

=  

 ∂   ∂  

    

∂ ∂  ∂ ∂  

 

H X



 



(1.7)

を表す。なお、この目的関数を最急降下法などの最適化手法で直接最小化する手法もあるが、調整計 算ではなく最適化法のため、精度評価は実施しないことが通常である。目的関数の最小化（未知数

で微分し

とおく）により、以下を得る。

0

( 0) d

⋅ ∆ = − E(X )d

H X X

X (1.8)

ここで、

0 2 _i ⁱ 2 _i ⁱ

j j j

e f

e e

X X X

∂ ∂ ∂

= ⋅ = − ⋅

∂

∑

∑

E(X )

(1.9)

であるため、

0 2 ^T

d

d = − ⋅

E(X )

A L

X (1.10)

となる。一般にヘッセ行列

下記の通り

次微分で近似する。

2 2

0 0

2 ⁱ 2 ( ^{i i} _i ⁱ ) 2 ( ^{i i} )

j k k j k i j j k j k j k

f f f f f f

E E

e e

x x x x x x x x x x x x

 ∂  = ∂ ∂  = − ∂ ⋅ ∂ = ∂ ⋅ ∂ − ⋅ ∂ ≈ ∂ ⋅ ∂

   

∂ ∂  ∂ ∂  ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂

   

∑ ∑ ∑

すなわち、

と近似することにより、

T ∆ = T

(A A) X A L (1.12)

が得られる。結果として、目的関数を直接解かず、ヘッセ行列の近似手法では、写真測量における正

7

規方程式と等価な計算を行っていることになる。

コンピュータビジョンでは、精度のよい近似値が得られないことも多く、先の目的関数のヘッセ行 列のランク落ちに対応するため、ダンピングファクター

を導入することにより、

0

( ( 0) ) d

λ d

+ ⋅ ⋅ ∆ = − E(X )

H X I X

X (1.13)

を用いたレーベンバーグ・マルカート法が適用されることも多い。本手法は、ガウス・ニュートン法

（

小）と最急降下法（

大）の中間手法となっている。ここでの、ダンピングファクターを不変と すれば、正則化項と解釈することができ、計量経済学等で用いられるリッジ回帰となる。

2.2

初期値設定の方法

共線条件式は非線形なため、その最適化においては、初期値（近似値）が必要となる。写真測量に おいては、通常、基準点等や

により初期値を設定するため、精度よく近似することが可能 である。また、Network design に基づき、撮影状況（画像取得の幾何学的構成等）などもコントロー ルしている。一方、コンピュータビジョンでは、自動処理を前提としているため、基準点等を用いず、

特徴点の対応関係から基本行列を算定し初期値を設定する。なお、基本行列の算定は、写真測量にお ける相互標定と数学的に等価である。この特徴点の対応関係を求めるために、特徴点の抽出・追跡手 法が発展してきた。代表的な手法としては、SIFT、

等が挙げられる。また、撮影状況 をコントロールしないため、外れ値除去手法の発展も重要である。そのため、近年では、写真測量に おいても、コンピュータビジョンで開発された特徴点の抽出・追跡手法が多く適用されている。

8

第

章 画像ベース手法の屋外での適用可能性の検証

3.1

特徴点抽出・追跡手法と屋外での適用可能性

画像ベース手法の屋外での適用可能性は、特徴点の抽出・追跡の性能に依存する。前述の通り、特 徴点抽出・追跡手法は、コンピュータビジョンの分野における発展が顕著である。代表的な手法の中 で、その性能および計算量から

可能性を検討する。

SURF

は、

では、積

分画像作成のためにHessian-Laplace detectorを近似した

積分画像により、

高速計算を可能にしている。ここでは、スケールの異なる複数のフィルタを利用する。また、ある半 径に含まれる点に対して、ノルムを計算し、最大ノルムとその方向を保存する。これらの特徴量によ り、拡大・縮小および回転の変化に対しても頑健なフィルタとなっている。最終的には、画像は

4

のブロックに分割され、各ブロックに対して、勾配とその強度に関して、合計

次元の値を表すベ クトルとして特徴量を計算する。詳細は、参考文献を参照されたい。

屋内・屋外における、

の適用結果を表

に示す。

表

屋内・屋外における

適用結果 屋内 屋外 抽出・追跡された特徴点数

フレーム数

屋外では、屋内と比較して特徴点数が減少するが、標定・3 次元復元のためには十分な数を確保でき る可能性があることと、誤対応があるため、それらを除外する工夫が必要であることが確認された。

3.2

特徴点追跡手法の改良

特徴点抽出においては、コーナーやエッジのような周囲との輝度差の激しい点において、特徴点の 強度変化を表す特徴量ベクトルをもつ。そこで、特徴点を動画像のフレーム間で追跡することにより 対応点を求めるよう改良する。

提案手法では動画像の各フレームから

アルゴリズムにより特徴点を抽出し、特徴量を参照す ることにより特徴点の追跡・対応付けを行う。時系列上で連続したフレーム

の

枚において特 徴点追跡を行う場合、

9

(1)

まずI

上の特徴点p

の特徴量に最も類似度の高い特徴量を持つp

をフレームI

から探索する。

(2)

その後も同様に

から

へと順々に一方方向で特徴点の探索を続ける。

(3)

最後に

で到達した

の対応点を

から探索し、I

上のその点を

(4) pi = pi’ならばその点をn

枚に共通して存在する特徴点候補として登録する。

(5)

さらに、誤対応による外れ値を除外するために、

と

の座標値から算出される特徴点の移動 量[pix]を、他の特徴点の移動量と比較し、閾値処理を行う。最終的に残った特徴点を

間で の正しい対応関係のものとする。

上記の処理を行っても、誤対応が多数残ることが確認された。誤対応の原因としては、特徴点の対 応付けを特徴量の比較のみで行っているためであると考えられる。そこで、本研究では、さらに、

RANSAC(Random Sample Consensus)アルゴリズム(Fischler and Bolles, 1981)も併用する。RANSAC

では、

ランダムに対応点の組を選択して、パラメータを推定し、残りのデータからその妥当性を評価する。

しかし、RANSAC を用いた誤対応除去は、正しい対応点に比べ、誤対応の数が多い場合に誤対応の 特徴点に引っ張られ、正しい対応点が求められないという問題が発生する。そのため、前フレームに おけるカメラの動きから、次フレームの動きがある程度予測できると仮定し、明らかな特徴点の誤対 応を予め除去した後、RANSAC を適用することにより対応点を求める．

3.3

適用

提案手法の有効性を確認するために、

画像間での双方向探索の結果、 特徴点追跡のみによる結果、

提案手法の適用結果を、図

において比較する。それぞれ

特徴点を求める際の閾値は一定で あり、フレーム間隔は

フレームである（

関係が成立した点は

点であったが、図からもわかるように多くが誤対応である。次に、

追跡によるマッチングでは対応数が

点と大幅に減少しているものの、多くの誤対応が除去された ことがわかる。しかし、図中に赤く強調してあるように、まだ誤対応は残っている状態であり、これ は、前述の通り、特徴量のみによる追跡の限界のためであると考えられる。それに対して(c)提案手法 では、そのような誤対応も排除されていることが観察される。

10

(a)

双方向探索による結果

(b)

特徴点の追跡結果のみによる場合

(c)

移動量による閾値設定を設けた場合 図

改良手法の適用結果

11

第

章 他センサのデータを付加した手法の開発

4.1

提案手法の枠組み

本章では、カメラに対する他センサとして、モバイル機器のセンサを併用した自己位置推定手法の 開発を行う。モバイル機器には、ジャイロセンサ・地磁気センサ・加速度センサ・GPS (総称して

POS :Position and Orientation System

と呼ぶことにする)が搭載されている。これらのセンサを用いて、

モバイル機器の位置・姿勢を求める自己位置推定手法を開発する。この自己位置推定手法では、それ らのセンサの精度に依存することになるが、モバイル機器に搭載された

は低性能であるため、

自己位置推定の精度も十分ではない。

一方で、既に議論を行ってきた、画像センサを用いた自己位置推定手法（標定手法）が存在する。

この手法では、位置や姿勢の初期値が必須となることや、カメラの実スケールでの位置を推定するた めに、カメラの移動量を与えなくてはならないといった課題が存在する。

そこで、

と画像センサに基づく自己位置推定手法を統合することにより、両センサを補完した 自己位置推定が行えることが期待される。具体的には、まず

を用いてカメラの位置・姿勢を求 め、その後フィルタリングによる誤差調整を行なった位置・姿勢を求める。その作業と並行して、動 画像中から特徴点の抽出・追跡・マッチングを行う。次に得られた特徴点ペアに対して、

から得 た位置・姿勢を用いて特徴点の

次元復元を行う。ここで得られた、カメラの位置・姿勢、および対 象物の3 次元座標を初期値とし、 バンドル調整を行うことによって各パラメータを更新する （図4-1） 。

図

提案手法の概要

次節以降、モバイル機器搭載の

をすべて組み合わせて行うフィルタリング手法の開発、POS から取得される位置・姿勢と画像センサから取得された画像上の特徴点の組み合わせによるバンドル 調整に関して論ずる。

フィルタリングによる誤差調整

画像入力

バンドル調整を用いた位置・姿勢の最適化

三次元復元

組み合わせによる位置・姿勢の推定

位置・姿勢

三次元座標 特徴点の

センサ 加速度

センサ GPS

12

4.2 POS

データのフィルタリング手法

4.2.1

フィルタリングの概要

本章では、モバイル機器として

を用いる。ここでは、モバイル機器の重心を、そのモバイ ル機器座標系の原点として考える。モバイル機器座標系としては、原点から画面垂直方向に

軸をと り、

軸との垂直面上でカメラの設置されている方向を

軸とし、右手座標系として

軸を定義する。

x, y, z

軸の周りの回転を、 写真測量における定義に従い

とする。また、 絶対座標系として、

モバイル機器の原点を初期位置とし、重力方向に

軸、磁北方向に

軸とし、モバイル機器の座標系 が絶対座標系と一致したときの姿勢を

とする。

POS

から取得されるデータを表

に示す。

表

から取得されるデータ

取得データ

ジャイロセンサ モバイル機器座標系における

軸・

軸・

軸周りの角速度

(ω_g, ϕ_g, κ_g)

地磁気センサ 右手座標系において、北を

軸の正方向、反重力方向を

軸の正方向とした場合 にモバイル機器が傾いていないとした場合のヨー角(

加速度センサ モバイル機器座標系における

軸・

軸・

軸方向の動加速度＋重力加速度

(x_a, y_a, z_a)

GPS WGS-84

座標系における経度[°]・緯度[°]・楕円体高

(E, N, h)

図

に、

データのフィルタリング手法の概要を示す。加速度・地磁気センサを用いて初期姿 勢を求めた後、ジャイロセンサを用いて運動中の姿勢を求める。次に、ジャイロセンサのドリフト誤 差を補正するために、加速度・地磁気センサの情報から得られた姿勢を併用し、カルマンフィルタに より最終的な姿勢の推定値とする。また、ここで推定された姿勢と加速度センサから得られた加速度 の情報を用いて絶対座標系における加速度を求め、その加速度と

の情報から求める直前の速度 の情報と組み合わせることにより、

から直接得られる位置の誤差補正を行う。ここでのデータの 組み合わせにおいても、カルマンフィルタを適用する。以下、これらの一連の計算を説明する。

4.2.2

姿勢の計算手法

まず、加速度センサおよび地磁気センサからの姿勢の計算方法を示す

。静止時には、

加速度センサは重力加速度

のみを検出する。この時、各軸方向の姿勢（回転角）を

とす ると、それらと加速度との関係は次式で表現される。

13

図

フィルタリング手法の概要

sin cos sin cos cos

a a

a a a

a a a

x g

y g

z g

ϕ

ϕ ω

ϕ ω

   − 

  = 

   

   

   







(4.1)

この式から、姿勢を求めれば、

1 1

2 2

tan ^a, tan ^a

a a

a a a

y x

z y z

ω = ⁻ ϕ = ^{− −}

+

 

   (4.2)

が得られる。また、ヨー角については、絶対座標系におけるものではないため、次式により絶対座標 系における回転角に変換する。

m ti

κ = −π κ (4.3)

これらの値は、初期姿勢としても用いることとする。

加速度センサと地磁気センサから算出した姿勢とジャイロセンサから得られる角速度を用いて運 動中の姿勢を表現するための回転行列を算出する方法を示す。各軸周りの姿勢をあらわすオイラー角 の時間微分( , ,

cos sin sin cos sin

1 0 cos cos sin cos

cos 0 sin cos

g g

g g

g g

B

ω ϕ ω ϕ ω ϕ κ κ

ϕ ω ϕ ω ϕ ϕ ϕ

κ ϕ ω ω ω ω

   

   

   

 =  −  =  

   

   

   

       

  

  

  

(4.4)

これを時間

により積分することにより、姿勢を求めることができる。

a a m

dt

ω ω ω

ϕ ϕ ϕ

κ κ κ

 

   

 

 =   +  

   

 

   

     

∫







(4.5)

これらの計算値には誤差が含まれるため、カルマンフィルタにより誤差の補正を行う。ここでは、

ジャイロセンサ

地磁気センサ

加速度センサ

GPS POS

1

1

1

1

姿勢R

位置T

14

モバイル機器が線形運動することを仮定している。カルマンフィルタにおける状態方程式、および観 測方程式は、以下の通りである。

xt = Ft xt-1 + Gt vt, yt = Ht xt + wt (4.6)

ここで、x

はそれぞれ状態ベクトル、観測ベクトル、および遷移行列であり、下記の通り定義 する。

1 0 0

0 1 0 B

0 0 1

, , F

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

a

a t

m

t t t

g g

g g

g g

ω ω

ϕ ϕ

κ κ

κ κ

ϕ ϕ

ω ω

     

     

     

     

=  =  = 

     

     

     

 

     

   

x y

 

 

 

(4.7)

ここで、

はオイラー角の時間微分のものと同じである。また、システムノイズ

イズ

は単位行列に設定する。カルマンフィルタに基づけば、下記の予測・フ

ィルタの漸化式が得られる。ここで、V は状態ベクトルの分散共分散行列をあらわし、添え字の

は、時刻

から

への予測分布を示すものとする。

予測：

Vt|t-1 = Ft Vt-1|t-1F^Tt + Gt Qt GtT (4.9)

フィルタ：

xt|t = xt|t-1 + Kt (yt - Ht xt|t-1) (4.11)

Vt|t = Vt|t-1 - Kt Ht Vt|t-1 (4.12)

最終的に得られる状態ベクトル

が姿勢の結果となる。

4.2.3

位置の計算手法

次に、位置に関する計算方法を示す。まず、

により得られるデータは、経度、緯度、楕円体高 であるため、これを平面直角座標系に変換し

による計 測値をそのまま用いるものとする。

運動中の位置に関しては、

、加速度センサを用いる。まず、

により得られた絶対座標に加 え、その値から、時刻

における速度を下記の通り計算する（Δ

は時間差）。

1 2

1 2

1 2

t 1 t t

t t t

t t t

X X X

Y Y Y

Z t Z Z

− −

− −

− −

   − 

 =  − 

  ∆  

   − 

 







(4.13)

また、加速度センサから得られる加速度を絶対座標に変換すれば、重力加速度も考慮し、

0 0

t a

t a

t a

X x

Y R R R y

Z z g

κ ϕ ω

      

 

 = −    − 

     

 

         

 

 

 

 

(4.14)

により計算可能である。これらを用いれば、時刻

における位置は、下記の通りになる。

15