MEMS センサを用いた INS/GPS 複合航法システム

成岡優 2004/11

目次

第 1 章序論 1 1.1 はじめに . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 仕様および検討 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.3 INS/GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

第 2 章 Kalman Filter 6 2.1 最小二乗法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 2.2 重み付き最小二乗法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.3 再帰的重み付き最小二乗法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.4 離散系 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.5 離散系 Kalman Filter の連続系への適用 . . . . . . . . . . . . . . . . . . . . . . . . 13 2.6 UD 分解 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

第 3 章 System Design 14 3.1 運動方程式 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 3.2 運動方程式の線形化 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 3.3 観測方程式 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.4 INS/GPS アルゴリズム . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

第 4 章数値シミュレーション 30 4.1 方法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 4.2 結果 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 4.3 考察 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

第 5 章実機試験 42 5.1 機材 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 5.2 方法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 5.3 結果 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

第 6 章まとめ 43 付録 A Quaternion Algebra 44

A.1 Quaternion の定義 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 A.2 共役 Quaternion の定義 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 A.3 Quaternion のスカラー積 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 A.4 Quaternion 間の加算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 A.5 Quaternion 間の乗算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 A.6 Quaternion による 3 次元ベクトルの回転 . . . . . . . . . . . . . . . . . . . . . . . 47 A.7 Quaternion による回転の合成 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 A.8 Quaternion の時間微分 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 A.9 回転 Quaternion と Direction Cosine Matrix の関係 . . . . . . . . . . . . . . . . . . 50

付録 B Coordinate Systems 51 B.1 Earth-Centered Inertial Frame (i-Frame) . . . . . . . . . . . . . . . . . . . . . . . . 51 B.2 Earth-Centered, Earth-Fixed Frame (e-Frame) . . . . . . . . . . . . . . . . . . . . . 51 B.3 Local Geodetic Frame (g-Frame) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 B.4 Navigation Frame (Wander Azimuth Frame) (n-Frame) . . . . . . . . . . . . . . . . 52 B.5 Body Frame (b-Frame) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 B.6 座標変換の表記 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 B.7 i-Frame と e-Frame の関係 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 B.8 e-Frame と g-Frame の関係 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 B.9 g-Frame と n-Frame の関係 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55 B.10 e-Frame と n-Frame の関係 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 B.11 n-Frame と b-Frame の関係 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

付録 C Earth Model(WGS-84) 59 C.1 楕円モデル . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 C.2 南北方向ならびに東西方向の曲率半径 . . . . . . . . . . . . . . . . . . . . . . . . . 60 C.3 重力 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 C.4 WGS-84 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

付録 D Allan Variance 63 D.1 Allan Variance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

付録 E ソースコード 64

付録 F 実機試験基板 65 F.1 回路図 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 F.2 ガーバ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

F.3  参考文献 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 参考文献  66

iii

図目次

3.1 向心力 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 3.2 Update 時 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.3 Correct 時 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.1 慣性センサモデル 1 の処理過程図 . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 4.2 慣性センサモデル 2 におけるジャイロの処理過程図 . . . . . . . . . . . . . . . . . 33 4.3 ADXRS150 の Allan Variance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 4.4 慣性センサモデル 2 のジャイロの Allan Variance . . . . . . . . . . . . . . . . . . . 34 4.5 慣性センサモデル 1 のジャイロの Allan Variance . . . . . . . . . . . . . . . . . . . 35 4.6 水平面上履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 4.7 高度履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 4.8 北方向速度履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 4.9 東方向速度履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 4.10 重力方向速度履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.11 ヨー履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 4.12 ピッチ履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 4.13 ロール履歴 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 4.14 5 周 (10 分間) の運用を仮定した場合の水平面上履歴 . . . . . . . . . . . . . . . . . 41 4.15 5 周 (10 分間) の運用を仮定した場合のロール履歴 . . . . . . . . . . . . . . . . . . 41

B.1 e-Frame と g-Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 B.2 g-Frame と n-Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 B.3 n-Frame と b-Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

C.1 地球モデル . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

表目次

1.1 代表的な位置・姿勢推定機構 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 本研究で提案する位置・姿勢推定機構の仕様 . . . . . . . . . . . . . . . . . . . . . 4 A.1 座標変換の方法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 B.1 座標変換の表記 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 C.1 WGS-84 の定数 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

第 1 章

序論

1.1 はじめに

近年、航空機の多方面への応用という観点から、従来では考えられなかった様々な運用環境においても航空機が活躍することが望まれている。その一つの例として、災害時における被害状況の把握というミッションがあげられる。このミッションでは、現地に急行しなければならないという迅速性が求められるだけでなく、時には自らに危険が及ぶ可能性があるハイリスクな情報収集活動である。従って、パイロットといった人為的な要素が重大な問題となる。加えて、観測点が多数になればなるほど、より有機的な情報統合が行えるといった理由から、多数の機体を導入するほど大きな成果があげられると考えられる。すなわち、機体の価格的なパフォーマンスも重要になる。

そのような中で、パイロットを必要としない無人航空機 (Unmanned Aero Vehicle、 UAV) は、航空機の構造から人為的な要素を排除することができるという理由から、航空機の活動の場を最大限に広げるものとして期待され、急速に開発が進められている。

現在開発されている UAV は、無人であるというその特徴を最大限生かし、現存の航空機と比べて遥かに小型かつ軽量、簡素な構造で安価な形態をとるものが多い。中には重量が百 g 程度、主翼スパンが十 cm 程度の超々小型のものも存在する。そして、開発途上にある UAV の多くは、その操縦方法として R/C、すなわちラジコン (Radio Contorl) を採用している。

UAV をより活用するためには、それがラジコンを卒業し、自律的に賢く航行する、すなわち知動化が必要である。その為の第一歩として、誘導および制御に必要となる、自機の位置・姿勢情報を精度よく推定できなければならない。しかしながら UAV には、現代の飛行機に搭載される高度な位置・姿勢推定機構を搭載するには、重量・サイズおよびコストの面から至っていない。例えば、現在の旅客機に搭載される位置・姿勢推定機器の 1 つである、慣性航法システム (Inertial Navigation System、 INS) は重量にして数十 kg、価格にして数千万円と、 UAV に搭載できるようなものではない。

UAV の知動化という観点からすれば、自機の位置・姿勢を精度よく安価に推定できることは UAV の発展に対して最重要課題であると思われる。そこで本研究では、最終的に UAV に搭載することを目的とした小型、軽量、安価な位置・姿勢推定機構の提案を行う。また、実際に機器を製作および試験することを通して、本研究で提案したシステムの有効性について議論する。

1.2 仕様および検討

現在の航空機において代表的な位置・姿勢推定機構は表 (1.1) に示すとおりである。以下、これらの機構について詳細をみていくことにする。

表 1.1 代表的な位置・姿勢推定機構

推定機構名{INS(ジンバル \atop 方式)} {INS(ストラッ \atop プダウン方式)} {GPS(単体測 \atop 位)} DGPS  磁気コンパス
位置の推定可可可可不可
姿勢の推定可可{速度方向のみ \atop 可}{可 (複数使用 \atop により可)} 可
必要となる内部機構{慣性センサ・ \atop ジンバル}{慣性センサ・ \atop 高性能計算機}{アンテナ・処 \atop 理ユニット}{アンテナ・処 \atop 理ユニット}{磁気センサ・ \atop 処理ユニット}
必要となる外部機構なしなし  GPS 電波{GPS 電波・補 \atop 正電波}地磁気
精度 (瞬間)  ジンバル・慣 性センサの精 度に依存{慣性センサの \atop 精度に依存}測位 GPS 数に 依存 (数 m 程 度) 測位 GPS 数に 依存 (数 cm 程 度) 地磁気に依存

精度 (時間依存)  {時間がたつご \atop とに悪化}{時間がたつご \atop とに悪化}時間依存なし時間依存なし時間依存なし
データ更新周期  10Hz 以上可 10Hz 以上可{最大数 Hz 程 \atop 度}{最大数 Hz 程 \atop 度}{最大数 Hz 程 \atop 度}

1. Inatial Navigation System (INS) 慣性サンセを用いて位置・姿勢推定をおこなう機構を Inatial Navigation System (INS) という。慣性センサは、慣性座標に対する加速度を感知する加速度計、および角速度を検出するジャイロの 2 種類で構成されている。加速度を数値演算によって積分することにより速度が、また速度を積分することにより位置がわかる。加えて角速度を積分することにより姿勢がわかる。このような理論を応用したのが INS である。 INS は連続的にデータが取得可能な慣性センサの情報のみを利用するため、更新周期が短く途切れのない推定情報を生成することが可能であることが大きな利点である。 INS には慣性センサをとりつける方式から次の 2 種類に大別される。

• ジンバル方式ジンバルと呼ばれる常に水平面を維持する機構上に慣性センサをとりつける方式をジンバル方式と呼ぶ。ジンバル上に置かれた慣性センサが加速度・および角速度を検出するが、それを打ち消すようにジンバルをアクチュエータによって動かすことによって水平を常に維持する仕組みになっている。ジンバルが常に水平面を保つことから、積分は単純に時間間隔を区切って足し算を行えばよいだけであるので、高度な演算装置は必要がない。しかし、ジンバルは機械的な構造であるため、ジンバル方式の INS を小さくつくることには限度がある。またジンバルの機械的誤差が存在するために、ある程度以上の精度の向上は難しい。

• ストラップダウン方式機体に直接慣性センサをとりつける方式をストラップダウン方式という。ジンバル方式と違い、ジンバルのような機械的な構造を必要としない。慣性センサは機体の軸にそった加速度および角速度を検出することになるため、速度や位置、姿勢を求めるためには座標変換を行う必要がある。そのため演算負荷が高く、ジンバル方式よりも高度な演算装置を必要とする。機械的な構造誤差はセンサの取り付け精度程度であるので、これは十分に補正が可能であり、ジンバル方式よりもよい精度をだすことができる。

どちらの方式をとるにしても INS で最重要な部品は慣性センサである。慣性センサは時間が経過することにゼロ点がランダムに移動するというランダムドリフトという性質をもっているため、 INS 単体での使用では時間が経つごとに精度が劣化する。ランダムドリフトが少ない慣性センサほど精度がよいセンサとなるのだが、例えば民間航空機で使用されている INS 用のジャイロはランダムドリフトが 0.001^{∘}/hr 以下の Ring Laser Gyro(RLG) と呼ばれる特殊なジャイロを使用している。精度がよい慣性センサほど価格が高くなるのだが、先に述べた RLG では数千万円/個価格である。一方、カーナビなどに使用される小型で価格も数万円程度の慣性センサが最近は市場に流通するようになった。 Micro Electro Mechanical Systems(MEMS) と呼ばれるテクノロジーを応用した製品であり、 1 枚のシリコンウェハースの上に微細加工技術によって慣性力を検出する機械的部分とそれを電気信号に変換する部分を形成したものである。これらのセンサは先の RLG に精度は遠く及ばないが、これからの技術革新によってより精度は向上するものと思われる。

2. GPS 地球上での航行を考えるのであれば Global Positioning System(GPS) を位置・姿勢推定機構として利用することができる。 GPS は地球軌道上を回る複数の軌道が既知の GPS 衛星からの電波を受信し、その電波の位相を比較することによって三角測量を行い現在の位置情報を推定する機構である。位置が断続的にわかれば、それを時間間隔で割ることによって速度情報もえることができる。 GPS は三角測量によって位置を検出することから、誤差数 m 以下の高い精度を出すことが可能であり、衛星数が多ければ多いほどこの精度はあがる。しかしながら電波を受信しなければならないという性質から情報の更新は最大でも数 Hz 程度、また電波が届かない地域では利用することができない。 GPS は電波の処理形態から単体測位と、それを発展させた DGPS にわけることができる。

• 単体測位単体測位とは、 GPS 衛星が放出する電波を利用するのみの測位方式である。電波は電離層や電波の反射 (マルチパス) から影響を受けるため、正しい測量を行えないことがある。従って単体測位ではある程度以上の精度向上は見込めない。

• DGPS 単体測位では得られない電波の誤差情報を他の手段によって取得し測位に反映させる方式がある。これが Differntial GPS(DGPS) と呼ばれる方式で、移動体においても誤差数 cm 程度という非常に高い精度を出すことが可能となる。この高い精度を利用することによって、複数台の GPS から姿勢を判定することも可能である。

GPS は民生品として市場で流通しているが数多く存在し、中には携帯電話に内蔵できるくらい小型なものも存在する。これらは非常に安価 (数万円) で入手することができる。

3. 磁気コンパス地球には磁場が存在するため、それを感知することによって姿勢情報を推定することが可能である。この機構を磁気コンパスという。地磁気は場所によって不安定であるため、ある程度以上の精度向上は望めない。磁気コンパスは磁気を感知する半導体センサを利用することによって小型・軽量なものが存在する。

ここで UAV に搭載することを目的とした位置・姿勢推定機構は、前節で述べたとおり以下の条件を満たしていなければならない。

1. できる限り精度よく自機の位置・姿勢を推定できること。

2. UAV に搭載できる重量・サイズであること。すなわちできる限り軽く、小さいこと。

3. 機構が安価であること。

この条件は相反するものであるため、トレードオフを行い仕様を特定する必要がある。そこで、本研究で提案する位置・姿勢推定機構は表 (1.2) の仕様を満足できるよう設計するものとする。

表 1.2 本研究で提案する位置・姿勢推定機構の仕様

想定する機体スパン長 1m〜2m 程度の UAV
使用環境地球地表面付近での使用
精度数分間の運用で最大誤差 10m 以内
重さ  100g 程度
サイズ  W 10 × D 10 × H 10 cm^{3} 以内
価格  10 万円程度

この仕様から本研究では次の位置・姿勢推定機構を導入することにした。 • INS

先にも述べたカーナビなどで使われている小型・安価である MEMS の慣性センサを利用した INS を導入することにする。 MEMS 慣性センサは精度は期待できないが、連続的に推定を行えることは魅力である。

• GPS INS を長時間使用すると慣性センサのランダムドリフトにより精度が悪化することは先にも述べた。そこでそれを補正する目的で GPS を導入する。 GPS では手軽さを考え、単体測位を想定する。

1.3 INS/GPS

前節で UAV 用の位置・姿勢推定機構として INS と GPS を導入することを述べたが、両者の長所を生かし、短所を互いに補うよう効果的な方法を導入したい。そのためには信頼がおける、つまり最も確からしい情報を組み合わせることが必要になる。これを実現するのが Kalman Filter と呼ばれる方法で、複数の情報から最も確からしい情報を抽出することが可能である。この『統合する』という意味をこめて本研究で提案する推定機構を INS/GPS と称することにする。

Kalman Filter の詳細については第 2 章で説明するが、 INS と GPS を Kalman Filter を統合するには次の 2 つの方法が存在する。

• Loose Coupling INS を主として位置・姿勢情報を連続的に推定し、 GPS から出力される位置・速度情報によって INS を補正する方法を Loose Coupling と呼ぶ。この場合、 GPS は単にリファレンスとしてしか機能していない。

• Tight Coupling INS と GPS、どちらに対しても補正をかける方法を Tight Coupling という。 Loose Coupling に比較して精度の向上と GPS の最大限の活用 (例えば測位衛星数が不足しても推定を行うことが可能になる) が見込めるが、アルゴリズムは複雑になる。

今回は GPS は常に使用可能であると考え、アルゴリズムがより単純な Loose Coupling を採用することにした。アルゴリズムの詳細については第 3 章で示す。

そして第 3 章で構成したアルゴリズムを元に第 4 章では数値シミュレーション、第 5 章では試験を行い、その結果ならびに考察を第 6 章で行う。

第 2 章

Kalman Filter

INS/GPS、すなわち INS と GPS からより確からしい情報を引き出す方法として本研究では Kalman Filter を用いる。そこで本章では Kalman Filter の導出過程を説明する。

同じ対象を観測して複数の観測データが得られたとき、そのデータから最も確からしい値を求めるには、全ての平均値をとることである。これは最小二乗法としてよく知られる方法であり、対象から得られる情報が真なる値 (真値) と観測ごとに変化するノイズ成分にわけられる経験的事実を適用したものである。いうなれば Kalman Filter とは、その最小二乗法を時間にそって連続的に適用したものである。ゆえに本章では最小二乗法を起点として導出過程を示す。

2.1 最小二乗法

\underline{x}なる定数を求めるために観測を行う。その観測値を\underline{z} とすれば、\underline{x}と\underline{z} の関係は次の方程式であらわされる。

\underline{z}= H\underline{x}+ \underline{v}  (2.1.1)

これを観測方程式と呼ぶ。ここで\underline{v}は誤差であり、平均 0、分散 R の正規白色ノイズを想定することにする。すなわち

E[\underline{v}]= 0, E[\underline{v}\underline{v}^{T}] = R  (2.1.2)

\underline{x}を\underline{z} から推定するためには、次の評価関数 J_{LS} を最小にするようにすればよい。

J_{LS} = (\underline{z}− H\underline{x})^{T}(\underline{z}− H\underline{x})  (2.1.3)

展開して

J_{LS} = \underline{z}^{T}\underline{z}− \underline{z}^{T}H\underline{x}− \underline{x}^{T}H^{T}\underline{z}+ \underline{x}^{T}H^{T}H\underline{x}  (2.1.4)

\underline{x}で偏微分して

\frac{∂J_{LS}}{∂\underline{x}} = \underline{0}^{T}− \underline{z}^{T}H − (H^{T}\underline{z})^{T}+ (H^{T}H\underline{x})^{T}+ \underline{x}^{T}H^{T}H
= −2(\underline{z}^{T}H − \underline{x}^{T}H^{T}H)

(2.1.5)

さらに\underline{x}で偏微分して

\frac{∂^{2}J_{LS}}{∂\underline{x}^{2}} = 2(H^{T}H)  (2.1.6)

これより\frac{∂^{2}J_{LS}}{∂\underline{x}^{2}} の要素は正となることから、\frac{∂J_{LS}}{∂\underline{x}} = \underline{0} となるように\underline{x} を決めれば J_{LS} が最小になる。すなわち、最も確からしい\underline{x}をˆ\underline{x}と書けば (2.1.5) より

\underline{z}^{T}H = ˆ \underline{x}^{T}H^{T}H  (2.1.7)

H^{T}H に逆行列が存在するとして、

ˆ\underline{x}= (H^{T}H)^{−1} H^{T}\underline{z}  (2.1.8)

となる。この推定値ˆ\underline{x}にはいくつかの重要な性質が存在する。

1. 推定誤差 (\underline{ϵ})

\begin{aligned}\underline{ϵ} & ≡ \underline{x}− ˆ \underline{x} \\ & = \underline{x}− (H^{T}H)^{−1} H^{T}\underline{z} \\ & = \underline{x}− (H^{T}H)^{−1} H^{T}(H\underline{x}+ \underline{v}) \\ & = −(H^{T}H)^{−1} H^{T}\underline{v} \end{aligned}  (2.1.9)

2. 残差 (計測値と推定値の差、\underline{ν})

\begin{aligned}\underline{ν} & ≡ \underline{z}− ˆ \underline{z} \\ & = H (\underline{x}− ˆ \underline{x}) + \underline{v} \\ & = H\underline{ϵ}+ \underline{v} \\ & = ( I − H (H^{T}H)^{−1} H^{T}) \underline{v} \end{aligned}  (2.1.10)

3. 推定誤差平均

\begin{matrix}E[\underline{ϵ}] & = E[−(HH^{T})^{−1} H^{T}\underline{v}] \\ = −(HH^{T})^{−1} H^{T}E[\underline{v}] \\ = \underline{0} \end{matrix}  (2.1.11)

4. 推定誤差共分散 (P)

P≡ E[\underline{ϵ}\underline{ϵ}^{T}]

= E[(\underline{x}− ˆ \underline{x}) (\underline{x}− ˆ \underline{x}) ^{T}]

= E[( (H^{T}H)^{−1} H^{T}\underline{v}) ( (H^{T}H)^{−1} H^{T}\underline{v})^{T}]

= (H^{T}H)^{−1} H^{T}E[\underline{v}\underline{v}^{T}]H ( (H^{T}H)^{−1} )^{T}

= (H^{T}H)^{−1} H^{T}RH ( (H^{T}H)^{−1} )^{T} (2.1.12)

2.2 重み付き最小二乗法

前節の最小二乗法を拡張して、ここでは重み付き最小二乗法を考える。『重み付き』とは観測ごとに確からしさが異なることに対応する。

ここでは観測方程式は (2.1.1) と同じである。このとき\underline{x}を\underline{z} から推定するためには、次の評価関数 J_{WLS} を最小にするようにすればよい。ここで W は『重み』をあらわす対称行列 W^{T}= W である。

J_{WLS}= (\underline{z}− H\underline{x})^{T}W (\underline{z}− H\underline{x})  (2.2.1)

展開して

J_{LWS}= \underline{z}^{T}W \underline{z}− \underline{x}^{T}H^{T}W \underline{z}− \underline{x}^{T}W H\underline{x}+ \underline{x}^{T}H^{T}W H\underline{x}  (2.2.2)

\underline{x}で偏微分すれば

\frac{∂J}{∂\underline{x}} = −\underline{z}^{T}W H − \underline{z}^{T}W H + \underline{x}^{T}H^{T}W H + \underline{x}^{T}H^{T}W H
= −2(\underline{z}^{T}W H − \underline{x}^{T}H^{T}W H)

(2.2.3)

よって評価関数 J_{WLS} を最小または最大にする\underline{x}をˆ\underline{x}とすれば

\underline{z}^{T}W H = ˆ \underline{x}^{T}H^{T}W H  (2.2.4)

すなわち

ˆ\underline{x}= (H^{T}W H)^{−1} H^{T}W \underline{z}  (2.2.5)

ここで W → R^{−1} なる置き換えを適用すると、重み付き最小二乗法における性質は次のとおり導かれる。

1. 推定誤差 (\underline{ϵ})

\begin{aligned}\underline{ϵ} & ≡ \underline{x}− ˆ \underline{x} \\ & = \underline{x}− (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} \underline{z} \\ & = \underline{x}− (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} (H\underline{x}+ \underline{v}) \\ & = −(H^{T}R^{−1} H)^{−1} H^{T}R^{−1} \underline{v} \end{aligned}  (2.2.6)

2. 推定誤差共分散 (P)

P≡ E [( (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} \underline{v})( (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} \underline{v})^{T}]

= E [( (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} \underline{v})( \underline{v}^{T}R^{−1} H (H^{T}R^{−1} H)^{−1} )]

= (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} E [\underline{v}\underline{v}^{T}]R^{−1} H (H^{T}R^{−1} H)^{−1} = (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} RR^{−1} H (H^{T}R^{−1} H)^{−1} = (H^{T}R^{−1} H)^{−1} (H^{T}R^{−1} H)(H^{T}R^{−1} H)^{−1} = (H^{T}R^{−1} H)^{−1} (2.2.7)

2.3 再帰的重み付き最小二乗法

前節の重み付き最小二乗法で、観測方程式は (2.1.1) であったが、これを成分ごとに書き下すと以下のようになる。

( z_{1} ; z_{2} ; ⋮ ; z_{m}) = [ \begin{matrix}h_{11} & h_{12} & ... & h_{1n} \\ h_{21} & h_{22} & ... & h_{2n} \\ ⋮ & ⋮ & & ⋮ \\ h_{m1} & h_{m2} & ... & h_{mn} \end{matrix}] ( x_{1} ; x_{2} ; ⋮ ; x_{m}) + ( v_{1} ; v_{2} ; ⋮ ; v_{m})  (2.3.1)

最も確からしい推定値をˆ\underline{x}_{m} 書くと、これは (2.2.5) より (すでに W → R^{−1} の置き換えをしたものとして)

ˆ\underline{x}_{m}= (H^{T}R^{−1} H)^{−1} H^{T}R^{−1} \underline{z}  (2.3.2)

また、推定誤差共分散を P_{m} と書くと、 (2.2.7) より

P_{m}= (H^{T}R^{−1} H)^{−1}  (2.3.3)

P_{m} を用いてˆ\underline{x}_{m} を書けば

ˆ\underline{x}_{m}= P_{m}H^{T}R^{−1} \underline{z}  (2.3.4)

今、あらたに観測値 z_{m+1} が得られたとすると

( \underline{z} ; z_{m+1}) = [ \begin{matrix}H \\ \underline{h} & T \end{matrix}] \underline{x}+ ( \underline{v} ; v_{m+1})  (2.3.5)

新しい観測値が得られたことによって、推定値ˆ\underline{x}_{m+1} は、観測値が得られる前の推定値ˆ\underline{x}_{m} に対して

ˆ\underline{x}_{m+1} = ˆ \underline{x}_{m}+ Δ\underline{x}  (2.3.6)

で求められれば、再帰的に次々とあらたな観測値が得られることによって推定値を更新していくことが可能となる。

ところでˆ\underline{x}_{m+1} は、評価関数 (2.2.1) を書き直して次の評価関数 J_{RWLS}を最小にするものである。

J_{RWLS}= ( ( \underline{z} ; z_{m+1}) − ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) x_{m+1}) ^{T}[ \begin{matrix}R^{−1} & \underline{0} \\ \underline{0}^{T} & r^{−1} \end{matrix} ] ( ( \underline{z} ; z_{m+1}) − ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) x_{m+1})  (2.3.7)

ここで『重み』に相当する行列 [ \begin{matrix}R^{−1} & \underline{0} \\ \underline{0}^{T} & r^{−1} \end{matrix} ] において最終行および最終列が対角成分以外 0 となっ

ているのは、加わった観測がそれまでの観測との相関関係がないことを示している。 (2.2.5)にならえば、この評価関数を最小にする推定値ˆ\underline{x}_{m+1} は

ˆ\underline{x}_{m+1} = ( ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) ^{T}[ \begin{matrix}R^{−1} & \underline{0} \\ \underline{0}^{T} & r^{−1} \end{matrix} ] ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) ) ^{−1} ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) ^{T}[ \begin{matrix}R^{−1} & \underline{0} \\ \underline{0}^{T} & r^{−1} \end{matrix} ] ( \underline{z} ; z_{m+1})
= (H^{T}R^{−1} H + \underline{h}r^{−1} \underline{h}^{T})^{−1} (H^{T}R^{−1} \underline{z}+ \underline{h}r^{−1} z_{m+1})

(2.3.8)

同様に (2.2.7) にならえば、推定誤差共分散 P_{m+1} は次のようになる。

P_{m+1} = ( ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) ^{T}[ \begin{matrix}R^{−1} & \underline{0} \\ \underline{0}^{T} & r^{−1} \end{matrix} ] ( \begin{matrix}H \\ \underline{h} & T \end{matrix}) ) ^{−1}
= (H^{T}R^{−1} H + \underline{h}r^{−1} \underline{h}^{T})^{−1}
= (P_{m}+ \underline{h}r^{−1} \underline{h}^{T})^{−1}
= P_{m}− P_{m}\underline{h}(\underline{h}^{T}P_{m}\underline{h}+ r)^{−1} \underline{h}^{T}P_{m}

(2.3.9)

最後の式変形においては以下の公式を用いた。

A^{−1} = B^{−1} + C^{T}D^{−1} C のとき (D + CBC^{T})^{−1} が存在するなら

A = B − BC^{T}(D + CBC^{T})^{−1} CB  (2.3.10)

ここで

\underline{k}_{m}≡ P_{m}\underline{h}(\underline{h}^{T}P_{m}\underline{h}+ r)^{−1}  (2.3.11)

なる\underline{k}_{m} を用いれば

P_{m+1} = (I − \underline{k}_{m}\underline{h}^{T}) P_{m}  (2.3.12)

ここで (2.3.8) に立ち返ってみれば

ˆ\underline{x}_{m+1} = P_{m+1} (H^{T}R^{−1} \underline{z}+ \underline{h}r^{−1} z_{m+1})

= (I − \underline{k}_{m}\underline{h}^{T}) P_{m}(H^{T}R^{−1} \underline{z}+ \underline{h}r^{−1} z_{m+1}) = (I − \underline{k}_{m}\underline{h}^{T}) ˆ \underline{x}_{m}+ (I − \underline{k}_{m}\underline{h}^{T}) P_{m}\underline{h}r^{−1} z_{m+1} = (I − \underline{k}_{m}\underline{h}^{T}) ˆ \underline{x}_{m}+ ( I − P_{m}\underline{h}(\underline{h}^{T}P_{m}\underline{h}+ r)^{−1} \underline{h}^{T}) \underline{k}_{m}(\underline{h}^{T}P_{m}\underline{h}+ r) r^{−1} z_{m+1}

= (I − \underline{k}_{m}\underline{h}^{T}) ˆ \underline{x}_{m}+ \frac{(\underline{h}^{T}P_{m}\underline{h}+ r) I − P_{m}\underline{h}\underline{h}^{T}}{\underline{h}^{T}P_{m}\underline{h}+ r}\underline{k}_{m}(\underline{h}^{T}P_{m}\underline{h}+ r) r^{−1} z_{m+1}

(∵ \underline{h}^{T}P_{m}\underline{h}+ r はスカラー) = (I − \underline{k}_{m}\underline{h}^{T}) ˆ \underline{x}_{m}+ r\underline{k}_{m}r^{−1} z_{m+1} = (I − \underline{k}_{m}\underline{h}^{T}) ˆ \underline{x}_{m}+ \underline{k}_{m}z_{m+1} = ˆ \underline{x}_{m}+ \underline{k}_{m}(z_{m+1} − \underline{h}^{T}ˆ\underline{x}_{m})

(2.3.13)

これは (2.3.6)の形をしている。つまり、観測値があたらに得られたら以上の手順を繰り返し行いˆ\underline{x} を更新していけばよい。観測値が複数の場合は z_{m+1} → \underline{z}_{m+1} および\underline{h}→ H とすればよい。

2.4 離散系 Kalman Filter

離散系では真値\underline{x}についてステップ間で次の関係が成り立つ。

\underline{x}_{k+1} = Φ_{k+1,k} \underline{x}_{k}+ Γ_{k}\underline{w}_{k}  (2.4.1)

ここで\underline{w}_{k} は時間的に相関関係がなく平均が 0 のノイズである。一方、最も確からしい推定値ˆ\underline{x}とその推定値から予測される値¯\underline{x}についてステップ間で次の関係が成り立つ。

¯\underline{x}_{k+1} ≡ Γ_{k+1,k} ˆ \underline{x}_{k}  (2.4.2)

ここで推定値ˆ\underline{x}と真値\underline{x}の残差ˆ\underline{ϵ}_{k} は

ˆ\underline{ϵ}_{k} = ˆ \underline{x}_{k}− \underline{x}_{k}  (2.4.3)

従って¯\underline{x}と真値との残差¯\underline{ϵ}_{k+1} は

\begin{aligned}¯\underline{ϵ}_{k+1} & = ˆ \underline{x}_{k+1} − \underline{x}_{k+1} \\ & = Φ_{k+1,k} ˆ \underline{x}_{k}− (Φ_{k+1,k} \underline{x}_{k}+ Γ_{k}\underline{w}_{k}) \\ & = Φ_{k+1,k} ˆ \underline{ϵ}_{k}− Γ_{k}\underline{w}_{k} \end{aligned}  (2.4.4)

であり、その期待値 E[¯\underline{ϵ}_{k+1}] は

\begin{matrix}E[¯\underline{ϵ}_{k+1}] & = E[¯\underline{x}_{k+1} − \underline{x}_{k+1}] \\ = Φ_{k+1,k}E[ˆ\underline{ϵ}_{k}] − Γ_{k}E[\underline{w}_{k}] \\ = \underline{0} \end{matrix}  (2.4.5)

でゼロである。共分散 P̄_{k+1} は

P̄_{k+1} ≡ E[¯\underline{ϵ}_{k+1} ¯ \underline{ϵ}_{k+1}^{T}]

= E[(Φ_{k+1,k} ˆ \underline{ϵ}_{k}− Γ_{k}\underline{w}_{k}) (Φ_{k+1,k} ˆ \underline{ϵ}_{k}− Γ_{k}\underline{w}_{k}) ^{T}] = Φ_{k+1,k}E[ˆ\underline{ϵ}_{k}ˆ\underline{ϵ}_{k}^{T}]Φ_{k+1,k}^{T} − Γ_{k}E[\underline{w}_{k}ˆ\underline{ϵ}_{k}^{T}]Φ_{k+1,k}^{T}

− Φ_{k+1,k}E[ˆ\underline{ϵ}_{k}\underline{w}_{k}^{T}]Γ_{k}^{T}+ Γ_{k}E[\underline{w}_{k}\underline{w}_{k}^{T}]Γ_{k}^{T}

(2.4.6)

ここで\underline{w}_{k} とˆ\underline{ϵ}_{k−1} が無関係であること、\underline{w} が時間的に相関関係がないことに注意すれば

E[\underline{w}_{k}\underline{ϵ}_{k}^{T}]= E[\underline{w}_{k}(Φ_{k,k−1} ˆ \underline{ϵ}_{k−1} − Γ_{k−1} \underline{w}_{k−1}) ^{T}]
= E[\underline{w}_{k}ˆ\underline{ϵ}_{k−1}^{T}]Φ_{k,k−1}^{T} − E[\underline{w}_{k}\underline{w}_{k−1}^{T}]Γ_{k}^{T}
= \underline{0}

(2.4.7)

で (2.4.6) の第 2 項と第 3 項はゼロであるから P̄_{k+1} は

P̄_{k+1} = Φ_{k+1,k} P̂_{k}Φ_{k+1,k}^{T} + Γ_{k}Q_{k}Γ_{k}^{T}  (2.4.8)

と書くことができる。

次に\underline{x}_{k+1} に関する観測値\underline{z}_{k+1} が得られたとする。このとき\underline{x}_{k+1} の最も確からしい値 x̂_{k+1} は前節の結論から次のように書くことが出来る。

ˆ\underline{x}_{k+1} = ¯ \underline{x}_{k+1} + K_{k+1} [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1}]  (2.4.9)

ここで

K_{k+1} = P̄_{k+1}H_{k+1}^{T} (H_{k+1} P̄_{k+1}H_{k+1}^{T} + R_{k+1})^{−1}  (2.4.10)

である。この推定値 x̂_{k+1} と真値 x⃗_{k+1} の残差ˆ\underline{ϵ}_{k+1} は

ˆ\underline{x}_{k+1} = \underline{x}_{k+1} + ˆ \underline{ϵ}_{k+1}  (2.4.11)

であるから計算すると

\begin{aligned}\underline{x}_{k+1} + ˆ \underline{ϵ}_{k+1} & = Φ_{k+1,k} (\underline{x}_{k}+ ˆ \underline{ϵ}_{k}) + K_{k+1} [\underline{z}_{k+1} − H_{k+1}Φ_{k+1,k} (\underline{x}_{k}+ ˆ \underline{ϵ}_{k}) ] \\ & = Φ_{k+1,k} \underline{x}_{k}+ Φ_{k+1,k} ˆ \underline{ϵ}_{k} \\ & + K_{k+1} [H_{k+1} (\underline{x}_{k+1} − Φ_{k+1,k} \underline{x}_{k}) + \underline{v}_{k+1} − H_{k+1}Φ_{k+1,k} ϵ̂_{k}] \end{aligned}  (2.4.12)

ここで (2.4.1) を用いれば

ˆ\underline{ϵ}_{k+1} = [I − K_{k+1}H_{k+1}]Φ_{k+1,k} ˆ \underline{ϵ}_{k}− [I − K_{k+1}H_{k+1}]Γ_{k}\underline{w}_{k}+ K_{k+1} \underline{v}_{k+1}  (2.4.13)

対応する共分散 P̂_{k+1} は

P̂_{k+1} = E[ˆ\underline{ϵ}_{k+1} ˆ \underline{ϵ}_{k+1}^{T}]

= [I − K_{k+1}H_{k+1}]Φ_{k+1,k} P̂_{k}Φ_{k+1,k}^{T} [I − K_{k+1}H_{k+1}]^{T} [I − K_{k+1}H_{k+1}]Γ_{k}Q_{k}Γ_{k}^{T}[I − K_{k+1}H_{k+1}]^{T}+ K_{k+1}R_{k+1}K_{k+1}^{T}

(2.4.14)

(2.4.8) を用いれば

P̂_{k+1} = [I − K_{k+1}H_{k+1}]P̄_{k+1}[I − K_{k+1}H_{k+1}]^{T}+ K_{k+1}R_{k+1}K_{k+1}^{T}  (2.4.15)

となる。さらに添え字を省略して (2.4.10) を考えれば

P̂= [I − KH]P̄[I − KH]^{T}+ KRK^{T}

= P̄− KHP̄− P̄H^{T}K^{T}+ KHP̄H^{T}K^{T}+ KRK^{T} = P − PH^{T}(HPH^{T}+ R)^{−1} HP − PH^{T}()^{−1} HP

+ PH^{T}()^{−1} HPH^{T}()^{−1} HP + PH^{T}()^{−1} R()^{−1} HP = P − PH^{T}()^{−1} HP − PH^{T}()^{−1} HP + PH^{T}()^{−1} ()()^{−1} HP = P − PH^{T}(HPH^{T}+ R)^{−1} HP = [I − KH]P̄

(2.4.16)

以上より離散系に対する式が得られたが、これが離散系に対する標準的な Kalman Filter である。整理して書けば

• 更新するとき (update 時)

\underline{x}_{k+1} = Φ_{k+1,k} \underline{x}_{k}+ Γ_{k}\underline{w}_{k}  (2.4.1)
P̄_{k+1} = Φ_{k+1,k} P̂_{k}Φ_{k+1,k}^{T} + Γ_{k}Q_{k}Γ_{k}^{T}  (2.4.8)

• 観測量を用いて修正するとき (correct 時)

ˆ\underline{x}_{k+1} = ¯ \underline{x}_{k+1} + K_{k+1} [\underline{z}_{k+1} − H_{k+1} ¯ \underline{x}_{k+1}]  (2.4.9)
K_{k+1} = P̄_{k+1}H_{k+1}^{T} (H_{k+1} P̄_{k+1}H_{k+1}^{T} + R_{k+1})^{−1}  (2.4.10)
P̂_{k+1} = [I − K_{k+1}H_{k+1}]P̄_{k+1}  (2.4.17)

2.5 離散系 Kalman Filter の連続系への適用

前節で述べた離散系 Kalman Filter は、その想定する系が (2.4.1) に表されるように離散系である。ここで次の連続線形系

\frac{d}{dt}\underline{x}≡ ˙ \underline{x}= A\underline{x}+ B\underline{w}  (2.5.1)

の Δt あたりの\underline{x}の微小変化 Δ\underline{x}を考えると、微分の定義から

Δ\underline{x}≈ AΔt \underline{x}+ BΔt \underline{w}  (2.5.2)

従って

\underline{x}+ Δ\underline{x}= (I + AΔt) \underline{x}+ BΔt \underline{w}  (2.5.3)

これと (2.4.1) を比較すれば

Φ_{k+1,k} = I + AΔt  (2.5.4)
Γ_{k} = BΔt  (2.5.5)

とすれば離散系 Kalman Filter を連続線形系へ適用できる。また、連続非線形系であれば線形化を行うことにより連続線形系に変換後、この手法を用いることが可能である。

2.6 UD 分解 Kalman Filter

式 (2.4.1), (2.4.17), (2.4.9), (2.4.17) によって Kalman Filter を構成することが可能であるが、具体的に数値計算による Kalman Filter を形成する場合では、数値演算の精度によって Filter が発散する可能がある。特に式 2.4.9 において Kalman Gain (K) を計算する際に逆行列を計算することになるが、数値演算の精度が十分でない場合などは桁落ちが発生し Filter が不安定になる。

そのような数値演算によってもたらされる Filter の不安定性については (参考文献) 等によって解析が行われていると同時に、計算を工夫することによって桁落ちの可能性を抑え安定性を増す手法がいくつか提案されている。そのうちの 1 つに、誤差共分散行列 ( P, Q, R) の対象性に注目した UD 分解 Kalman Filter というものがある。

2.6.1 UD 分解

実対象行列

第 3 章

System Design

本章では提案する INS/GPS のアルゴリズムについて述べる。まず運動方程式を構築し、それを INS/GPS の骨子である Kalman Filter に適用できるよう線形

化を行う。続いて観測方程式について議論し、最後にそれらを統合し INS/GPS のアルゴリズムとする。

式の計算については Quaternion(q̃や\{ \begin{matrix}q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}\} )、 3 次元ベクトル (x⃗や( \begin{matrix}x_{1} \\ x_{2} \\ x_{3} \end{matrix}) ) を用いる。これにつ

いては付録 A で述べる。また複数の座標系 (b-Frame など) が登場するが、それについては付録 B で述べる。

3.1 運動方程式

本節では INS に必要な機体の運動方程式を構築する。

3.1.1 速度の運動方程式

e-Frame と i-Frame における機体の位置を r⃗_{e}、 r⃗_{i} と表記すると、 e-Frame と i-Frame の原点は一致することから、回転 Quaternion によって両者の関係をあらわすことができる。

{ 0 ; r⃗_{e}} = q̃_{i}^{e}^{∗}{ 0 ; r⃗_{i}} q̃_{i}^{e}  (3.1.1)

ここで Quaternion の時間微分の公式より

˙q̃ = \frac{1}{2} \{ 0 ; ω⃗\} q̃  (3.1.2)

˙q̃^{∗} = ( ˙q̃)^{∗} = \frac{1}{2}(\{ 0 ; ω⃗\} q̃)^{∗} = −\frac{1}{2}q̃^{∗}\{ 0 ; ω⃗\}  (3.1.3)

であるから、 (3.1.1) を時間微分すると

\begin{aligned}\{ \begin{matrix}0 \\ ṙ_{e}^{e} \end{matrix}\} & = \frac{d}{dt}( q̃_{i}^{e}^{∗}\{ 0 ; r⃗_{i}\} q̃_{i}^{e}) \\ & = ˙ q̃_{i}^{e}^{∗}\{ 0 ; r⃗_{i}\} q̃_{i}^{e}+q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ṙ_{i}^{i} \end{matrix}\} q̃_{i}^{e}+q̃_{i}^{e}^{∗}\{ 0 ; r⃗_{i}\} ˙ q̃_{i}^{e} \\ & = −\frac{1}{2}q̃_{i}^{e}^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} \{ 0 ; r⃗_{i}\} q̃_{i}^{e}+q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ṙ_{i}^{i} \end{matrix}\} q̃_{i}^{e}+ \frac{1}{2}q̃_{i}^{e}^{∗}\{ 0 ; r⃗_{i}\} \{ 0 ; ω⃗_{e/i}^{i} \} q̃_{i}^{e} \\ & = q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ṙ_{i}^{i} \end{matrix}\} q̃_{i}^{e}−q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×r⃗_{i} \end{matrix} \} q̃_{i}^{e} \\ & ( = \{ \begin{matrix}0 \\ ṙ_{i}^{e} \end{matrix}\} −q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×r⃗_{i} \end{matrix} \} q̃_{i}^{e}) \end{aligned}  (3.1.4)

ここで ṙ_{1}^{2} は 1-Frame における機体の速度を 2-Frame で観測した値であり、 ω⃗_{1/2}^{3} は 2-Frame に対する 1-Frame の回転角速度を 3-Frame で観測した値である。

ω⃗_{e/i} が時間変化しないことに注意して、さらに時間微分すれば

\{ \begin{matrix}0 \\ r̈_{e}^{e} \end{matrix}\} = \frac{d}{dt}( q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ṙ_{i}^{i} \end{matrix}\} q̃_{i}^{e}−q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×r⃗_{i} \end{matrix} \} q̃_{i}^{e})

= ( q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ r̈_{i}^{i} \end{matrix}\} q̃_{i}^{e}−q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×ṙ_{i}^{i} \end{matrix} \} q̃_{i}^{e}) − ( q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×ṙ_{i}^{i} \end{matrix} \} q̃_{i}^{e}−q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} × ( ω⃗_{e/i}^{i} ×r⃗_{i}) \end{matrix} \} q̃_{i}^{e})

= q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ r̈_{i}^{i} \end{matrix}\} q̃_{i}^{e}− 2q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×ṙ_{i}^{i} \end{matrix} \} q̃_{i}^{e}+q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} × ( ω⃗_{e/i}^{i} ×r⃗_{i}) \end{matrix} \} q̃_{i}^{e}

= \{ \begin{matrix}0 \\ r̈_{i}^{e} \end{matrix}\} − \underbrace{2q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×ṙ_{i}^{i} \end{matrix} \} q̃_{i}^{e}}_{コリオリ力}+\underbrace{q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} × ( ω⃗_{e/i}^{i} ×r⃗_{i}) \end{matrix} \} q̃_{i}^{e}}_{遠心力}

(3.1.5)

ここで e-Frame における速度は次のとおりである。

\{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} = q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ṙ_{e}^{e} \end{matrix}\} q̃_{e}^{n}  (3.1.6)

微分して

\begin{aligned}\{ \begin{matrix}0 \\ r̈_{e}^{n} \end{matrix}\} & = \frac{d}{dt}( q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ṙ_{e}^{e} \end{matrix}\} q̃_{e}^{n}) \\ & = q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ r̈_{e}^{e} \end{matrix}\} q̃_{e}^{n}−q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{n/e}^{e} ×ṙ_{e}^{e} \end{matrix} \} q̃_{e}^{n} \end{aligned}  (3.1.7)

(3.1.5) を代入すれば

\begin{aligned}\{ \begin{matrix}0 \\ r̈_{e}^{n} \end{matrix}\} & = q̃_{e}^{n}^{∗}( \{ \begin{matrix}0 \\ r̈_{i}^{e} \end{matrix}\} − 2q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×ṙ_{i}^{i} \end{matrix} \} q̃_{i}^{e}+q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} × ( ω⃗_{e/i}^{i} ×r⃗_{i}) \end{matrix} \} q̃_{i}^{e}) q̃_{e}^{n}−q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{n/e}^{e} ×ṙ_{e}^{e} \end{matrix} \} q̃_{e}^{n} \\ & = \{ \begin{matrix}0 \\ r̈_{i}^{n} \end{matrix}\} −q̃_{e}^{n}^{∗}( 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×ṙ_{i}^{e} \end{matrix} \} − \{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{e} ×ṙ_{e}^{e} \end{matrix} \} ) q̃_{e}^{n} \end{aligned}

(3.1.8)

さらに (3.1.4) より

\begin{aligned}\{ \begin{matrix}0 \\ ṙ_{i}^{e} \end{matrix}\} & = \{ \begin{matrix}0 \\ ṙ_{e}^{e} \end{matrix}\} +q̃_{i}^{e}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{i} ×r⃗_{i} \end{matrix} \} q̃_{i}^{e} \\ & = \{ \begin{matrix}0 \\ ṙ_{e}^{e} \end{matrix}\} + \{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×r⃗_{e} \end{matrix} \} \end{aligned}  (3.1.9)

従って

\begin{aligned}2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×ṙ_{i}^{e} \end{matrix} \} & = 2\{ 0 ; ω⃗_{e/i}^{e} \} \{ \begin{matrix}0 \\ ṙ_{i}^{e} \end{matrix}\} \\ & = 2\{ 0 ; ω⃗_{e/i}^{e} \} [\{ \begin{matrix}0 \\ ṙ_{e}^{e} \end{matrix}\} + \{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×r⃗_{e} \end{matrix} \} ] \\ & = 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×ṙ_{e}^{e} \end{matrix} \} + 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} \end{aligned}  (3.1.10)

であるから、 (3.1.8) に代入して

\begin{aligned}\{ \begin{matrix}0 \\ r̈_{e}^{n} \end{matrix}\} & = \{ \begin{matrix}0 \\ r̈_{i}^{n} \end{matrix}\} −q̃_{e}^{n}^{∗}( 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×ṙ_{e}^{e} \end{matrix} \} + 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} \\ −\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{e} ×ṙ_{e}^{e} \end{matrix} \} ) & q̃_{e}^{n} \\ & = \{ \begin{matrix}0 \\ r̈_{i}^{n} \end{matrix}\} −q̃_{e}^{n}^{∗}( 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×ṙ_{e}^{e} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{e} ×ṙ_{e}^{e} \end{matrix} \} ) q̃_{e}^{n} \end{aligned}  (3.1.11)

ところで機体に固定された加速度計の出力 a^{b} は、 i-Frame における機体の加速度 (地球上に静止していることによる自転の向心力を含む) に地球の万有引力を加えたものを b-Frame 上で観測した値であり

\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} = q̃_{n}^{b}^{∗}\{ \begin{matrix}0 \\ r̈_{i}^{n} \end{matrix}\} q̃_{n}^{b}− \underbrace{\{ \begin{matrix}0 \\  & g^{b} \end{matrix}\}}_{地球重力}  (3.1.12)

変形して

\begin{aligned}\{ \begin{matrix}0 \\ r̈_{i}^{n} \end{matrix}\} & = q̃_{b}^{n}^{∗}( \{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} + \{ \begin{matrix}0 \\  & g^{b} \end{matrix}\} ) q̃_{b}^{n} \\ & = q̃_{b}^{n}^{∗}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{b}^{n}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} \end{aligned}  (3.1.13)

これを (3.1.11)に代入すれば、 (3.1.14)のとおり n-Frame での機体の速度の運動方程式が得られる。

\begin{aligned}\frac{d}{dt}\{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} & ≡ \{ \begin{matrix}0 \\ r̈_{e}^{n} \end{matrix}\} \\ & = q̃_{b}^{n}^{∗}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{b}^{n}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} \\ & −q̃_{e}^{n}^{∗}( 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} ×ṙ_{e}^{e} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{e} ×ṙ_{e}^{e} \end{matrix} \} ) q̃_{e}^{n} \\ & = q̃_{b}^{n}^{∗}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{b}^{n}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} \\ & − ( 2\{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} ×ṙ_{e}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} ×ṙ_{e}^{n} \end{matrix} \} ) −q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n} \\ & = q̃_{b}^{n}^{∗}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{b}^{n}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} \\ & − \{ \begin{matrix}0 \\ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) ×ṙ_{e}^{n} \end{matrix} \} −q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n} \end{aligned} (3.1.14)

ここにおいて ω⃗_{e/i}^{n} は

{ 0 ; ω⃗_{e/i}^{n} } = q̃_{e}^{n}^{∗}{ 0 ; ω⃗_{e/i}^{e} } q̃_{e}^{n}  (3.1.15)

であり、 ω_{e/i} は地球の自転速度ベクトル、すなわち

ω⃗_{e/i}^{i} = ω⃗_{e/i}^{e} = ( \begin{matrix}0 \\ 0 \\ Ω_{e/i} \end{matrix} )  (3.1.16)

である (Ω_{e/i} は地球の自転速度)。また ω⃗_{n/e}^{n} は

{ 0 ; ω⃗_{n/e}^{n}} = q̃_{g}^{n}^{∗}{ 0 ; ω⃗_{n/e}^{g}} q̃_{g}^{n}  (3.1.17)

であるが、 ω⃗_{n/e}^{g} は以下の手順で求めることが可能である。緯度、経度をそれぞれ ϕ、 λ とすると

\frac{d}{dt}ϕ ≡ ϕ̇= \frac{v_{N}}{R_{meridian}+ h}  (3.1.18)
\frac{d}{dt}λ ≡ λ̇ = \frac{v_{E}}{β} = \frac{v_{E}}{(R_{normal}+ h) cosϕ}  (3.1.19)

ただし R_{meridian}、 R_{normal}はそれぞれ南北方向と東西方向における極率半径を表し、 h は高度を、 v_{N}、 v_{E} はそれぞれ北方向、東方向の速度をあらわすとする。地球モデル (WGS-84、付録 C 参照) より R_{meridian}、 R_{normal}は

R_{meridian}= \frac{r_{e}(1 − ϵ^{2})}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{3}{2}}}  (3.1.20)

R_{normal}= \frac{r_{e}}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{1}{2}}}  (3.1.21)

v_{N}、 v_{E} および下方向の速度 v_{D} は

\{ \begin{matrix}& 0 \\ ( & \begin{matrix}v_{N} \\ v_{E} \\ v_{D} \end{matrix} & ) \end{matrix} \} ≡ \{ \begin{matrix}0 \\ ṙ_{e}^{g} \end{matrix}\} = q̃_{n}^{g}^{∗}\{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} q̃_{n}^{g}  (3.1.22)

より得られる。ここで図より ω⃗_{n/e} は、

(ω⃗_{n/e}^{g})_{east}= −ϕ̇  (3.1.23)
(ω⃗_{n/e}^{g})_{down}= (ω⃗_{n/e}^{n})_{down}= λ̇ sinϕ  (3.1.24)
(ω⃗_{n/e}^{e})_{z} = λ̇  (3.1.25)

であるから重ね合わせて

\{ 0 ; ω⃗_{n/e}^{g}\} = \{ \begin{matrix}& 0 \\ ( & 0 (ω⃗_{n/e}^{g})_{east} (ω⃗_{n/e}^{g})_{down} & ) \end{matrix} \} +q̃_{e}^{g}^{∗} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}0 \\ 0 \\ (ω⃗_{n/e}^{e})_{z} \end{matrix} & ) \end{matrix} \} q̃_{e}^{g}  (3.1.26)

計算すると、

ω⃗_{n/e}^{g} = ( \begin{matrix}λ̇ cosϕ \\ −ϕ̇ \\ 0 \end{matrix} )

= ( \frac{v_{E}}{R_{normal}+h} −\frac{v_{N}}{R_{meridian}+h} 0 )

(3.1.27)

さらに ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) は、図 3.1 より

ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) = Ω_{e/i}^{2}β ( \begin{matrix}−cosλ \\ −sinλ \\ 0 \end{matrix} )

= Ω_{e/i}^{2}(R_{normal}+ h)cosϕ ( \begin{matrix}−cosλ \\ −sinλ \\ 0 \end{matrix} )

(3.1.28)

ここで付録 B より q̃_{e}^{n} の三角関数表現を適用すると

ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) = 2Ω_{e/i}^{2}(R_{normal}+ h)( \begin{matrix}(q̃_{e}^{n})_{0}(q̃_{e}^{n})_{2}+ (q̃_{e}^{n})_{1}(q̃_{e}^{n})_{3} \\ (q̃_{e}^{n})_{3}(q̃_{e}^{n})_{2}− (q̃_{e}^{n})_{1}(q̃_{e}^{n})_{0} \\ 0 \end{matrix} )  (3.1.29)

図 3.1 向心力

3.1.2 位置の運動方程式

機体の位置は緯度 ϕ、経度 λ、高度 h であらわされるものとする。すなわち q̃_{e}^{n} と h について運動方程式を立てれば、それが機体の位置の運動方程式となる。

q̃_{e}^{n} の運動方程式は Quaternion の時間微分の公式より

\begin{aligned}\frac{d}{dt}q̃_{e}^{n} ≡ ˙ q̃_{e}^{n} & = \frac{1}{2} \{ 0 ; ω⃗_{n/e}^{e}\} q̃_{e}^{n} \\ & = \frac{1}{2}q̃_{n}^{e}^{∗}\{ 0 ; ω⃗_{n/e}^{n}\} q̃_{n}^{e}q̃_{e}^{n} \\ & = \frac{1}{2}q̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{n}\} \end{aligned}  (3.1.30)

ω⃗_{n/e}^{n} は (3.1.27) より得られる。

また高度 h の運動方程式は

\frac{d}{dt}h = −v_{D} ≡ −(ṙ_{e}^{n})_{Z}  (3.1.31)

である。

3.1.3 姿勢の運動方程式

機体の姿勢は q̃_{n}^{b} であらわされ、その運動方程式は Quaternion の時間微分の公式から次のようにあらわされる。

\frac{d}{dt}q̃_{n}^{b} ≡ ˙ q̃_{n}^{b} = \frac{1}{2} \{ 0 ; ω⃗_{b/n}^{n}\} q̃_{n}^{b}  (3.1.32)

ここで

\begin{aligned}ω⃗_{b/n}^{n} & = ω⃗_{b/i}^{n} − ω⃗_{n/i}^{n} \\ & = ω⃗_{b/i}^{n} − ( ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) \end{aligned}  (3.1.33)

{ 0 ; ω⃗_{b/i}^{n}} = q̃_{b}^{n}^{∗}{ 0 ; ω⃗_{b/i}^{b}} q̃_{b}^{n}  (3.1.34)

であり、 ω⃗_{e/i}^{n} 、 ω⃗_{n/e}^{n} はそれぞれ (3.1.15)、 (3.1.27) から求まる。また ω⃗_{b/i}^{b} は、 i-Frame に対して b-Frame の回転角速度を b-Frame 上で観測した値であり、これはまさに機体に固定されたジャイロの出力である。

以上まとめて (3.1.32) は

\begin{aligned}˙q̃_{n}^{b} & = \frac{1}{2} [q̃_{b}^{n}^{∗}\{ 0 ; ω⃗_{b/i}^{b}\} q̃_{b}^{n}− ( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) ] q̃_{n}^{b} \\ & = \frac{1}{2} [q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} − ( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{n}^{b}] \end{aligned}  (3.1.35)

3.2 運動方程式の線形化

INS/GPS は INS と GPS を Kalman Filtering をもって統合する。そのために前節で求めた運動方程式における非線形項を解消する必要がある。そこで本節では前節で求めた運動方程式を真値との残差で線形化する。残差は Δ をつけて表現することにする。

3.2.1 速度の運動方程式の線形化

速度の運動方程式 (3.1.14) を真値との残差について線形化すると次のようになる。

\frac{d}{dt}\{ \begin{matrix}0 \\ Δṙ_{e}^{n} \end{matrix}\} = \frac{d}{dt}( \{ \begin{matrix}0 \\ ṙ_{e}^{n}+ Δṙ_{e}^{n} \end{matrix} \} − \{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} )

= [( q̃_{n}^{b}+ Δq̃_{n}^{b}) \{ \begin{matrix}0 \\ a^{b}+ Δa^{b} \end{matrix} \} ( q̃_{n}^{b}+ Δq̃_{n}^{b}) ^{∗}+ \{ \begin{matrix}0 \\ g^{n}+ Δg^{n} \end{matrix} \}

− \{ \begin{matrix}0 \\ ( 2( ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} ) + ( ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n}) ) × (ṙ_{e}^{n}+ Δṙ_{e}^{n}) \end{matrix} \}

− (q̃_{e}^{n}+ Δq̃_{e}^{n}) ^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × (r⃗_{e}+ Δr⃗_{e}) ) \end{matrix} \} (q̃_{e}^{n}+ Δq̃_{e}^{n}) ]

− [q̃_{n}^{b}\{ \begin{matrix}0 \\ & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\ & g^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) ×ṙ_{e}^{n} \end{matrix} \}

− q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}]

(3.2.1)

\frac{d}{dt}\{ \begin{matrix}0 \\ Δṙ_{e}^{n} \end{matrix}\} = Δq̃_{n}^{b}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+q̃_{n}^{b}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} Δq̃_{n}^{b}^{∗}+q̃_{n}^{b}\{ \begin{matrix}0 \\ Δa^{b} \end{matrix}\} q̃_{n}^{b}^{∗}+ \{ \begin{matrix}0 \\ Δg^{n} \end{matrix}\}

− \{ \begin{matrix}0 \\ ( 2Δω⃗_{e/i}^{n} + Δω⃗_{n/e}^{n}) ×ṙ_{e}^{n}+ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) × Δṙ_{e}^{n} \end{matrix} \}

−q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) \end{matrix} \} q̃_{e}^{n}

− Δq̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n}−q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} Δq̃_{e}^{n}

(3.2.2)

ここで

• Δω⃗_{e/i}^{n} は (3.1.15)、 (3.1.16) より ω⃗_{e/i} は地球の自転速度であるから一定と考え

\{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} \end{matrix} \} = (q̃_{e}^{n}+ Δq̃_{e}^{n}) ^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} (q̃_{e}^{n}+ Δq̃_{e}^{n}) −q̃_{e}^{n}^{∗}\{ 0 ; ω⃗_{e/i}^{i} \} q̃_{e}^{n}  (3.2.3)

• ω⃗_{n/e}^{n} が (3.1.27)、 (3.1.22)、 (3.1.20)、 (3.1.21) から以下のように近似できる。

{ 0 ; ω⃗_{n/e}^{n}} = q̃_{g}^{n}^{∗}{ 0 ; ω⃗_{n/e}^{g}} q̃_{g}^{n}

= q̃_{g}^{n}^{∗} \{ \begin{matrix}& 0 \\ ( & \frac{v_{E}}{R_{normal}+h} −\frac{v_{N}}{R_{meridian}+h} 0 & ) \end{matrix} \} q̃_{g}^{n} ≈ q̃_{g}^{n}^{∗} \{ \begin{matrix}& 0 \\ \frac{1}{r_{e}+h} & ( \begin{matrix}v_{E} \\ −v_{N} \\ 0 \end{matrix} ) \end{matrix} \} q̃_{g}^{n}

= \frac{1}{r_{e}+ h} q̃_{g}^{n}^{∗}q̃_{n}^{g}^{∗} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} & ) \end{matrix} \} q̃_{n}^{g}q̃_{g}^{n}

(∵ Z_{g} 軸と Z_{n} 軸は一致)

= \frac{1}{r_{e}+ h} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} & ) \end{matrix} \}

(3.2.4)

従って Δω⃗_{n/e}^{n} は

\begin{aligned}Δω⃗_{n/e}^{n} & = \frac{1}{r_{e}+ (h + Δh)} ( \begin{matrix}(ṙ_{e}^{n})_{Y}+ Δ(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X}− Δ(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) − \frac{1}{r_{e}+ h} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) \\ & = \frac{1}{r_{e}+ h} ( \begin{matrix}Δ(ṙ_{e}^{n})_{Y} \\ −Δ(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) − \frac{1}{(r_{e}+ h)^{2}} ( \begin{matrix}(ṙ_{e}^{n})_{Y} \\ −(ṙ_{e}^{n})_{X} \\ 0 \end{matrix} ) Δh \end{aligned}  (3.2.5)

• ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} × Δr⃗_{e}) は (3.1.29) より

ω⃗_{e/i}^{e} ^{2}× ( ω⃗_{e/i}^{e} × Δr⃗_{e}) = 2Ω_{e/i}^{2}(R_{normal}+ (h + Δh) )
( \begin{matrix}(q_{0}+ Δq_{0}) (q_{2}+ Δq_{2}) + (q_{1}+ Δq_{1}) (q_{3}+ Δq_{3}) \\ (q_{3}+ Δq_{3}) (q_{2}+ Δq_{2}) − (q_{1}+ Δq_{1}) (q_{0}+ Δq_{0}) \\ 0 \end{matrix} ) _{q̃_{e}^{n}}

− 2Ω_{e/i}^{2}(R_{normal}+ h) ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}

= 2Ω_{e/i}^{2} \{ ( \begin{matrix}q_{0}q_{2}+ q_{1}q_{3} \\ q_{3}q_{2}− q_{1}q_{0} \\ 0 \end{matrix} ) _{q̃_{e}^{n}}Δh

+(R_{normal}+ h) [ \begin{matrix}q_{2} & q_{3} & q_{0} & q_{1} \\ −q_{1} & −q_{0} & q_{3} & q_{2} \\ 0 & 0 & 0 & 0 \end{matrix}]_{q̃_{e}^{n}}Δq̃_{e}^{n}\}

(3.2.6)

3.2.2 位置の運動方程式の線形化

位置の運動方程式 (3.1.30) および (3.1.31) を真値との残差について線形化すると次のようになる。

\begin{aligned}\frac{d}{dt}Δq̃_{e}^{n} & = \frac{1}{2} (q̃_{e}^{n}+ Δq̃_{e}^{n}) \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} − \frac{1}{2}q̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{n}\} \\ & = \frac{1}{2}q̃_{e}^{n}\{ \begin{matrix}0 \\ Δω⃗_{n/e}^{n} \end{matrix} \} + \frac{1}{2}Δq̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{n}\} \end{aligned}  (3.2.7)
\begin{aligned}\frac{d}{dt}Δh & = −((ṙ_{e}^{n})_{Z}+ Δ(ṙ_{e}^{n})_{Z}) − (−(ṙ_{e}^{n})_{Z}) \\ & = −Δ(ṙ_{e}^{n})_{Z} \end{aligned}  (3.2.8)

3.2.3 姿勢の運動方程式の線形化

姿勢の運動方程式 (3.1.35) を真値との残差について線形化すると次のようになる。

\frac{d}{dt}Δq̃_{n}^{b} = \frac{1}{2} [( q̃_{n}^{b}+ Δq̃_{n}^{b}) \{ \begin{matrix}0 \\ ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ \begin{matrix}0 \\ ω⃗_{e/i}^{n} + Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ ω⃗_{n/e}^{n} + Δω⃗_{n/e}^{n} \end{matrix} \} ) ( q̃_{n}^{b}+ Δq̃_{n}^{b}) ]

− \frac{1}{2} [q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} − ( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{n}^{b}]

= \frac{1}{2} [Δq̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} +q̃_{n}^{b}\{ \begin{matrix}0 \\ Δω⃗_{b/i}^{b} \end{matrix} \}

−( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) Δq̃_{n}^{b}− ( \{ \begin{matrix}0 \\ Δω⃗_{e/i}^{n} \end{matrix} \} + \{ \begin{matrix}0 \\ Δω⃗_{n/e}^{n} \end{matrix} \} ) q̃_{n}^{b}]

(3.2.9)

3.2.4 線形化された運動方程式

真値との残差について線形化した速度の運動方程式 (3.2.1)、位置の運動方程式 (3.2.7)(3.2.8)、姿勢の運動方程式 (3.2.9) をすべて行列形式で書き下すと以下のとおりになる。

\frac{d}{dt} \underline{x}= A \underline{x}+ B \underline{w}  (3.2.10)

ここで\underline{x}は

\underline{x}= Δ[ \begin{matrix}(ṙ_{e}^{n})_{X} \\ (ṙ_{e}^{n})_{Y} \\ (ṙ_{e}^{n})_{Z} \\ (q_{e}^{n})_{0} \\ (q_{e}^{n})_{1} \\ (q_{e}^{n})_{2} \\ (q_{e}^{n})_{3} \\ h \\ (q_{n}^{b})_{0} \\ (q_{n}^{b})_{1} \\ (q_{n}^{b})_{2} \\ (q_{n}^{b})_{3} \end{matrix}]  (3.2.11)

また\underline{w} は

\underline{w}= Δ[ \begin{matrix}(a^{b})_{X} \\ (a^{b})_{Y} \\ (a^{b})_{Z} \\ (ω_{b/i}^{b})_{X} \\ (ω_{b/i}^{b})_{Y} \\ (ω_{b/i}^{b})_{Z} \\ g \end{matrix} ]  (3.2.12)

行列 A、 B は

A= [ \begin{matrix}\frac{(ṙ_{e}^{n})_{Z}}{r_{e}+h} & ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) _{Z} & −( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) _{Y} \\ −( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) _{Z} & \frac{(ṙ_{e}^{n})_{Z}}{r_{e}+h} & ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) _{X} \\ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) _{Y}− \frac{(ṙ_{e}^{n})_{X}}{r_{e}+h} & −( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) _{X}− \frac{(ṙ_{e}^{n})_{Y}}{r_{e}+h} & 0 \\ \frac{(q_{e}^{n})_{2}}{2(r_{e}+h)} & −\frac{(q_{e}^{n})_{1}}{2(r_{e}+h)} & 0 \\ \frac{(q_{e}^{n})_{3}}{2(r_{e}+h)} & \frac{(q_{e}^{n})_{0}}{2(r_{e}+h)} & 0 \\ −\frac{(q_{e}^{n})_{0}}{2(r_{e}+h)} & \frac{(q_{e}^{n})_{3}}{2(r_{e}+h)} & 0 \\ −\frac{(q_{e}^{n})_{1}}{2(r_{e}+h)} & −\frac{(q_{e}^{n})_{2}}{2(r_{e}+h)} & 0 \\ 0 & 0 & −1 \\ −\frac{(q_{n}^{b})_{2}}{2(r_{e}+h)} & \frac{(q_{n}^{b})_{1}}{2(r_{e}+h)} & 0 \\ \frac{(q_{n}^{b})_{3}}{2(r_{e}+h)} & −\frac{(q_{n}^{b})_{0}}{2(r_{e}+h)} & 0 \\ \frac{(q_{n}^{b})_{0}}{2(r_{e}+h)} & \frac{(q_{n}^{b})_{3}}{2(r_{e}+h)} & 0 \\ −\frac{(q_{n}^{b})_{1}}{2(r_{e}+h)} & −\frac{(q_{n}^{b})_{2}}{2(r_{e}+h)} & 0 \end{matrix}

−4Ω_{e/i}((q_{e}^{n})_{1}(ṙ_{e}^{n})_{Z}− (q_{e}^{n})_{0}(ṙ_{e}^{n})_{Y})  −4Ω_{e/i}((q_{e}^{n})_{0}(ṙ_{e}^{n})_{Z}+ (q_{e}^{n})_{1}(ṙ_{e}^{n})_{Y})  −4Ω_{e/i}((q_{e}^{n})_{3}(ṙ_{e}^{n})_{Z}+ (q_{e}^{n})_{2}(ṙ_{e}^{n})_{Y})  −4Ω_{e/i}((q_{e}^{n})_{2}(ṙ_{e}^{n})_{Z}− (q_{e}^{n})_{3}(ṙ_{e}^{n})_{Y})
−4Ω_{e/i}((q_{e}^{n})_{0}(ṙ_{e}^{n})_{X}+ (q_{e}^{n})_{2}(ṙ_{e}^{n})_{Z})  −4Ω_{e/i}(−(q_{e}^{n})_{1}(ṙ_{e}^{n})_{X}− (q_{e}^{n})_{3}(ṙ_{e}^{n})_{Z})  −4Ω_{e/i}(−(q_{e}^{n})_{2}(ṙ_{e}^{n})_{X}+ (q_{e}^{n})_{0}(ṙ_{e}^{n})_{Z})  −4Ω_{e/i}((q_{e}^{n})_{3}(ṙ_{e}^{n})_{X}− (q_{e}^{n})_{1}(ṙ_{e}^{n})_{Z})
−4Ω_{e/i}(−(q_{e}^{n})_{2}(ṙ_{e}^{n})_{Y}− (q_{e}^{n})_{1}(ṙ_{e}^{n})_{X})  −4Ω_{e/i}((q_{e}^{n})_{3}(ṙ_{e}^{n})_{Y}− (q_{e}^{n})_{0}(ṙ_{e}^{n})_{X})  −4Ω_{e/i}(−(q_{e}^{n})_{0}(ṙ_{e}^{n})_{Y}− (q_{e}^{n})_{3}(ṙ_{e}^{n})_{X})  −4Ω_{e/i}((q_{e}^{n})_{1}(ṙ_{e}^{n})_{Y}− (q_{e}^{n})_{2}(ṙ_{e}^{n})_{X})
0  −(ω_{n/e}^{e})_{X} /2  −(ω_{n/e}^{e})_{Y} /2  −(ω_{n/e}^{e})_{Z} /2
(ω_{n/e}^{e})_{X} /2  0  (ω_{n/e}^{e})_{Z} /2  −(ω_{n/e}^{e})_{Y} /2
(ω_{n/e}^{e})_{Y} /2  −(ω_{n/e}^{e})_{Z} /2  0  (ω_{n/e}^{e})_{X} /2
(ω_{n/e}^{e})_{Z} /2  (ω_{n/e}^{e})_{Y} /2  −(ω_{n/e}^{e})_{X} /2  0
0  0  0  0
−Ω_{e/i}{(q_{n}^{b})_{1}(q_{e}^{n})_{2}− (q_{n}^{b})_{2}(q_{e}^{n})_{1}− (q_{n}^{b})_{3}(q_{e}^{n})_{0}}  −Ω_{e/i}{−(q_{n}^{b})_{1}(q_{e}^{n})_{3}− (q_{n}^{b})_{2}(q_{e}^{n})_{0}+ (q_{n}^{b})_{3}(q_{e}^{n})_{1}}  −Ω_{e/i}{(q_{n}^{b})_{1}(q_{e}^{n})_{0}− (q_{n}^{b})_{2}(q_{e}^{n})_{3}+ (q_{n}^{b})_{3}(q_{e}^{n})_{2}}  −Ω_{e/i}{−(q_{n}^{b})_{1}(q_{e}^{n})_{1}− (q_{n}^{b})_{2}(q_{e}^{n})_{2}− (q_{n}^{b})_{3}(q_{e}^{n})_{3}}
−Ω_{e/i}{−(q_{n}^{b})_{0}(q_{e}^{n})_{2}+ (q_{n}^{b})_{3}(q_{e}^{n})_{1}− (q_{n}^{b})_{2}(q_{e}^{n})_{0}}  −Ω_{e/i}{(q_{n}^{b})_{0}(q_{e}^{n})_{3}+ (q_{n}^{b})_{3}(q_{e}^{n})_{0}+ (q_{n}^{b})_{2}(q_{e}^{n})_{1}}  −Ω_{e/i}{−(q_{n}^{b})_{0}(q_{e}^{n})_{0}+ (q_{n}^{b})_{3}(q_{e}^{n})_{3}+ (q_{n}^{b})_{2}(q_{e}^{n})_{2}}  −Ω_{e/i}{+(q_{n}^{b})_{0}(q_{e}^{n})_{1}+ (q_{n}^{b})_{3}(q_{e}^{n})_{2}− (q_{n}^{b})_{2}(q_{e}^{n})_{3}}
−Ω_{e/i}{(q_{n}^{b})_{3}(q_{e}^{n})_{2}+ (q_{n}^{b})_{0}(q_{e}^{n})_{1}+ (q_{n}^{b})_{1}(q_{e}^{n})_{0}}  −Ω_{e/i}{−(q_{n}^{b})_{3}(q_{e}^{n})_{3}+ (q_{n}^{b})_{0}(q_{e}^{n})_{0}− (q_{n}^{b})_{1}(q_{e}^{n})_{1}}  −Ω_{e/i}{(q_{n}^{b})_{3}(q_{e}^{n})_{0}+ (q_{n}^{b})_{0}(q_{e}^{n})_{3}− (q_{n}^{b})_{1}(q_{e}^{n})_{2}}  −Ω_{e/i}{−(q_{n}^{b})_{3}(q_{e}^{n})_{1}+ (q_{n}^{b})_{0}(q_{e}^{n})_{2}+ (q_{n}^{b})_{1}(q_{e}^{n})_{3}}
−Ω_{e/i}{−(q_{n}^{b})_{2}(q_{e}^{n})_{2}− (q_{n}^{b})_{1}(q_{e}^{n})_{1}+ (q_{n}^{b})_{0}(q_{e}^{n})_{0}}  −Ω_{e/i}{(q_{n}^{b})_{2}(q_{e}^{n})_{3}− (q_{n}^{b})_{1}(q_{e}^{n})_{0}− (q_{n}^{b})_{0}(q_{e}^{n})_{1}}  −Ω_{e/i}{−(q_{n}^{b})_{2}(q_{e}^{n})_{0}− (q_{n}^{b})_{1}(q_{e}^{n})_{3}− (q_{n}^{b})_{0}(q_{e}^{n})_{2}}  −Ω_{e/i}{(q_{n}^{b})_{2}(q_{e}^{n})_{1}− (q_{n}^{b})_{1}(q_{e}^{n})_{2}+ (q_{n}^{b})_{0}(q_{e}^{n})_{3}}

\begin{matrix}−\frac{(ṙ_{e}^{n})_{X}(ṙ_{e}^{n})_{Z}}{(r_{e}+h)^{2}} & 2\{(a^{b})_{X}(q_{n}^{b})_{0}− (a^{b})_{Y}(q_{n}^{b})_{3}+ (a^{b})_{Z}(q_{n}^{b})_{2}\} & 2\{(a^{b})_{X}(q_{n}^{b})_{1}+ (a^{b})_{Y}(q_{n}^{b})_{2}+ (a^{b})_{Z}(q_{n}^{b})_{3}\} & 2\{−(a^{b})_{X}(q_{n}^{b})_{2}+ (a^{b})_{Y}(q_{n}^{b})_{1}+ (a^{b})_{Z}(q_{n}^{b})_{0}\} & 2\{−(a^{b})_{X}(q_{n}^{b})_{3}− (a^{b})_{Y}(q_{n}^{b})_{0}+ (a^{b})_{Z}(q_{n}^{b})_{1}\} \\ −\frac{(ṙ_{e}^{n})_{Y}(ṙ_{e}^{n})_{Z}}{(r_{e}+h)^{2}} & 2\{(a^{b})_{X}(q_{n}^{b})_{3}+ (a^{b})_{Y}(q_{n}^{b})_{0}− (a^{b})_{Z}(q_{n}^{b})_{1}\} & 2\{(a^{b})_{X}(q_{n}^{b})_{2}− (a^{b})_{Y}(q_{n}^{b})_{1}− (a^{b})_{Z}(q_{n}^{b})_{0}\} & 2\{(a^{b})_{X}(q_{n}^{b})_{1}+ (a^{b})_{Y}(q_{n}^{b})_{2}+ (a^{b})_{Z}(q_{n}^{b})_{3}\} & 2\{(a^{b})_{X}(q_{n}^{b})_{0}− (a^{b})_{Y}(q_{n}^{b})_{3}+ (a^{b})_{Z}(q_{n}^{b})_{2}\} \\ \frac{(ṙ_{e}^{n})_{X}^{2}+(ṙ_{e}^{n})_{Y}^{2}}{(r_{e}+h)^{2}} & 2\{−(a^{b})_{X}(q_{n}^{b})_{2}+ (a^{b})_{Y}(q_{n}^{b})_{1}+ (a^{b})_{Z}(q_{n}^{b})_{0}\} & 2\{(a^{b})_{X}(q_{n}^{b})_{3}+ (a^{b})_{Y}(q_{n}^{b})_{0}− (a^{b})_{Z}(q_{n}^{b})_{1}\} & 2\{−(a^{b})_{X}(q_{n}^{b})_{0}+ (a^{b})_{Y}(q_{n}^{b})_{3}− (a^{b})_{Z}(q_{n}^{b})_{2}\} & 2\{(a^{b})_{X}(q_{n}^{b})_{1}+ (a^{b})_{Y}(q_{n}^{b})_{2}+ (a^{b})_{Z}(q_{n}^{b})_{3}\} \\ \frac{−(q_{e}^{n})_{2}(ṙ_{e}^{n})_{X}+(q_{e}^{n})_{1}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & 0 & 0 & 0 & 0 \\ \frac{−(q_{e}^{n})_{3}(ṙ_{e}^{n})_{X}−(q_{e}^{n})_{0}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & 0 & 0 & 0 & 0 \\ \frac{(q_{e}^{n})_{0}(ṙ_{e}^{n})_{X}−(q_{e}^{n})_{3}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & 0 & 0 & 0 & 0 \\ \frac{(q_{e}^{n})_{1}(ṙ_{e}^{n})_{X}+(q_{e}^{n})_{2}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ \frac{(q_{n}^{b})_{2}(ṙ_{e}^{n})_{X}−(q_{n}^{b})_{1}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & 0 & \{−(ω_{b/i}^{b})_{X}+ (ω_{e/i}^{n}+ ω_{n/e}^{n})_{X}\} /2 & \{−(ω_{b/i}^{b})_{Y}+ (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Y}\} /2 & \{−(ω_{b/i}^{b})_{Z}+ (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Z}\} /2 \\ \frac{−(q_{n}^{b})_{3}(ṙ_{e}^{n})_{X}+(q_{n}^{b})_{0}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & \{(ω_{b/i}^{b})_{X}− (ω_{e/i}^{n}+ ω_{n/e}^{n})_{X}\} /2 & 0 & \{(ω_{b/i}^{b})_{Z}+ (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Z}\} /2 & \{−(ω_{b/i}^{b})_{Y}− (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Y}\} /2 \\ \frac{−(q_{n}^{b})_{0}(ṙ_{e}^{n})_{X}−(q_{n}^{b})_{3}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & \{(ω_{b/i}^{b})_{Y}− (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Y}\} /2 & \{−(ω_{b/i}^{b})_{Z}− (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Z}\} /2 & 0 & \{(ω_{b/i}^{b})_{X}+ (ω_{e/i}^{n}+ ω_{n/e}^{n})_{X}\} /2 \\ \frac{(q_{n}^{b})_{1}(ṙ_{e}^{n})_{X}+(q_{n}^{b})_{2}(ṙ_{e}^{n})_{Y}}{2(r_{e}+h)^{2}} & \{(ω_{b/i}^{b})_{Z}− (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Z}\} /2 & \{(ω_{b/i}^{b})_{Y}+ (ω_{e/i}^{n}+ ω_{n/e}^{n})_{Y}\} /2 & \{−(ω_{b/i}^{b})_{X}− (ω_{e/i}^{n}+ ω_{n/e}^{n})_{X}\} /2 & 0 \end{matrix} ]

(3.2.13)

24

B = [ \begin{matrix}(q_{n}^{b})_{0}^{2}+ (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}− (q_{n}^{b})_{3}^{2} & 2((q_{n}^{b})_{1}(q_{n}^{b})_{2}− (q_{n}^{b})_{0}(q_{n}^{b})_{3}) & 2((q_{n}^{b})_{1}(q_{n}^{b})_{3}+ (q_{n}^{b})_{0}(q_{n}^{b})_{2}) & 0 & 0 & 0 & 0 \\ 2((q_{n}^{b})_{1}(q_{n}^{b})_{2}+ (q_{n}^{b})_{0}(q_{n}^{b})_{3}) & (q_{n}^{b})_{0}^{2}− (q_{n}^{b})_{1}^{2}+ (q_{n}^{b})_{2}^{2}− (q_{n}^{b})_{3}^{2} & 2((q_{n}^{b})_{2}(q_{n}^{b})_{3}− (q_{n}^{b})_{0}(q_{n}^{b})_{1}) & 0 & 0 & 0 & 0 \\ 2((q_{n}^{b})_{1}(q_{n}^{b})_{3}− (q_{n}^{b})_{0}(q_{n}^{b})_{2}) & 2((q_{n}^{b})_{2}(q_{n}^{b})_{3}+ (q_{n}^{b})_{0}(q_{n}^{b})_{1}) & (q_{n}^{b})_{0}^{2}− (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}+ (q_{n}^{b})_{3}^{2} & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & −(q_{n}^{b})_{1} /2 & −(q_{n}^{b})_{2} /2 & −(q_{n}^{b})_{3} /2 & 0 \\ 0 & 0 & 0 & (q_{n}^{b})_{0} /2 & −(q_{n}^{b})_{3} /2 & (q_{n}^{b})_{2} /2 & 0 \\ 0 & 0 & 0 & (q_{n}^{b})_{3} /2 & (q_{n}^{b})_{0} /2 & −(q_{n}^{b})_{1} /2 & 0 \\ 0 & 0 & 0 & −(q_{n}^{b})_{2} /2 & (q_{n}^{b})_{1} /2 & (q_{n}^{b})_{0} /2 & 0 \end{matrix} ]  (3.2.14)

25

3.3 観測方程式

本研究で用いる INS/GPS では、 GPS により INS によってえられた情報の修正を行う。そのための観測方程式を本節では導く。

GPS によって得られる情報は以下のとおりである。

• 位置、すなわち緯度 (ϕ) ・経度 (λ) ・高度 (h)

• 速度、すなわち速さ・速さの真方位

ここで線形化した運動方程式 (3.2.10) の形にあわせ、真値との残差の方程式をえるために、次の操作を行う。ただし、ここでは INS、 GPS の出力であることを協調するためにそれぞれ INS、 GPS の添字をするものとする。

• GPS からは現在の緯度 (ϕ) ・経度 (λ) が得られるので、これと INS の Azimuth 角 (α) がわかれば、 GPS から q̃_{e}^{n}_{GPS}を作成することができる。これと INS の q̃_{e}^{n}_{INS} を比較したものを観測量その 1 とする。

• GPS からは現在の高度 (h_{GPS}) が得られるので、これと INS が出力する高度 (h_{INS}) を比較したものを観測量その 2 とする。

• GPS からは現在の速度が得られるので、 INS より Azimuth 角 (α) が得られれば、 n-Frame における速度 (ṙ_{e}^{n}_{GPS})_{X}、 (ṙ_{e}^{n}_{GPS})_{Y} が得られる。これと (ṙ_{e}^{n}_{INS})_{X}、 (ṙ_{e}^{n}_{INS})_{Y} を比較したものを観測量その 3 とする。

以上まとめて書くと、次の観測方程式が得られる。

\underline{z}= H\underline{x}+ \underline{v}  (3.3.1)

ここで\underline{x}は (3.2.4) と同じであり、\underline{v} は観測誤差である。また\underline{z} は

\underline{z}= [ \begin{matrix}(ṙ_{e}^{n}_{GPS})_{X}− (ṙ_{e}^{n}_{INS})_{X} \\ (ṙ_{e}^{n}_{GPS})_{Y}− (ṙ_{e}^{n}_{INS})_{Y} \\ (q_{e}^{n}_{GPS})_{0}− (q_{e}^{n}_{INS})_{0} \\ (q_{e}^{n}_{GPS})_{1}− (q_{e}^{n}_{INS})_{1} \\ (q_{e}^{n}_{GPS})_{2}− (q_{e}^{n}_{INS})_{2} \\ (q_{e}^{n}_{GPS})_{3}− (q_{e}^{n}_{INS})_{3} \\ h_{GPS}− h_{INS} \end{matrix} ]  (3.3.2)

行列 H は

H = [ \begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \end{matrix} ]  (3.3.3)

3.4 INS/GPS アルゴリズム

本節では INS/GPS として提案するアルゴリズムについて述べる。

まず、状態の初期値を設定するために、初期化を行う。この状態を Initialize 状態とよぶことにする。初期化が完了すると Flight 可能なり、その状態を Flight 時と称することにする。さらに Flight 時では、 INS を利用し時間で更新を行うときと、 GPS から情報を取得し状態量を修正するときの 2 つの状態にわけることができる。前者を Update 時、 Correct 時と称することにする。以下、これら 3 つの状態ごとにアルゴリズムの詳細を述べる。

3.4.1 Initialize 状態

状態の初期値を設定するのが Initialize 状態である。設定すべき値は INS の状態量である速度 (ṙ_{e}^{n})、位置 (q̃_{e}^{n})、姿勢 (q̃_{n}^{b}) である。

簡単のために、この状態においては静止状態におく、すなわち ṙ_{e}^{n} = 0 になっているものとする。また位置 ṙ_{e}^{n} については GPS を利用して取得することにする。 GPS は静止状態に数分間置くことにより、非常によい精度 (数 m 以下のオーダー) で現在位置を取得可能であることが知られている。姿勢については、静止状態であるから重力方向から 3 自由度のうち 2 自由度を決定することが可能であり、残りの 1 自由度については Initialize 時を経過し Filight 時になった直後の GPS の速度方向をもって決定することにする。

また INS/GPS で Kalman Filter を行うために必要となる、 INS の状態量と真値との誤差について共分散をとった誤差共分散行列 (P̄) を適当な値の対角行列で初期化する。

3.4.2 Flight 状態:Update 時

INS を時間更新する状態が Update 時である。更新する状態量は INS の状態量である速度 (ṙ_{e}^{n})、位置 (q̃_{e}^{n})、姿勢 (q̃_{n}^{b})、および INS/GPS の Kalman Filter における誤差共分散行列 (P̄) である。

• INS の状態量 (ṙ_{e}^{n}、 q̃_{n}^{b}、 q̃_{n}^{b}) の更新 INS 用の慣性センサである加速度計とジャイロの出力を 3.1.1 節で導いた運動方程式を使用し更新時間間隔で積分することにより、 INS の状態量を更新する。

• 誤差共分散行列 (P̄) の更新 INS の状態量を用いて (2.4.8) で誤差共分散行列 (P̄) を更新する。式中の Φ や Γ は (2.5.4)、 (2.5.5)、 (3.2.13)、 (3.2.13) で計算される。

図 3.2 Update 時

3.4.3 Flight 状態:Correct 時

GPS から情報が取得できたとき、 GPS から得られた情報をもとに Kalman Filter を用いて補正を行うときが Correct 時である。このときは GPS の観測値と INS の状態量の差を用いて、誤差共分散行列 (P̄) の更新、および INS の状態量である速度 (ṙ_{e}^{n})、位置 (q̃_{e}^{n})、姿勢 (q̃_{n}^{b}) の補正を行う。

• 誤差共分散行列 (P̄) の更新誤差共分散行列 (P̄) を (2.4.17) で更新する。このとき GPS の観測量と INS の状態量の差から 3.3 節で述べた観測方程式を導いて用いる。

• INS の状態量 (ṙ_{e}^{n}、 q̃_{n}^{b}、 q̃_{n}^{b}) の更新 GPS の観測量と INS の状態量の差、ならびに誤差共分散行列 (P̄) の更新の際に得られたカルマンゲイン、観測方程式用いて (2.4.9) で INS の状態量を更新する。

図 3.3 Correct 時

29

第 4 章

数値シミュレーション

本章ではコンピュータを用いて行った数値シミュレーションについて述べる。数値シミュレーションを行う意義は、実機試験の成否を論じるための対象としてだけでなく、実

機に近い数値モデルを獲得できたかどうか論じるためでもある。前者はこの数値シミュレーションという再現性がある安定した環境と外乱の極めて多い実機試験を行った環境の差について考察するためである。後者は実機試験を行う前にシミュレーションという形で数値モデルが得られれば、今後実機試験以前にある程度結果の先読みができることにつながるためである。

このような考えのもとに数値シミュレーションを行った。

4.1 方法

前章で構築したアルゴリズムに対し仮想的な INS 用 MEMS 慣性センサ (加速度計、ジャイロ)、および GPS の出力値を適用することにより、シミュレーションを行った。

シミュレーションは次の条件下で行った。

• 水平等速旋回軌道を仮定半径 100m の円上を 120 秒で水平等速旋回飛行で 1 周する。このとき機速は 100[m]× 2π/120 [sec]≈ 5[m/s]で、実際の UAV の飛行速度に近い速度とした。このような軌道を仮定したのは常に加速度が加わっている状態にあることにより位置や速度に対し慣性センサの誤差の影響を結果に大きく反映することができるためである。

• 初期状態シミュレーション開始時では INS の状態量は真値であり、既に Flight 状態で開始するものとする。これは Initialize 状態に十分な時間をかけることによりこのような仮定を実現できると考えたためである。

• GPS GPS は 1Hz で位置情報、速度情報が得られるものとする。ただし得られる情報は真値に対して水平面上で約 20m、高度で約 30m、速さで約 1m/s、速度方向で約 5^{∘} のホワイトノイズ

を含むものとした。

• 慣性センサ慣性センサは 2 つのモデルを作成した。 1 つは誤差として単純なホワイトノイズのみを含むモデルであり、もう一方はホワイトノイズに加えランダムドリフトまで考慮したモデルである。前者は理想的な慣性センサ、つまりはランダムドリフトのまったくない非常に精度のよいセンサであり、後者はより現実に近いモデルである。実際の慣性にはこの他にも温度によるドリフト効果や他軸相関、ミスアライメントなど様々な誤差要因が存在するが、これらは十分なキャリブレーションを行うことによってある程度除去できることが知られており、従ってここではホワイトノイズならびにランダムドリフトのみを考慮することにした。モデルの詳細については小節にて述べるが、 INS/GPS のアルゴリズムである Kalman Filter をもってすればホワイトノイズによる誤差は観測が得られることにより除去が可能である。つまりは両者を比較することにより、ランダムドリフトがどの程度悪影響を与えるかを知ることが可能である。

• 誤差共分散行列 Q、 R INS/GPS の Kalman Filter における誤差共分散行列 Q、 R はそれぞれセンサ、 GPS のホワイトノイズの分散が計測できたものとして、その値を用いることにした。慣性センサモデル 1 と慣性センサモデル 2 ではこれらの値は同じである。

• アルゴリズム INS のみの場合、すなわち単純にセンサの出力データを時間積分する場合と、 INS/GPS の場合、すなわち GPS による補正を行う場合を両センサモデルに対して適用した。

4.1.1 慣性センサモデル 1: 単純なホワイトノイズのみの場合

慣性センサのモデルとして単純なホワイトノイズのみを考えたモデルである。ホワイトノイズとは時間が変化しても時間とは無関係に加わるノイズであり、その性質、すなわちノイズの偏差が事前に測定できれば Kalman Filter によって除去できることが期待される。

慣性センサのホワイトノイズの偏差 (σ_{WN}) は通常次式であらわされる。

σ_{WN}= σ_{1Hz}^{\sqrt{f}}  (4.1.1)

ここで σ_{1Hz} は 1Hz でセンサの出力値を取得したときに発生するホワイトノイズの偏差であり、 f はセンサの出力値を取得するサンプリング周波数である。つまりサンプリング周波数が高ければ高いほど出力値にホワイトノイズが加わることを示している。

ここでは市場に数多く流通している Analog Device 社製の MEMS 加速度計 (製品名 ADXL103)、 MEMS ジャイロ (製品名 ADXRS150) を用いたとし、カタログスペックから

(σ_{1Hz})_{ADXL103}= 110[μg]≈ 10^{−3} [m/s^{2}]
(σ_{1Hz})_{ADXRS150}= 0.05[^{∘}/s]≈ 10^{−3} [rad/s]

であるから、これらの値を用いて真値に対してホワイトノイズを加えたものを慣性センサモデル 1 とする。

図 4.1 慣性センサモデル 1 の処理過程図

4.1.2 慣性センサモデル 2: ジャイロのランダムドリフトまで考慮した場合

慣性センサモデル 2 では慣性センサモデル 1 で考慮したホワイトノイズに加え、ジャイロのランダムドリフトを考慮する。

ランダムドリフト (x_{drift}) は時間に関係する過程であり以下の式でよくモデル化されることが知られている。

\frac{d}{dt}x_{drift}= −βx_{drift}(t) + w(t)  (4.1.2)

ここで β は定数であり、 w(t) は以下の関係を満たすホワイトノイズである。

E[w(t)w(τ)] = Nδ(t − τ)  (4.1.3)

すなわち β が 0 のとき完全なランダムウォークとなる。これを離散系に変換すると

x_{drift}_{k+1} = −βx_{drift}Δt + \sqrt{N}u_{k+1} Δ\sqrt{t}  (4.1.4)

となる。ここで u_{k+1} は単位ホワイトノイズ、すなわち偏差が 1 のホワイトノイズである。

通常ランダムドリフトは加速度計に比べジャイロに対して効果が大きいので、本モデルではジャイロに対してのみランダムドリフトを考慮することにする。また慣性センサモデル 1 と同様に MEMS ジャイロとして Analog Devices 社製の MEMS ジャイロ (製品名 ADXRS150) を仮定する。しかしながらカタログにはランダムドリフトのスペックが記されていないので、実機を計測することによりランダムドリフトの定数 (β ならびに\sqrt{N}) を決定することにした。

以上より慣性センサモデル 2 のジャイロでは図 4.2 のようになる。

定数の決定方法は以下の手順を踏んだ。

1. センサを 1 時間静止状態に置いたまま、 100Hz でサンプリングを行う。この際外部環境も温度がかわらないようにするなどして外乱の影響を極力排除する。

2. Allan Variance を用いて計測データを処理し、計測データからホワイトノイズとランダムドリフトを区別できるようにする。

3. 定数 β、\sqrt{N} を決定したセンサモデルを作成し、静置状態に置いたとしてそのモデルから 100Hz でサンプリングを行う。

図 4.2 慣性センサモデル 2 におけるジャイロの処理過程図

4. 3 でサンプリングしたデータに対して 2 同様 Allan Variance をとる。

5. 2 と 4 の Allan Variance の傾向が一致するまで 3、 4 を繰り返す。

Allan Variance についての詳細は付録 D で述べるが、一定のサンプリング間隔ごとに分散をとることによってデータの時間に依存する部分を定性的にみることが可能になる手法である。

これより慣性センサモデル 2 におけるジャイロのランダムドリフトの定数 β_{Gyro}、\sqrt{N}_{Gyro} は以下のとおり決定した。

β_{Gyro}= 0.16
\sqrt{N}_{Gyro}= 0.00008 / SF_{ADXRS150}[rad/s]
SF_{ADXRS150}= 12.5 [mV/deg/s]

SF_{ADXRS150}は ADXRS150 の Scale Factor である。

図 4.3、図 4.4 はそれぞれ ADXRS150 実機、ならぴに慣性センサモデル 2 のジャイロの静置状態における Allan Variance である。縦軸はサンプリング時間間隔、横軸は Allan Variance である。図 4.3、図 4.4 は時間間隔が大きい部分でよく一致していることから、このモデルはランダムドリフトをよく実機により近いものになったと考えることができる。時間間隔が小さい部分では再現性に多少の問題があるが、これは実機には Low Pass Filter が入っているためであると推測される。本シミュレーションではランダムドリフトの効果について重きをおいているのでこれは問題にはならないと思われる。また図 4.5 は慣性センサモデル 1 の Allan Variance であるが、時間間隔が大きい部分で一致していない。

4.2 結果

シミュレーション結果を記す。

図 4.3 ADXRS150 の Allan Variance 図 4.4 慣性センサモデル 2 のジャイロの Allan Variance

図 4.5 慣性センサモデル 1 のジャイロの Allan Variance

4.2.1 位置履歴

図 4.6 が水平面上履歴、図 4.7 が高度履歴である。 MEMS 慣性センサの性能では理想化された慣性センサモデル 1 の場合でさえ、 1 周できていな

いことが図 4.6 から読み取れる。一方、慣性センサモデル 1 での INS/GPS では真値に近い値で 1 周できており、また慣性センサモデル 2 では INS/GPS は 3/4 周まではそれに近い値を出していることがわかる。両者の違いがランダムドリフトの効果であると考えられる。

また高度についてはリファレンスである GPS の誤差が標準偏差にして 30m と大きい割にはよく推定できていると考えられる。

4.2.2 速度履歴

図 4.8、図 4.9、図 4.10 が北方向、東方向、重力方向に対する速度履歴である。リファレンスが存在する北方向、東方向速度については INS/GPS においてどちらのセンサモデ

ルでも推定がよく行われていることが確認できる。

図 4.6 水平面上履歴図 4.7 高度履歴

36

図 4.8 北方向速度履歴図 4.9 東方向速度履歴

37

図 4.10 重力方向速度履歴

4.2.3 姿勢履歴

図 4.11、図 4.12、図 4.13 がヨー、ピッチ、ロール履歴である。

どちらのセンサモデルにおいても INS/GPS で真値に収束する傾向がない。

4.3 考察

4.3.1 センサモデルについて

全ての結果において単純積分、すなわち INS のみの慣性センサモデル 1 と慣性センサモデル 2 についての大きな違いをみることができる。これによるといかにランダムドリフトの効果がいかに無視できないものであるか知ることができる。従ってセンサを用いたシミュレーションを行う場合はランダムドリフトまで考慮しなければならないであろう。

またランダムドリフトが INS/GPS のアルゴリズムである Kalman Filter の推定に対しても悪影響を与えていることが見て取れる。例えば、図 4.6 ではドリフトなし (慣性センサモデル 1) の INS/GPS では真値に近い値へと 1 周かけて収束していっていることが見て取れるが、 INS/GPS では残り 1/4 周では真値から逆に離れていってしまっている。このことは Kalman Filter が誤差成分

図 4.11 ヨー履歴図 4.12 ピッチ履歴

39

図 4.13 ロール履歴

としてホワイトノイズであることを仮定していることによるものだと思われる。

4.3.2 収束性について

リファレンスがある位置 (図 4.6、図 4.7)、水平面上速度 (図 4.8、図 4.9) については INS/GPS の結果が真値に収束する傾向をみてとれることから、 Kalman Filter が効果的に働いていると考えられる。一方、リファレンスが存在しない姿勢については全くといっていいほど INS/GPS において推定が良く行われていない。

このことから考えると、この姿勢の推定のずれによって他の状態量である速度や位置に対して悪影響が及ぼされるであろう。つまり本シミュレーションでは 2 分と短時間の運用のため速度や位置に対して真値への収束傾向をみてとることができたが、運用時間がながくなるにつれ発散傾向をしめすことが考えられる。実際、本シミュレーションと同じ条件にて 5 周、すなわち 10 分間の運用を想定してシミュレーションした結果が図 4.14 である。全くといっていいほど収束していない。これは姿勢の推定に失敗していることが原因である。例えばロール履歴は図 4.15 である。

従って MEMS 慣性センサという性能があまり期待できないセンサを用いてより長い時間の運用を行う場合は、 INS/GPS だけでなくなんらかの姿勢に対する観測手段をいれなければ不可能であると考えられる。

図 4.14 5 周 (10 分間) の運用を仮定した場合の水平面上履歴図 4.15 5 周 (10 分間) の運用を仮定した場合のロール履歴

41

第 5 章

実機試験

5.1 機材 5.2 方法 5.3 結果

第 6 章

まとめ

本研究では数分間の運用に耐えうるシステムを構築する予定であったが、 INS/GPS また、 MEMS センサの性能は数分程度の慣性航法を行うのにも適していないことが明らかになった。しかし MEMS 技術はまだ発展途上の技術であり、これからの性能向上に大きな期待が寄せられる。

付録 A

Quaternion Algebra

本研究では、 3 次元空間における座標変換を行う必要がある。座標変換を行う方法としては、代表的なものとして

1. Euler 角を使用する方法

2. Direction Cosine Matrix(DCM) を使用する方法

3. Quaternion を使用する方法

があげられる。これらは以下の特徴をもつ。

表 A.1 座標変換の方法

方法  1. Euler 角  2. DCM  3. Quaternion
変数の個数  3  9  4
長所変数の数が最小特異点は存在しない、行 列計算であるため理解 しやすい{特異点は存在しない、変 \atop 数の数は 4 つと少ない}

短所特異点が存在するため、 演算が成り立つか常に 確かめる必要がある{変数の個数が多いため \atop 計算負荷が大きい}理解しにくい

変数の数が少ないほど、経験的に数値演算誤差が少ないとされている。また演算が成り立つかどうかを調べることは、計算回数が増すにつれ計算負荷が大きくなることを意味する。本研究では、第??章で述べるような Kalman Filtering を導入するが、これにはかなりの計算負荷が予想される。一方、数値演算の精度向上もしたい。従って本研究では、数値誤差および演算負荷の軽減という観点から Quaternion による方法を採用することにした。

本章では、 Quaternion による演算について言及することにする。なお、数学的な厳密さを求めたものではなく、本研究で用いる計算方法の説明として考えて頂きたい。

以下、 Quaternion の表記として、 q̃または\{ \begin{matrix}q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}\} のようなを用いることにする。また 3 次元ベ

クトルの表記として x⃗または( \begin{matrix}x_{1} \\ x_{2} \\ x_{3} \end{matrix}) のようなを用いることにする。加えて、 \{ q_{0} ; p⃗\} や\{ \begin{matrix}& q_{0} \\ ( & \begin{matrix}p_{1} \\ p_{2} \\ p_{3} \end{matrix} & ) \end{matrix} \}

のような表記は、 Quaternion ではあるが p⃗ および p_{1}、 p_{2}、 p_{3} を 3 次元ベクトルとみなした表記である。

A.1 Quaternion の定義

Quaternion は互いに 1 次独立である基底 111,iii,jjj,k k kを用いて

q̃≡ \{ \begin{matrix}q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}\} ≡ 111∗ q_{0}+ iii∗ q_{1}+ jjj ∗ q_{2}+ k k k∗ q_{3}  (A.1.1)

と定義される。基底 111,iii,jjj,k k kには以下の関係が存在する。

111∗ 111 = 111  (A.1.2)
iii∗ iii = jjj ∗ jjj = k k k∗ k k k= −111  (A.1.3)
iii∗ jjj = −(jjj ∗ iii) = k k k  (A.1.4)
jjj ∗ k k k= −(k k k∗ jjj) = iii  (A.1.5)
k k k∗ iii = −(iii∗ k k k) = jjj  (A.1.6)

A.2 共役 Quaternion の定義

共役 Quaternion q̃^{∗} とは

q̃^{∗} ≡ \{ \begin{matrix}q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}\} ^{∗} = \{ \begin{matrix}q_{0} \\ −q_{1} \\ −q_{2} \\ −q_{3} \end{matrix} \}  (A.2.1)

A.3 Quaternion のスカラー積

Quarternion はスカラーとの演算が定義される。 Quarternion q̃≡ \{ \begin{matrix}q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}\} とし、スカラーを a とす

れば

q̃∗ a ≡ a ∗ q̃≡ \{ \begin{matrix}q_{0}∗ a \\ q_{1}∗ a \\ q_{2}∗ a \\ q_{3}∗ a \end{matrix}\}  (A.3.1)

A.4 Quaternion 間の加算

Quaternion は Quaternion 間の加算が定義される。

q̃_{a} ≡ \{ \begin{matrix}q_{a0} \\ q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}\} , q̃_{b} ≡ \{ \begin{matrix}q_{b0} \\ q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}\}  (A.4.1)

であるとき、

\begin{aligned}q̃_{a}+ q̃_{b} & = (111∗ q_{a0}+ iii∗ q_{a1}+ jjj ∗ q_{a2}+ k k k∗ q_{a3}) + (111∗ q_{b0}+ iii∗ q_{b1}+ jjj ∗ q_{b2}+ k k k∗ q_{b3}) \\ & = 1 ∗ (q_{a0}+ q_{b0}) + i ∗ (q_{a1}+ q_{b1}) + j ∗ (q_{a2}+ q_{b2}) + k ∗ (q_{a3}+ q_{b3}) \\ & ≡ \{ \begin{matrix}q_{a0}+ q_{b0} \\ q_{a1}+ q_{b1} \\ q_{a2}+ q_{b2} \\ q_{a3}+ q_{b3} \end{matrix} \} \end{aligned}  (A.4.2)

この演算は可換である。すなわち、

q̃_{b}+ q̃_{a} = q̃_{a}+ q̃_{b}  (A.4.3)

A.5 Quaternion 間の乗算

Quaternion は Quaternion 間の乗算が定義される。

q̃_{a} ≡ \{ \begin{matrix}q_{a0} \\ q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}\} , q̃_{b} ≡ \{ \begin{matrix}q_{b0} \\ q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}\}  (A.5.1)

であるとき、

\begin{aligned}q̃_{a}q̃_{b} & = (1 ∗ q_{a0}+ i ∗ q_{a1}+ j ∗ q_{a2}+ k ∗ q_{a3}) (1 ∗ q_{b0}+ i ∗ q_{b1}+ j ∗ q_{b2}+ k ∗ q_{b3}) \\ & = 1 ∗ (q_{a0}∗ q_{b0}− q_{a1}∗ q_{b1}− q_{a2}∗ q_{b2}− q_{a3}∗ q_{b3}) \\ & + i ∗ (q_{a0}∗ q_{b1}+ q_{a1}∗ q_{b0}+ q_{a2}∗ q_{b3}− q_{a3}∗ q_{b2}) \\ & + j ∗ (q_{a0}∗ q_{b2}− q_{a1}∗ q_{b2}+ q_{a2}∗ q_{b0}+ q_{a3}∗ q_{b1}) \\ & + k ∗ (q_{a0}∗ q_{b3}+ q_{a1}∗ q_{b2}− q_{a2}∗ q_{b1}+ q_{a3}∗ q_{b0}) \\ & ≡ \{ \begin{matrix}q_{a0}∗ q_{b0}− q_{a1}∗ q_{b1}− q_{a2}∗ q_{b2}− q_{a3}∗ q_{b3} \\ q_{a0}∗ q_{b1}+ q_{a1}∗ q_{b0}+ q_{a2}∗ q_{b3}− q_{a3}∗ q_{b2} \\ q_{a0}∗ q_{b2}− q_{a1}∗ q_{b2}+ q_{a2}∗ q_{b0}+ q_{a3}∗ q_{b1} \\ q_{a0}∗ q_{b3}+ q_{a1}∗ q_{b2}− q_{a2}∗ q_{b1}+ q_{a3}∗ q_{b0} \end{matrix} \} \end{aligned}  (A.5.2)

この結果から、 3 次元ベクトルの内積、および外積の定義を用いて Quaternion 間の乗算を定義することができる。ただし、内積は ·、外積は × であらわすものとする。

\begin{aligned}q̃_{a}q̃_{b} & = \{ \begin{matrix}& q_{a0} \\ ( & \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix} & ) \end{matrix} \} \{ \begin{matrix}& q_{b0} \\ ( & \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix} & ) \end{matrix} \} \\ & ≡ \{ \begin{matrix}q_{a0}∗ q_{b0}− ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) · ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) \\ q_{a0} ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) + q_{b0} ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) + ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) × ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) \end{matrix} \} \end{aligned}  (A.5.3)

この演算は可換ではない。すなわち、

\begin{aligned}q̃_{b}q̃_{a} & = \{ \begin{matrix}q_{b0}∗ q_{a0}− ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) · ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) \\ q_{b0} ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) + q_{a0} ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) + ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) × ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) \end{matrix} \} \\ & = \{ \begin{matrix}q_{a0}∗ q_{b0}− ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) · ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) \\ q_{a0} ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) + q_{b0} ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) − ( \begin{matrix}q_{a1} \\ q_{a2} \\ q_{a3} \end{matrix}) × ( \begin{matrix}q_{b1} \\ q_{b2} \\ q_{b3} \end{matrix}) \end{matrix} \} \\ & ≠ q̃_{a}q̃_{b} \end{aligned}  (A.5.4)

A.6 Quaternion による 3 次元ベクトルの回転

本項では、ベクトルによる演算と Quaternion による演算の結果が一致することを示し、Quaternion の演算により 3 次元ベクトルの回転を行うことが可能であることを示す。

3 次元ベクトル p⃗ を 3 次元単位ベクトル r⃗( ただしr⃗·r⃗ = 1) を軸として、 θ だけ回転した 3 次元ベクトル p′はベクトルの演算より次のように求めることができる。

p′= cosθ ∗ p⃗_{⊥} + sinθ ∗ q⃗+ p⃗_{∥}  (A.6.1)

ただし、 p⃗_{⊥} 、 p⃗_{∥} はそれぞれ p⃗ の r⃗ に対する垂直成分、水平成分とする。すなわち、

p⃗_{⊥} ≡ p⃗− p⃗_{∥}, p⃗_{∥} ≡ (r⃗· p⃗)r⃗  (A.6.2)

また、 q⃗ は p⃗_{⊥} を r⃗ を軸に 90 度回転したものとする。すなわち、

q⃗≡ p⃗_{⊥} ×r⃗  (A.6.3)

従って p′を p⃗ と r⃗ を用いてあらわすと以下のようになる。

p′= cosθ ∗ p⃗_{⊥} + sinθ ∗ q⃗+ p⃗_{∥}

= cosθ ∗ p⃗_{⊥} + sinθ ∗ (p⃗_{⊥} ×r⃗) + p⃗_{∥} = cosθ (p⃗− p⃗_{∥}) + sinθ ((p⃗− p⃗_{∥}) ×r⃗) + p⃗_{∥} = cosθ ∗ p⃗+ (1 − cosθ) (r⃗· p⃗)p⃗+ sinθ (p⃗×r⃗)

(A.6.4)

ここで p̃≡ \{ 0 ; p⃗\} と q̃≡ \{ \begin{matrix}cos & \frac{θ}{2} \\ sin \frac{θ}{2} ∗r⃗ \end{matrix}\} による次の演算を考えてみる。

q̃^{∗}p̃q̃≡ \{ \begin{matrix}cos & \frac{θ}{2} \\ −sin \frac{θ}{2} ∗r⃗ \end{matrix} \} \{ 0 ; p⃗\} \{ \begin{matrix}cos & \frac{θ}{2} \\ sin \frac{θ}{2} ∗r⃗ \end{matrix}\}

= \{ \begin{matrix}sin & \frac{θ}{2} & (r⃗· p⃗) \\ cos \frac{θ}{2} ∗ p⃗− sin \frac{θ}{2} (r⃗× p⃗) \end{matrix} \} \{ \begin{matrix}cos & \frac{θ}{2} \\ sin \frac{θ}{2} ∗r⃗ \end{matrix}\}

= \{ \begin{matrix}sin \frac{θ}{2} cos \frac{θ}{2} (r⃗· p⃗)− (cos \frac{θ}{2} ∗ p⃗− sin \frac{θ}{2} (r⃗× p⃗) ) · (sin \frac{θ}{2} ∗r⃗) \\ sin^{2} \frac{θ}{2} (r⃗· p⃗)r⃗+ cos \frac{θ}{2} (cos \frac{θ}{2} ∗ p⃗− sin \frac{θ}{2} (r⃗× p⃗) ) − (cos \frac{θ}{2} ∗ p⃗+ sin \frac{θ}{2} (r⃗× p⃗) ) × (sin \frac{θ}{2} ∗r⃗) \end{matrix} \}

= \{ \begin{matrix}0 \\ sin^{2} \frac{θ}{2} (r⃗· p⃗)r⃗+ cos^{2} \frac{θ}{2} ∗ p⃗− 2sin \frac{θ}{2} cos \frac{θ}{2} (r⃗× p⃗) − sin^{2} \frac{θ}{2} (r⃗× p⃗×r⃗) \end{matrix} \}

( ∵ (r⃗× p⃗) ·r⃗ = 0)

= \{ \begin{matrix}0 \\ sin^{2} \frac{θ}{2} (r⃗· p⃗)r⃗+ cos^{2} \frac{θ}{2} ∗ p⃗− 2sin \frac{θ}{2} cos \frac{θ}{2} (r⃗× p⃗) − sin^{2} \frac{θ}{2} ((r⃗·r⃗)p⃗− (r⃗· p⃗)r⃗) \end{matrix} \}

(∵ r⃗× p⃗×r⃗ = (r⃗·r⃗)p⃗− (r⃗· p⃗)r⃗)

= \{ \begin{matrix}0 \\ 2sin^{2} \frac{θ}{2} (r⃗· p⃗)r⃗+ (cos^{2} \frac{θ}{2} − sin^{2} \frac{θ}{2}) p⃗− 2sin \frac{θ}{2} cos \frac{θ}{2} (r⃗× p⃗) \end{matrix} \}

= \{ \begin{matrix}0 \\ cosθ ∗ p⃗+ (1 − cosθ) ∗ (r⃗· p⃗)r⃗+ sinθ ∗ (p⃗×r⃗) \end{matrix} \}

≡ \{ \begin{matrix}0 \\ & p′ \end{matrix}\}

(A.6.5)

以上の計算結果から、 Quaternion p̃および q̃を用いて演算を行うことにより、 p⃗ を回転した p′を求めることが可能であることが示された。従って、 3 次元ベクトルの回転は Quaternion の演算により求めることが可能である。

以後、

\begin{aligned}q̃ & ≡ \{ \begin{matrix}cos & \frac{θ}{2} \\ sin \frac{θ}{2} ∗r⃗ \end{matrix}\} \\ & ( ただし、r⃗·r⃗ = 1) \end{aligned}  (A.6.6)

のような形の Quaternion q̃を回転 Quaternion と呼ぶことにする。回転 Quaternion の性質として、回転 Quaternion q̃に対してその共役 Quarternion q̃^{∗} は

q̃^{∗} ≡ \{ \begin{matrix}cos & \frac{θ}{2} \\ sin \frac{θ}{2} ∗r⃗ \end{matrix}\} ^{∗}

= \{ \begin{matrix}cos & \frac{θ}{2} \\ −sin \frac{θ}{2} ∗r⃗ \end{matrix} \}

= \{ \begin{matrix}cos & \frac{−θ}{2} \\ sin \frac{−θ}{2} ∗r⃗ \end{matrix}\}

(A.6.7)

すなわち共役を考えることにより、逆回転を表現することができることが示された。

A.7 Quaternion による回転の合成

前項 A.6 では、Quaternion により 3 次元ベクトルの回転が可能であることを示したが、Quaternion を用いることにより回転の合成を行うことも可能である。

例えば、回転 Quaternion q̃_{1} と q̃_{2} が存在したとする。そのとき、 { 0 ; p⃗} を q̃_{1} で回転したあと、

q̃_{2} で回転した \{ \begin{matrix}0 \\ & p′ \end{matrix}\} は、

\begin{aligned}\{ \begin{matrix}0 \\  & p′ \end{matrix}\} & = q̃_{2}^{∗}( q̃_{1}^{∗}\{ 0 ; p⃗\} q̃_{1}) q̃_{2} \\ & = (q̃_{1}^{∗}q̃_{2}^{∗})\{ 0 ; p⃗\} (q̃_{1}q̃_{2}) \\ & = (q̃_{1}q̃_{2})^{∗}\{ 0 ; p⃗\} (q̃_{1}q̃_{2}) \\ & ≡ q̃_{12}^{∗}\{ 0 ; p⃗\} q̃_{12}  (q̃_{12} ≡ q̃_{1}q̃_{2}) \end{aligned}  (A.7.1)

で求めることが可能である。すなわち、 q̃_{12} ≡ q̃_{1}q̃_{2} なる回転 Quaternion を考えればよい。

A.8 Quaternion の時間微分

Quaternion q̃の時間微分は次の演算により求められることが知られている。

\frac{d}{dt}q̃≡ ˙ q̃ = \frac{1}{2} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}ω_{1} \\ ω_{2} \\ ω_{3} \end{matrix} & ) \end{matrix} \} q̃  (A.8.1)

ここで( \begin{matrix}ω_{1} \\ ω_{2} \\ ω_{3} \end{matrix} ) は 3 軸まわりの回転角速度を意味する。

A.9 回転 Quaternion と Direction Cosine Matrix の関係

A.6 で回転 Quaternion により 3 次元ベクトルの回転が行えることを示したが、本章の冒頭にて Direction Cosine Matrix(DCM) によっても 3 次元ベクトルの回転が行えることを述べた。実際、両者には以下の関係が存在する。

回転 Quaternion を q̃≡ \{ \begin{matrix}q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}\} とすれば、対応する DCM C は、

C = [ \begin{matrix}q_{0}^{2}+ q_{1}^{2}− q_{2}^{2}− q_{3}^{2} & 2(q_{1}q_{2}+ q_{0}q_{3}) & 2(q_{1}q_{3}− q_{0}q_{2}) \\ 2(q_{1}q_{2}− q_{0}q_{3}) & q_{0}^{2}− q_{1}^{2}+ q_{2}^{2}− q_{3}^{2} & 2(q_{2}q_{3}+ q_{0}q_{1}) \\ 2(q_{1}q_{3}+ q_{0}q_{2}) & 2(q_{2}q_{3}− q_{0}q_{1}) & q_{0}^{2}− q_{1}^{2}− q_{2}^{2}+ q_{3}^{2} \end{matrix} ]  (A.9.1)

であり、

\{ \begin{matrix}0 \\  & p′ \end{matrix}\} = q̃^{∗}\{ 0 ; p⃗\} q̃  (A.9.2)

ならば、

p′= Cp⃗  (A.9.3)

である。

付録 B

Coordinate Systems

本研究で取り扱う座標系は次の 5 つの座標系である。

1. Earth-Centered Inetial Frame (以下、 i-Frame とも表記)

2. Earth-Centered, Earth-Fixed Frame (同、 e-Frame)

3. Body Frame (同、 b-Frame)

4. Local Geodetic Frame (同、 g-Frame)

5. Navigation Frame (Wander Azimuth Frame) (同、 n-Frame)

すべて右手系の直交座標系を採用することにし、図??のように ( X 軸- Y 軸- Z 軸) を定める。表記については、例えば X^{i} 軸と書いた場合、 i-Frame における X 軸を表すとする。

以下、これらの座標系について説明を行う。また各座標系間の変換についても言及する。

B.1 Earth-Centered Inertial Frame (i-Frame)

Newton の法則が成り立つ慣性座標系のうち、地球中心を原点とし、地球の自転軸と平行になるよう Z^{i} 軸を定めた右手系直交座標系を Earth-Centered Inertial Frame(i-Frame) とする。

B.2 Earth-Centered, Earth-Fixed Frame (e-Frame)

地球中心を原点とし、 X^{e} 軸を緯度 0^{∘}、経度 0^{∘} 方向とし、かつ地球の自転軸と平行になるように Z^{e} 軸を定めた地球固定の右手系直交座標系を Eeath-Centered, Earth-Fixed Frame(e-Frame) とする。

B.3 Local Geodetic Frame (g-Frame)

機体を原点とし、北方向を X^{g} 軸とし、地球による重力方向を Z^{g} 軸とした右手形直交座標系を Local Geodetic Frame(g-Frame) とする。すなわち Y^{g} 軸は東方向を向く。

この座標系の特徴を示すために、 ( X^{g} 軸- Y^{g} 軸- Z^{g} 軸) を (North-East-Down) の頭文字をとって

図 B.1 e-Frame と g-Frame

( N 軸- E 軸- D 軸) とも表記することにする。 N 軸と E 軸がつくる平面は地表面に対して平行である。

B.4 Navigation Frame (Wander Azimuth Frame) (n-Frame)

機体を原点とし、地球による重力方向を Z^{n} 軸とした右手形直交座標系を Navigation Frame(g-Frame) とする。 g-Frame を Z^{g} 軸、すなわち D 軸を中心に α rad だけ回転したとき、この座標系、すなわち n-Frame に一致するものとする。この角度を Azimuth 角と呼び、次式によって定義される。

α̇ = λ̇ sinϕ  (B.4.1)

ただし λ̇、 ϕ はそれぞれ経度の時間変化、緯度をあらわすものとする。

なおこのような座標系を導入したのは極地方において特異点を発生することなく演算を行うためである。

B.5 Body Frame (b-Frame)

機体を原点とし、機体の進行方向を X^{b} 軸、機体の揚力方向を Z^{b} 軸とした右手系直交座標系を Body Frame(b-Frame) とする。 b-Frame において、 X^{b} 軸まわりの回転をロール、 Y^{b} 軸まわりの回転をピッチ、 Z^{b} 軸まわりの回転をヨーという。なお、本研究で開発したシステムでは、この座標系に対して慣性センサを固定する方式をとっている。

図 B.2 g-Frame と n-Frame

図 B.3 n-Frame と b-Frame

B.6 座標変換の表記

各座標系間の変換は付録 A で言及した Quaternion によって行う。それにあたって表 B.1 の表記を導入する。

式 (A.6.7) より共役 Quaternion は逆回転を表現する。従って、例えば

q̃_{i}^{e} = q̃_{e}^{i}^{∗}  (B.6.1)

の関係がある。また、式 (A.7.1) より、例えば

q̃_{i}^{b} = q̃_{i}^{e}q̃_{e}^{b} = q̃_{i}^{e}q̃_{e}^{n}q̃_{n}^{b}  (B.6.2)

表 B.1 座標変換の表記

[figure: 変換元; 変換先] i-Frame e-Frame g-Frame n-Frame b-Frame
i-Frame  q̃_{i}^{e}  q̃_{i}^{g}  q̃_{i}^{n}  q̃_{i}^{b}
e-Frame  q̃_{e}^{i}  q̃_{e}^{g}  q̃_{e}^{n}  q̃_{e}^{b}
g-Frame  q̃_{g}^{i}  q̃_{g}^{e}  q̃_{g}^{n}  q̃_{g}^{b}
n-Frame  q̃_{n}^{i}  q̃_{n}^{e}  q̃_{n}^{g}  q̃_{n}^{b}
b-Frame  q̃_{b}^{i}  q̃_{b}^{g}  q̃_{b}^{n}  q̃_{b}^{e}

B.7 i-Frame と e-Frame の関係

i-Frame と e-Frame は原点が一致し、 e-Frame は i-Frame に対して Z^{i} 軸に対して、自転周期 (Earth Rate、 Ω_{e/i} と表記) で回転している。すなわち、式 (A.8.1) から

˙q̃_{i}^{e} = \frac{1}{2} \{ \begin{matrix}& 0 \\ ( & \begin{matrix}0 \\ 0 \\ Ω_{i/e} \end{matrix} & ) \end{matrix} \} q̃_{i}^{n}  (B.7.1)

i-Frame と e-Frame は Z^{i} 軸と Z^{e} 軸が一致するから、以下の関係も成立する。

ω⃗_{i/e}^{e} = ω⃗_{i/e}^{i} = ( \begin{matrix}0 \\ 0 \\ Ω_{i/e} \end{matrix} )  (B.7.2)

B.8 e-Frame と g-Frame の関係

e-Frame と g-Frame は、 e-Frame を以下の手順で操作することによって、その軸が g-Frame の軸と平行になる。 (図 B.1 参照)

1. n-Frame の原点位置を東経 λ rad とすれば、 λ rad だけ Z^{e} 軸に沿って ( X^{e} 軸- Y^{e} 軸- Z^{e} 軸) を回転する。回転した結果 ( X^{e} 軸- Y^{e} 軸- Z^{e} 軸) は ( X^{e′} 軸- Y^{e′} 軸- Z^{e′} 軸) になったとする。

2. g-Frame の原点位置を北緯 ϕ rad とすれば、 −ϕ rad だけ Y^{e′} 軸に沿って ( X^{e′} 軸- Y^{e′} 軸- Z^{e′} 軸) を回転する。回転の結果 ( X^{e′} 軸- Y^{e′} 軸- Z^{e′} 軸) は ( X^{e′′}軸- Y^{e′′}軸- Z^{e′′}軸) になったとする。

3. Y^{e′′}軸に対して X^{e′′}軸、 Z^{e′′}軸を −90^{∘} 回転する。結果、回転された ( X^{e′′}軸- Y^{e′′}軸- Z^{e′′} 軸) は ( N 軸- E 軸- D 軸) に一致する。

この手順を、 Quaternion を用いて数式的に説明する。なお、例えば (q̃_{e}^{g})_{1} は操作 1 における回転 Quaternion とする。

(q̃_{e}^{g})_{1} = \{ \begin{matrix}cos & \frac{λ}{2} \\ sin \frac{λ}{2} ∗ ( \begin{matrix}0 \\ 0 \\ 1 \end{matrix}) \end{matrix} \} = \{ \begin{matrix}cos & \frac{λ}{2} \\ 0 \\ 0 \\ sin & \frac{λ}{2} \end{matrix} \}  (B.8.1)

(q̃_{e}^{g})_{2} = \{ \begin{matrix}cos & \frac{−ϕ}{2} \\ sin \frac{−ϕ}{2} ∗ ( \begin{matrix}0 \\ 1 \\ 0 \end{matrix}) \end{matrix} \} = \{ \begin{matrix}cos & \frac{−ϕ}{2} \\ 0 \\ sin & \frac{−ϕ}{2} \\ 0 \end{matrix} \}  (B.8.2)

(q̃_{e}^{g})_{3} = \{ \begin{matrix}cos & \frac{−90^{∘}}{2} \\ sin \frac{−90^{∘}}{2} ∗ ( \begin{matrix}0 \\ 1 \\ 0 \end{matrix}) \end{matrix} \} = \{ \frac{1}{\sqrt{2}} ; 0 ; −\frac{1}{\sqrt{2}} ; 0 \}  (B.8.3)

式 (A.7.1) より、全ての操作を合成した回転 Quaternion q̃_{e}^{g} は

q̃_{e}^{g} = ((q̃_{e}^{g})_{1}(q̃_{e}^{g})_{2}(q̃_{e}^{g})_{3})

= \{ \begin{matrix}cos & \frac{λ}{2} \\ 0 \\ 0 \\ sin & \frac{λ}{2} \end{matrix} \} \{ \begin{matrix}cos & \frac{−ϕ}{2} \\ 0 \\ sin & \frac{−ϕ}{2} \\ 0 \end{matrix} \} \{ \frac{1}{\sqrt{2}} ; 0 ; −\frac{1}{\sqrt{2}} ; 0 \} = \frac{1}{\sqrt{2}} \{ \begin{matrix}cos & \frac{λ}{2} & ( cos \frac{−ϕ}{2} + sin \frac{−ϕ}{2} ) \\ sin & \frac{λ}{2} & ( cos \frac{−ϕ}{2} − sin \frac{−ϕ}{2} ) \\ −cos \frac{λ}{2} ( cos \frac{−ϕ}{2} − sin \frac{−ϕ}{2} ) \\ sin & \frac{λ}{2} & ( cos \frac{−ϕ}{2} + sin \frac{−ϕ}{2} ) \end{matrix} \} (B.8.4)

となる。

B.9 g-Frame と n-Frame の関係

g-Frame を D 軸を軸に α rad だけ回転したものが n-Frame に一致することから、 g-Frame と n-Frame の関係は次のとおりである。 (図 B.2 参照)

q̃_{g}^{n} = \{ \begin{matrix}cos & \frac{α}{2} \\ 0 \\ 0 \\ sin & \frac{α}{2} \end{matrix} \}  (B.9.1)

B.10 e-Frame と n-Frame の関係

e-Frame と n-Frame の関係は

q̃_{e}^{n} = q̃_{e}^{g}q̃_{g}^{n}

= \frac{1}{\sqrt{2}} \{ \begin{matrix}cos & \frac{λ}{2} & ( cos \frac{−ϕ}{2} + sin \frac{−ϕ}{2} ) \\ sin & \frac{λ}{2} & ( cos \frac{−ϕ}{2} − sin \frac{−ϕ}{2} ) \\ −cos \frac{λ}{2} ( cos \frac{−ϕ}{2} − sin \frac{−ϕ}{2} ) \\ sin & \frac{λ}{2} & ( cos \frac{−ϕ}{2} + sin \frac{−ϕ}{2} ) \end{matrix} \} \{ \begin{matrix}cos & \frac{α}{2} \\ 0 \\ 0 \\ sin & \frac{α}{2} \end{matrix} \}

= \frac{1}{\sqrt{2}} \{ \begin{matrix}cos & \frac{λ+α}{2} & ( cos \frac{−ϕ}{2} + sin \frac{−ϕ}{2} ) \\ sin & \frac{λ−α}{2} & ( cos \frac{−ϕ}{2} − sin \frac{−ϕ}{2} ) \\ −cos \frac{λ−α}{2} ( cos \frac{−ϕ}{2} − sin \frac{−ϕ}{2} ) \\ sin & \frac{λ+α}{2} & ( cos \frac{−ϕ}{2} + sin \frac{−ϕ}{2} ) \end{matrix} \}

(B.10.1)

である。これより

ϕ = arcsin{−(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}− (q_{e}^{n})_{3}^{2}} = arcsin{1 − 2((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) }  (B.10.2)
λ = arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} − arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}  (B.10.3)
α = arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} + arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}  (B.10.4)

すなわち q̃_{e}^{n} から緯度 ϕ、経度 λ、 Azimuth 角 α を知ることができる。

また式に三角関数を必要とするものが多いことから、ここでその関係を述べておく。まず ϕ は、

sinϕ = 1 − 2((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2})  (B.10.5)

\begin{aligned}cos^{2}ϕ & = 1 − sin^{2}ϕ \\ & = 1 − (1 − 2((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) ) ^{2} \\ & = 4((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) − 4((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) ^{2} \\ & = 4((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) (1 − ((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) ) \\ & = 4((q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}) ((q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}) \end{aligned}  (B.10.6)

cosϕ = 2\sqrt{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} \sqrt{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}}  (≧ 0)  (B.10.7)

次に λ は

tanλ = tan( arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} − arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}})

= \frac{tan( arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}}) − tan( arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}})}{1 + tan( arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}}) tan( arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}})}

= \frac{\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} − \frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}}{1 + \frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} \frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}}

= \frac{(q_{e}^{n})_{3}(q_{e}^{n})_{2}− (q_{e}^{n})_{1}(q_{e}^{n})_{0}}{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}(q_{e}^{n})_{1}}

(B.10.8)

\begin{aligned}cos^{2}λ & = \frac{1}{1 + tan^{2}λ} \\ & = \frac{1}{1 + \begin{matrix}\{(q_{e}^{n})_{3}(q_{e}^{n})_{2}−(q_{e}^{n})_{1}(q_{e}^{n})_{0}\} & ^{2} \\ \{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+(q_{e}^{n})_{3}(q_{e}^{n})_{1}\} & ^{2} \end{matrix}} \\ & = \frac{\{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}(q_{e}^{n})_{1}\}^{2}}{\{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}(q_{e}^{n})_{1}\}^{2}+ \{(q_{e}^{n})_{3}(q_{e}^{n})_{2}− (q_{e}^{n})_{1}(q_{e}^{n})_{0}\}^{2}} \\ & = \frac{\{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}(q_{e}^{n})_{1}\}^{2}}{\{(q_{e}^{n})_{0}(q_{e}^{n})_{2}\}^{2}+ \{(q_{e}^{n})_{3}(q_{e}^{n})_{1}\}^{2}+ \{(q_{e}^{n})_{3}(q_{e}^{n})_{2}\}^{2}+ \{(q_{e}^{n})_{1}(q_{e}^{n})_{0}\}^{2}} \\ & = \begin{matrix}\{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}(q_{e}^{n})_{1}\} & 2 \\ \{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}\} & 2 & \{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}\} & 2 \end{matrix} \end{aligned}  (B.10.9)

cosλ = −\frac{(q_{e}^{n})_{0}(q_{e}^{n})_{2}+ (q_{e}^{n})_{3}(q_{e}^{n})_{1}}{\sqrt{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} \sqrt{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}}}  (B.10.10)

sin^{2}λ = \frac{tan^{2}λ}{1 + tan^{2}λ}

= \begin{matrix}\{(q_{e}^{n})_{3}(q_{e}^{n})_{2}− (q_{e}^{n})_{1}(q_{e}^{n})_{0}\} & 2 \\ \{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}\} & 2 & \{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}\} & 2 \end{matrix}

(B.10.11)

sinλ = \frac{(q_{e}^{n})_{0}(q_{e}^{n})_{1}− (q_{e}^{n})_{2}(q_{e}^{n})_{3}}{\sqrt{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} \sqrt{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}}}  (B.10.12)

最後に α は

tanα = tan( arctan\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} + arctan\frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}})

= \frac{\frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} + \frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}}{1 − \frac{(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}} \frac{(q_{e}^{n})_{1}}{(q_{e}^{n})_{2}}}

= \frac{(q_{e}^{n})_{0}(q_{e}^{n})_{1}+ (q_{e}^{n})_{2}(q_{e}^{n})_{3}}{(q_{e}^{n})_{0}(q_{e}^{n})_{2}− (q_{e}^{n})_{1}(q_{e}^{n})_{3}}

(B.10.13)

cosα = \frac{(q_{e}^{n})_{1}(q_{e}^{n})_{3}− (q_{e}^{n})_{0}(q_{e}^{n})_{2}}{\sqrt{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} \sqrt{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}}}  (B.10.14)

sinα = \frac{(q_{e}^{n})_{0}(q_{e}^{n})_{1}+ (q_{e}^{n})_{2}(q_{e}^{n})_{3}}{\sqrt{(q_{e}^{n})_{0}^{2}+ (q_{e}^{n})_{3}^{2}} \sqrt{(q_{e}^{n})_{1}^{2}+ (q_{e}^{n})_{2}^{2}}}  (B.10.15)

B.11 n-Frame と b-Frame の関係

n-Frame と b-Frame の関係は機体の現在の姿勢に相当する。 n-Frame をオイラー角 yawing Ψ、 pitching Θ、 rolling Φ の順で回転させたときに b-Frame に一致する (図 B.3 参照) から

q̃_{n}^{b} = \{ \begin{matrix}cos & \frac{Ψ}{2} \\ sin & \frac{Ψ}{2} & ( \begin{matrix}0 \\ 0 \\ 1 \end{matrix}) \end{matrix} \} \{ \begin{matrix}cos & \frac{Θ}{2} \\ sin & \frac{Θ}{2} & ( \begin{matrix}0 \\ 1 \\ 0 \end{matrix}) \end{matrix} \} \{ \begin{matrix}cos & \frac{Φ}{2} \\ sin & \frac{Φ}{2} & ( \begin{matrix}1 \\ 0 \\ 0 \end{matrix}) \end{matrix} \}
= \{ \begin{matrix}cos \frac{Ψ}{2} cos \frac{Θ}{2} cos \frac{Φ}{2} + sin \frac{Ψ}{2} sin \frac{Θ}{2} sin \frac{Φ}{2} \\ cos \frac{Ψ}{2} cos \frac{Θ}{2} sin \frac{Φ}{2} − sin \frac{Ψ}{2} sin \frac{Θ}{2} cos \frac{Φ}{2} \\ cos \frac{Ψ}{2} sin \frac{Θ}{2} cos \frac{Φ}{2} + sin \frac{Ψ}{2} cos \frac{Θ}{2} sin \frac{Φ}{2} \\ sin \frac{Ψ}{2} cos \frac{Θ}{2} cos \frac{Φ}{2} − cos \frac{Ψ}{2} sin \frac{Θ}{2} sin \frac{Φ}{2} \end{matrix} \}

(B.11.1)

また逆に q̃_{n}^{b} がわかれば

Ψ = arctan\frac{2((q_{n}^{b})_{1}(q_{n}^{b})_{2}+ (q_{n}^{b})_{0}(q_{n}^{b})_{3})}{(q_{n}^{b})_{0}^{2}+ (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}− (q_{n}^{b})_{3}^{2}}  (B.11.2)
Θ = arcsin( 2( (q_{n}^{b})_{0}(q_{n}^{b})_{2}− (q_{n}^{b})_{1}(q_{n}^{b})_{3}) )  (B.11.3)
Φ = arctan\frac{2((q_{n}^{b})_{2}(q_{n}^{b})_{3}+ (q_{n}^{b})_{0}(q_{n}^{b})_{1})}{(q_{n}^{b})_{0}^{2}− (q_{n}^{b})_{1}^{2}− (q_{n}^{b})_{2}^{2}+ (q_{n}^{b})_{3}^{2}}  (B.11.4)

である。

付録 C

Earth Model(WGS-84)

本研究で構築する航法システムは地球での航行をターゲットとしている。従って、 INS を用いるにあたり地球の曲率、および重力を数値的に計算できるようモデル化する必要がある。本研究では、現在一般的に採用されている地球の数値計算モデルである WGS-84 を採用することにした。そこで本章では、地球の数値計算モデルについて、および WGS-84 について言及する。

C.1 楕円モデル

一般に地球の数値計算モデルを考える際は、自転軸通るように地球を切った切り口で考えられる。その切り口の形を楕円と近似することによって地球の数値計算モデルを構築することが容易となる。本項では、楕円の性質について説明する。

図 C.1 地球モデル

楕円は一般に次の式で表される。

\frac{β^{2}}{r_{e}^{2}} + \frac{z^{2}}{r_{p}^{2}} = 1  (C.1.1)

両辺微分して

\frac{2βdβ}{r_{e}^{2}} + \frac{2zdz}{r_{p}^{2}}= 0  (C.1.2)

すなわち

\frac{dz}{dβ} = −\frac{br_{p}^{2}}{zr_{e}^{2}}  (C.1.3)

図より緯度 ϕ と\frac{dz}{dβ} の関係は、

\frac{dz}{dβ} = tan( \frac{π}{2} + ϕ) = −\frac{1}{tanϕ}  (C.1.4)

従って、

− \frac{1}{tanϕ} = −\frac{βr_{p}^{2}}{zr_{e}^{2}}  (C.1.5)

ここで、離心率 ϵ なる変数

ϵ = ( 1 − \frac{r_{p}^{2}}{r_{e}^{2}}) ^{\frac{1}{2}}  (C.1.6)

を導入すれば、

\frac{r_{p}^{2}}{r_{e}^{2}} = −(ϵ^{2}− 1)  (C.1.7)

より

\frac{z}{β} = (1 − ϵ^{2}) tanϕ  (C.1.8)

この式と (C.1.1) から、 z を消去すると、

β = \frac{r_{e}cosϕ}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{1}{2}}}  (C.1.9)

同様に β を消去すれば、

z = \frac{r_{e}(1 − ϵ^{2}) sinϕ}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{1}{2}}}  (C.1.10)

C.2 南北方向ならびに東西方向の曲率半径

前節で述べた数値計算モデルの南北方向 (経線上) の極率半径は次の式で与えられる。

R_{meridian}= \frac{( 1 + ( \frac{dz}{dβ})^{2}) ^{\frac{3}{2}}}{\frac{d^{2}z}{dβ^{2}}}  (C.2.1)

ここで (C.1.3) を β で微分すれば、

\frac{d^{2}z}{dβ^{2}} = −\frac{r_{p}^{4}}{r_{e}^{2}z^{3}}  (C.2.2)

であるから、これと (C.1.3)、 (C.1.8)、 (C.1.9) より

R_{meridian}= \frac{r_{e}(1 − ϵ^{2})}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{3}{2}}}  (C.2.3)

また東西方向の極率半径は次の式で与えられる。

R_{normal}= \frac{β}{cosϕ}  (C.2.4)

従って (C.1.9) より

R_{normal}= \frac{r_{e}}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{1}{2}}}  (C.2.5)

C.3 重力

地球上で重力として観測されるものは 2 つに分離することが可能である。すなわち、万有引力と遠心力である。遠心力は地球は慣性系に対して、 Earth Rate(Ω_{i/e}) で回転しているために発生する力である。

i-Frame に対して万有引力 G⃗^{i} は次の式で与えられる。

\begin{aligned}G⃗^{i} & = ( \begin{matrix}−\frac{μ}{R^{2}}( 1 + \frac{3}{2}J_{2}( \frac{r_{e}}{R})^{2}( 1 − 5( \frac{z}{R})^{2}) ) \frac{x}{R}+ ··· \\ −\frac{μ}{R^{2}}( 1 + \frac{3}{2}J_{2}( \frac{r_{e}}{R})^{2}( 1 − 5( \frac{z}{R})^{2}) ) \frac{y}{R}+ ··· \\ −\frac{μ}{R^{2}}( 1 + \frac{3}{2}J_{2}( \frac{r_{e}}{R})^{2}( 3 − 5( \frac{z}{R})^{2}) ) \frac{y}{R}+ ··· \end{matrix}) \\ & ( ただしR = \sqrt{x^{2}+ y^{2}+ z^{2}} ) \end{aligned}  (C.3.1)

i-Frame に対して遠心力 g⃗_{CF}は次の式で与えられる。

g⃗_{CF}= −Ω_{e/i}Ω_{e/i}r^{i}  (C.3.2)

従って両者を加えて、重力 g^{i} は

g^{i} = G⃗^{i}− Ω_{e/i}Ω_{e/i}r^{i}  (C.3.3)

C.4 WGS-84

本研究で採用する地球の数値計算モデル、 WGS-84 の定数は表 (C.1) によって与えられる。 WGS-84 における南北方向、東西方向の極率半径はそれぞれ (C.2.3)、 (C.2.5) に必要な定数を代

入することによって求められる。また重力は WGS-84 では以下の式で与えられる。

g = gWGS_{0} \frac{(1 + gWGS_{1} sin^{2}ϕ)}{(1 − ϵ^{2}sin^{2}ϕ) ^{\frac{1}{2}}}  (C.4.1)

表 C.1 WGS-84 の定数

赤道半径 (r_{e})  6378137  m
自転速度 (Earth Rate、 Ω_{e/i}) 7.292115 × 10^{−5}  rad/s
地球重力定数 (μ)  3.986005 × 10^{14}  m^{3}/s^{2}
地球重力第二定数 (J_{2})  1.08263 × 10^{−3}
平滑度 (f)  298.257223563
極半径 (r_{p})  6356752.3142  m
第一偏心性 (ϵ)  0.0818191908426
赤道上重力 (gWGS_{0})  9.7803267714  m/s^{2}
重力公式定数 (gWGS_{1})  0.00193185138639

付録 D

Allan Variance

本研究で使用する慣性センサを評価するにあたり Allan Variance を用いた。本章では、 Allan Variance について説明するとともに、それを用いてセンサのモデルを作成する方法を述べる。

D.1 Allan Variance

Allan Variance とは、時計等の精度の指標として定められた値である。 2 標本分散の 1 種であり、この指標を用いることによって、どれだけ時間的に機器が安定性しているかを知ることができる。時計を例にして言うならば、あるとき時計を見てそれから 1 日が経過した後に、その同じ時計がどの程度正確に 1 日を刻んでいるかを測る指標である。

今回は慣性センサとして MEMS ジャイロ、 MEMS 加速度計を使用したが、これらの機器は電源を入れた後にゼロ点が時間的に大きく変動する性質がある。そこで今回、このゼロ点の時間的安定性をモデル化するために、 Allan Variance を用いた。

AVAR(τ) = \frac{1}{2(n − 1)} ∑^{n} (\frac{∑_{forward} y − ∑_{backward} y}{τ})^{2}  (D.1.1)

付録 E ソースコード

64

付録 F

実機試験基板

F.1 回路図 F.2 ガーバ F.3 参考文献

参考文献

[1] Robert M. Rogers. Applied Mathematics in Integrated Navigation System, Second Edition. AIAA Education Series, 2003. ISBN 1-56347-656-8.

[2] 多摩川精機�『ジャイロ活用技術入門』工業調査会, 2003. ISBN 4-7693-1208-3.

[3] 遠坂俊昭『計測のためのフィルタ回路設計』 CQ 出版社, 1998. ISBN 4-7898-3282-1.

謝辞

この卒業論文を書くにあたり、指導教官の土屋先生をはじめてとして鈴木真二先生、ならびに助手の柄沢さん、鈴木・土屋研の先輩からは多大なご指導・ご支援を賜りました。ありがとうございます。

MEMS 慣性センサの試験ならびにモデル構築にあたっては三菱電機の廣川氏、実松氏にお世話になりました。ありがとうございます。

最後に鈴木土屋研の同輩には、互いに激励しあい、ついには論文完成まで漕ぎ着けられたことに感謝の意を表したいと思います。

2004/11 成岡優