在線性加速度下使用之姿態估測器

在線性加速度下使用之姿態估測器

An Orientation Estimation under Accelerated Situation

研 究 生：張紹宸 Student：Shao-Chen Chang

指導教授：陳宗麟 Advisor：Tony Chen

國 立 交 通 大 學

機 械 工 程 學 系

碩 士 論 文

A Thesis

Submitted to Department of Computer and Mechanical Engineering College of Engineering

National Chiao Tung University in partial Fulfillment of the Requirements

for the Degree of Master

in

Mechanical Engineering May 2008

Hsinchu, Taiwan, Republic of China

在 線 性 加 速 度 下 使 用 之 姿 態 估 測 器

研究生：張紹宸 指導教授：陳宗麟博士

國立交通大學機械工程學系

摘要

： 以往使用量測慣性的感測器所組合成的姿態估測器，大部分都只能在沒有運 動加速度的狀態下使用，可是實際上我們想量測姿態的對象，是很難不具有運動 加速度的。在本論文中，我們提出新的物體姿態估測方法，利用平面式純加速規 慣性量測單元、磁場感應器、固定點距離偵測感應器搭配卡爾曼估測器，設計出 可以估測到具有加速度運動的系統姿態、角速度與加速度。 在具有運動加速度的狀態下估測姿態，若加速度快速變化時，我們搭配記憶 褪去式卡爾曼估測器的設計，提高系統追跡變化訊號之能力，大大增進狀態收斂 的速度。 本論文目前的成果，利用Matlab 做系統的模擬，共計使用了七個單軸加速 規、一個三軸磁場感應器和一個距離感應器，可以成功估測出具有運動加速度的 物體姿態，即使物體具有變加速度運動，也可以追跡成功。在估測精度方面，穩 態時的姿態四元數精度可達到7.80×10-5_{，加速度估測精度2.87×10}-3 _m/s2_，角速 度估測精度0.78 rad/s。

An Orientation Estimation under Accelerated Situation

Student : Shao-Chen Chang Advisor : Dr. Tsung-Lin Chen

Department of Mechanical Engineering National Chiao Tung Unniversity

Abstract

In the past, the estimation of an object in motion is done by using inertial sensor. The conventional approach can not apply to the object under a linear acceleration.

Although, in most cases, the object to be measured work under linear acceleration. In this thesis, we proposed a novel design of orientation determination, using coplanar gyro-free inertial measurement unit, magnetic field inductor and a distance sensor. These sensor accompany with Kalman filter technique, can determine attitude, angular velocity and acceleration of the object in motion.

When we determine orientation under linear acceleration, if the acceleration changes fast, we design observer that accompany with “Fading Memory” technique, that will be raised tracking ability of variable signal, and improve speed of state convergence. According to our simulation by Matlab, the proposed design determines attitude in motion successfully, which use seven single-axis accelerometers, a 3-axis

magnetometer and a distant sensor. Even though the acceleration is changed fast, this observer tracks successful. Tracking error standard deviation on simulative data are shown, 7.80×10-5 in quaternion of attitude, 2.87×10-3 m/s2 in acceleration and 0.78 rad/s in angular velocity.

目 錄

摘要... i Abstract ... ii 目 錄... iii 圖 目 錄... iv 表 目 錄... v 第一章：緒論... 1 1-1 研究動機與文獻回顧 ... 1 1-2 論文架構 ... 2 第二章：姿態量測序言... 3 2-1 座標系統 ... 3 2-2 座標轉換 ... 4 2-3 姿態表示法 ... 6 2-4 舊式角度量測法 ... 9 2-5 平面式之純加速規慣性量測單元 ... 10 第三章：卡爾曼估測器... 16 3-1 Kalman Filter ... 16

3-2 Extended Kalman Filter ... 16

3-3 儲存記憶褪去式卡曼估測器 ... 17 第四章：新的姿態估測方法... 20 4-1 系統模型 ... 20 4-2 流程方塊圖 ... 21 第五章：穩定性分析... 23 第六章：模擬結果與討論... 26 6.1 運動加速度為定值時的姿態估測... 26 6.2 運動加速度非定值時的姿態估測... 33 6.3 使用記憶褪去法的估測器追跡... 38 6.4 模擬結果討論... 45 第七章：結論... 49 參考文獻... 50

圖 目 錄

圖（2.1） ECEF 及 NED 座標系... 4  圖（2.2） 附體座標系... 4  圖（2.3） 側滾角之示意圖及座標... 5  圖（2.4） 俯仰角之示意圖及座標... 5  圖（2.5） 航向角之示意圖及座標... 6  圖（2.6） 尤拉定理... 8  圖（2.7） 舊式角度量測法簡易示意圖... 9  圖（2.8） 立方體 IMU 及平面式 IMU ... 11  圖（2.9） 固定座標及附體座標... 11  圖（2.10） 加速規配置圖... 13  圖（3.1） EKF 流程圖 ... 17  圖（3.2） AFKF 流程圖 ... 18 

圖（3.3） EKF 與 AFKF 的比較，真實值與 AFKF 估測值幾乎重疊... 19 

圖（4.1）流程方塊圖... 22  圖（6.1）三軸加速規與磁場感應器訊號圖... 27  圖（6.2）姿態四元數的真實值與估測值(幾乎重疊) ... 27  圖（6.3）導航座標加速度的真實值與估測值(幾乎重疊) ... 28  圖（6.4）附體座標角速度的真實值與估測值... 28  圖（6.5） 10 個估測狀態之誤差收斂情形... 29  圖（6.6）尤拉角的真實值與估測值... 29  圖（6.7）尤拉角誤差收斂情形... 30  圖（6.8）姿態四元數的真實值與估測值... 31  圖（6.9）導航座標加速度的真實值與估測值... 31  圖（6.10）附體座標角速度的真實值與估測值... 32  圖（6.11）10 個估測狀態之誤差收斂情形 ... 32  圖（6.12）尤拉角的真實值與估測值... 33  圖（6.13）三軸加速規與磁場感應器訊號圖... 34  圖（6.14）姿態四元數的真實值與估測值(幾乎重疊) ... 34  圖（6.15）導航座標加速度的真實值與估測值... 35  圖（6.16）附體座標角速度的真實值與估測值... 35  圖（6.17）10 個估測狀態之誤差收斂情形... 36  圖（6.18）尤拉角的真實值與估測值... 36  圖（6.19）導航座標加速度的真實值與估測值... 37  圖（6.20）三軸加速規與磁場感應器訊號圖... 38  圖（6.21）姿態四元數的真實值與估測值... 39  圖（6.22）導航座標加速度的真實值與估測值... 39  圖（6.23）附體座標角速度的真實值與估測值... 40 

圖（6.24）10 個估測狀態之誤差收斂情形... 40  圖（6.25）尤拉角的真實值與估測值... 41  圖（6.26）三軸加速規與磁場感應器訊號圖... 42  圖（6.27）姿態四元數的真實值與估測值... 42  圖（6.28）導航座標加速度的真實值與估測值... 43  圖（6.29）附體座標角速度的真實值與估測值... 43  圖（6.30）10 個估測狀態之誤差收斂情形... 44  圖（6.31）尤拉角的真實值與估測值... 44  圖（6.32）導航座標加速度的真實值與估測值(下圖估測值與實際值幾乎重疊) 45  圖（6.33）[Case1 以 EKF 方法估測固定加速度訊號]10 個估測狀態之誤差標準差 ... 46  圖（6.34）[Case3 以 EKF 方法估測步階變化加速度訊號]10 個估測狀態之誤差標 準差... 46  圖（6.35）[Case6 以 AFKF 方法估測三軸連續變化加速度訊號]10 個估測狀態之 誤差標準差... 47 

表 目 錄

表（2.1） 加速規配置位置... 13  表（6.1）穩態時之估測誤差標準差... 47  表（6.2）與舊式估測器比較精度... 48 

第一章：緒論

—研究動機與文獻回顧、計畫書架構 1-1 研究動機與文獻回顧 精準又隨時的系統姿態估測裝置，對於人駕駛機械，或者機械獨立運作時， 這些資訊十分重要。例如衛星在太空中運作時，必須透過本身精準的位置判定與 姿態控制，才能從事相關的空拍，量測…等實驗。其他如機器人、航太、潛水艇、 虛擬實境…等都是需要隨時知道姿態資訊的。因此，發展可用在人或者物體上的 姿態感測器，兼具穩定、輕便等特性的重要性越來越高。 近年來，常見量測物體運動狀態的感測器，有量測慣性力的加速度計、量測 運動角速度的陀螺儀、量測磁場方向的磁場感測器…等等是已經被運用許久的工 具。有許多更新的量測姿態元件已經被發展了，例如可以同時量測三軸的磁場、 重力、角速度的MARG 感測器，都已經被廣泛運動在許多工業或者虛擬實境上。 但目前大部分的姿態估測器，都是利用加速度計量測重力、磁場感測器量測 磁場，搭配一些估測設計，可以知道系統的姿態。估測器設計的方法有很多種， 如文獻[5]利用滑模估測器(sliding mode observer)來估測姿態與角度，或者文獻[6]

中利用Bootstrap Filter 來設計估測器，達到系統姿態的獲得。但這樣的估測器， 若在有運動加速度的狀態下，加速度計量到的就不只是重力，還有物體的運動加 速度都會一起量到，如此以往大部分的姿態估測器就沒有辦法使用了。 再者，常用於此類姿態判定裝置中的感應器，陀螺儀可用以量測運動時之角 速度。在實際應用中，陀螺儀的輸出大多帶有雜訊且其訊號有飄移現象(drift)， 因此在使用時必須將此非理想的狀況一並考慮，才可確保其應用時的可行性。尤 其是在利用角速度量測來獲取角度的應用中，角速度的雜訊及偏差(bias)值會隨 著積分的運算過程被累積、放大。因此積分的時間不能太長，否則必須設法估測 其訊號偏差值，才能確保所獲得的角度訊號不至於發散。 因此本論文提出一種新的方法，讓姿態估測器在具有運動加速度的狀態下， 仍可以成功估測出系統姿態。加上平面式純加速規慣性量測單元的設計，避免使 用陀螺儀當作系統的感應器。透過本論文所提出的新式估測方法，在安裝平面式 純加速規慣性量測單元、三軸磁場感測器與固定點位置測量感應器之後，搭配引 用記憶褪去式卡爾曼估測器之設計，可以成功追蹤到加速度、姿態、加速度等共 十個參數，對於系統控制有極大助益。

1-2 論文架構 本文共分為七章，第一章「緒論」，介紹研究動機、文獻回顧與計畫書架構。 第二章「姿態量測序言」，介紹使用的座標、四元數法以及平面式之純加速規慣 性量測單元。第三章「卡爾曼估測器」介紹卡爾曼濾波器、擴增卡爾曼濾波器以 及儲存記憶褪去式卡爾曼估測器。第四章「新的姿態估測方法」提出本篇論文新 式方法的系統模型與流程方塊圖。第五章「穩定性分析」證明本估測器的觀察性。 第六章「模擬結果與討論」，記錄目前系統模擬和分析後的結果。第七章「結論」， 報告此計畫目前為止的成果。

第二章：姿態量測序言

2-1 座標系統

在導航與定位的過程中，除了仰賴外來的輔助取得觀測量外，往往需要進行 一些數學的計算以推論出正確的航向與位置。而本節將介紹不同的座標系及座標 系的轉換。

2-1.1 地心地固座標系(Earth Centered Earth Fixed Frame, ECEF)

也稱為傳統地面座標系(Convention Terrestrial Reference System ,CTRS)，是 以地心為原點但其三個軸相對於地球卻是固定的，因此是隨著地球旋轉。其三軸 與原點規定如下 原點：位於地球的質量中心。 X 軸：通過格林威治之天文子午圈，即通為經度的零度。 Z 軸：為地球的自轉軸，由原點指向北極點，即平行於 CIO(Conventional International Origin)之平均北極。 Y 軸：由右手座標系

Z X

×

來決定。 2-1.2 導航座標系(Navigation Frame) 這組座標系通常用於導航使用，它可以直接提供導航者方位、航向等等訊 息。而軸向定義如下。 原點：定義為導航者所處的位置。 N 軸：指向地理北方。 D 軸：指向地心，即平行地球的重力方向。 E 軸：由右手座標系

D N

×

來決定，故指向東方。因此也簡稱為NED 座標系。

D N E x z y Z X Y 圖（2.1） ECEF 及 NED 座標系 2-1.3 附體座標系(Body frame) 它是一組正交座標系，軸向定義通常對準於載具(vehicle)前方－右側方－下 方，而原點位於載具的重心，如圖所示。 圖（2.2） 附體座標系 2-2 座標轉換 前述之地心地固座標系、導航座標系、附體座標系，在定向及導航都有其用 途，因此有必要探討同一點在不同座標系統表示間關係。關於物體的三種不同的 旋轉方式，分別是側滾角(Roll)、俯仰角(Pitch)及航向角(Yaw)，並由這三種不同 的角度來求得附體座標與導航座標之關係。 2-2.1 側滾角(φ) 在航空學中側滾角表示為機翼的上/下的旋轉，也等效於對附體座標的 x 軸 來旋轉。如圖(2.3)所示

Down East z y ψ ψ 圖（2.3） 側滾角之示意圖及座標 若以數學表示

( )

( )

( )

( )

( )

( )

1 0 0 , , 0 cos sin 0 sin cos x N y R x E R x z D φ φ φ φ φ φ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥₌ ⎢ ⎥ _{= ⎢} ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ − ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.1) 2-2.2 俯仰角(θ) 在航空學中俯仰角表示為機鼻的上/下的旋轉，也等於對附體座標的 y 軸來 旋轉。如下圖(2.4)所示 North Down x z θ θ 圖（2.4） 俯仰角之示意圖及座標 若以數學表示

( )

( )

( )

( )

( )

( )

cos 0 sin , , 0 1 0 sin 0 cos x N y R y E R y z D θ θ θ θ θ θ ⎡ − ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥₌ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.2) 2-2.3 航向角(Ψ) 在航空學中航向角表示為機鼻的左/右的旋轉，也等於對附體座標的 z 軸來

旋轉。如下圖(2.5)所示 East North x y Ψ Ψ 圖（2.5） 航向角之示意圖及座標 若以數學表示

( )

,

( )

, cossin

( )

( )

cossin

( )

( )

00

0 0 1 x N y R z E R z z D ψ ψ ψ ψ ψ ψ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥₌ ⎢ ⎥ _{= −} ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.3) 2-3 姿態表示法 數學上有多種的姿態表示法，有方向餘弦法、尤拉角法、四元數法。因為本 論法主要以四元數法為主，尤拉角法為輔，因此本節主要介紹四元數法及尤拉角 法，有關於更詳細的姿態表示法可參閱文獻[9][10][11][12][18][19][20]。 2-3.1 尤拉角法 (Euler angle) 以尤拉角為姿態表示，其變數只有三個而且互相獨立。若以前述三個旋轉角 度，依前後次序旋轉航向角(Yaw)、俯仰角(Pitch)及側滾角(Roll)則可獲得附體座 標與導航座標的轉換矩陣。

( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

cos cos cos sin sin

cos sin sin cos sin cos cos sin sin sin cos sin

cos cos sin sin sin cos sin sin cos sin cos cos

, , , b n C R x R y R z θ ψ θ ψ θ ψ θ φ φ ψ φ ψ θ φ ψ θ φ φ ψ θ φ ψ φ θ ψ ψ φ θ φ φ θ ψ − − + + − = ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (2.4) b n

C

表示為導航座標(Navigation frame)轉換至附體座標(Body frame)之轉換矩陣，

為了確保尤拉角之唯一性，故要求

2 2

π π

π ψ π θ π φ π

b n x N y C E z D ⎡ ⎤ ⎡ ⎤ ⎢ ⎥₌ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ (2.5) 因為

C

_nb為一個單位正交矩陣，故具有下列之性質。

( ) ( )

_b T _n 1 n b

C

=

C

− (2.6)

( )

det

b n

C

=

I

(2.7) 由以上可知，若我們己知道三個尤拉角的角度，即可獲得

C

_nb矩陣。並且可 利用

C

_nb可以求得由附體座標的角速度可以轉換至尤拉角的角速度，因為我們己 知轉換矩陣是由R(x,ψ)R(y,θ)R(z,Ψ)的順序所組成，故

( )

( ) ( )

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ψ θ φ φ θ φ φ θ φ θ ψ θ φ θ φ φ ω ω ω & & & cos cos sin 0 sin cos cos 0 sin 0 1 0 0 , , 0 0 , 0 0 R x R x R y z y x (2.8)

( ) ( )

( ) ( )

( )

( )

( ) ( )

( ) ( )

1 sin tan cos tan

0 cos sin

0 sin sec cos sec

x y z φ _{φ θ φ θ ω} θ φ φ ω ψ φ θ φ θ _ω ⎡ ⎤ _{⎡ ⎤⎡ ⎤} ⎢ ⎥ _{⎢ ⎥⎢ ⎥} ⎢ ⎥ =_⎢ − _{⎥⎢ ⎥} ⎢ ⎥ _{⎢ ⎥⎢ ⎥} ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ & & & (2.9) 觀察上式可發現當俯仰(Pitch)角為+/- 90∘時，也就是載具垂直向上或垂直 向下飛行，其轉換矩陣會發生奇異性。故利用此方程式來計算姿態的轉換矩陣時 俯仰角(Pitch)會有所限制。 2-3.2 四元數法 (Quaternion) 由於用尤拉角來表示的姿態轉換矩陣會有奇異性問題，因此使用四元數姿態 的表示式，可避免此問題。在介紹四元數法之前先介紹尤拉定理，其定義為： 剛體繞固定點旋轉的運動，可表示為繞某一根通過該固定點之軸線

( )

λ r 旋轉 一個角度(μ)的運動。如圖(2.6)所示：

X Y Z x z y μ λ 1 λ = ⋅λ Xr r 2 λ = ⋅λ Yr r 3 λ = ⋅λ Zr r 圖（2.6） 尤拉定理 若以單位向量λ 為轉軸，將座標系(XYZ)對其旋轉一角度 μ 則可得座標(xyz)，因 此轉換矩陣C 以 λ 及 μ 表示為：

(

,

)

cos

3

(

1 cos

)

sin

b _T n

C

λ μ

₌

μ

_{⋅ + −}

I

μ λλ

₋

μ λ

_⋅

× (2.10) 其中 3 2 3 1 2 1 0 0 0 λ λ λ λ λ λ λ × ₌⎡⎢ − ₋ ⎤⎥ ⎢ ⎥ ⎢− ⎥ ⎣ ⎦ λ1~λ3為轉軸λ 投影至 XYZ 座標的分量，μ 為繞 λ 軸旋轉的角度。 我們可令四元數向量為 4 3 2 1 1 q q iq jq kq q Qr = + r= +r + r + r (2.11)

(

)

(

)

(

)

(

)

4 3 3 3 2 2 2 1 1 1 2 / sin 2 / sin 2 / sin 2 / cos q q q q = = = = = = = = μ λ ε μ λ ε μ λ ε μ η 　　 (2.12) 因 為 四 元 數 並 不 具 有 四 個 自 由 度 因 此 必 須 滿 足 單 範 的 性 質 限 制 (normalization constraint) 1 2 2 3 2 2 2 1 +ε +ε +η = ε (2.13) 因此轉換矩陣如下所示

(

)

(

)

(

)

(

)

(

)

(

)

⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + − − − + + − + − − − + − − + = 2 3 2 2 2 1 2 1 3 2 2 3 1 1 3 2 2 3 2 2 2 1 2 3 2 1 2 3 1 3 2 1 2 3 2 2 2 1 2 2 2 2 2 2 2 ε ε ε η ηε ε ε ηε ε ε ηε ε ε ε ε ε η ηε ε ε ηε ε ε ηε ε ε ε ε ε η b n C (2.14) 可由(2.4)(2.14)式可獲得四元數與尤拉角之關係

(

)

(

)

(

)

(

)

⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ − − + + = + − = ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ + − − + = − − − 2 3 2 2 2 1 2 3 2 1 1 2 3 1 1 2 3 2 2 2 1 2 1 3 2 1 2 tan 2 sin 2 tan ε ε ε η ηε ε ε ψ ηε ε ε θ ε ε ε η ηε ε ε φ (2.15) 若姿態連續變化時，則四元數滿足(2.16)的微分方程式 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − − − − − = × z b y b x b q q ω ω ω η ε ε ε η ε ε ε η ε ε ε 1 2 1 3 2 3 3 2 1 1 4 ₂ 1 & (2.16) 比較於尤拉角而言四元數有以下的優點： z 四元數可適用於任意的姿態角，不會有奇異點發生。 z 四元數的值均介於+/-1 之間，非常適合數值計算。 z 因此本論文是以四元數來描敘物體的姿態。 2-4 舊式角度量測法 從簡單圖示中來看，系統運作時，本身的附體座標(xyz)已知，加速規可以測量到 重力的方向(D-direction)，磁力計可以測量到地磁方向(N-direction)，由 D 與 N 方 向可以正交得到E 方向，如此就可以得到附體座標與導航座標間的關係。 圖（2.7） 舊式角度量測法簡易示意圖 假設重力場與磁場為na=

[

0 0 g

]

T，nm=

[

a 0 b

]

T …(2.17) 其中a 和 b 代表當地磁場的強度，搭配座標轉換矩陣，我們可以知道重力和磁場

在附體座標上的值為：

[

]

T

n b i

b_{g=C ⋅ a= −g⋅sinθ g⋅sinφcosθ g⋅cosφcosθ …(2.18)}

[

] [

T

]

T b T T n n n n b n b v v v b a a m x R y R m z R m m z R y R x R m C m 3 2 1 sin cos ) , ( ) , ( ) , ( 1 0 0 0 cos sin 0 sin cos cos 0 sin 0 1 0 sin 0 cos cos sin 0 sin cos 0 0 0 1 ) , ( ) , ( ) , ( = − ⇒ ⋅ ⋅ = ⋅ ⇒ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⋅ ⋅ ⋅ = ⋅ = ψ ψ φ θ ψ ψ ψ ψ ψ θ θ θ θ φ φ φ φ ψ θ φ …(2.19) 經過簡單的整理：

(

)

⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛ ⋅ ⋅ + ⋅ ⋅ + ⋅ ⋅ + ⋅ − = ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ − = ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛ = − = − − − − φ θ φ θ θ φ φ ψ φ θ cos sin sin sin cos sin cos tan tan tan / sin 1 1 2 1 1 1 z b y b x b z b y b z b y b x b m m m m m v v g g g g …(2.20) 物體姿態角度可以從附體座標的重力和磁場得到，也就是說，只要系統有裝載三 軸加速規和陀螺儀，就可以測量到角度。

2-5 平面式之純加速規慣性量測單元(Gyroscope-Free IMU Theory)

一般而言，慣性導航系統(Inertial Navigation System)使用加速規量測線性加 速度，陀螺儀量測角速度。但是利用多組加速規透過適當擺置方式，可取代陀螺 儀而求得載具姿態運動。我們可利用剛體運動學原理，具有角速度或角加速度之 剛體，於剛體上之不同位置有不同的加速度，量得剛體上不同位置之相對加速 度，可估測載具之角速度和角加速度，積分後可得載具姿態。 要描述一個載具在空間運動方式，需要此載具在空間的線性加速度及角速 度。此篇論文主要使用平面式之純加速規慣性量測單元來獲得物體線性加速度及 角速度，此種裝置有別於一般的純加速規慣性量測單元，傳統式的純加速規慣性 量測單元需要較複雜的三維空間的放置及對準，而此裝置只需要考慮二維空間的 對準如圖(2.8)。由於是平面式之裝置，更有助於未來使用微機電製造技術把平面 慣性量測單元量產化。

1

2

3

4

5

6

1

2

3

4

5

6

圖（2.8） 立方體 IMU 及平面式 IMU 根據剛體運動學，一個質點在空間中的運動，其加速度可用固定座標及運動 座標之關係來表示，其中本篇論文是以導航座標(Navigation frame)為固定座標， 而附體座標(body frame)為運動座標，其方程式如下所示： x y z

N

E

D

o_b j r j R r o R r j r

ω

r

O

_n 圖（2.9） 固定座標及附體座標

(

)

2

n n j o j j j j

R

&&

r

=

R

&&

r

+ × + ×

ω

r

&

r

r

ω ω

r

r

×

r

r

+

ω

r

× +

r

r r

& &&

r

(2.21)

n j

R

r&&

：j 點相對於導航座標(navigation frame)的加速度。

n o

R

r&&

：Ob相對於On之加速度。

ω

r

：附體座標系相對於導航座標之角速度。

ω

r&

_{：附體座標系相對於導航座標之角加速度。}

rr&

：j 點相對於附體座標原點 Ob之速度。

j

rr&&

：j 點相對於附體座標原點 Ob之加速度。

2

ω

r r&

×

r

：科氏加速度(Coriolis acceleration)

其中 j

rr

在附體座標上為一固定常數，因此

r

r

& &&

_j

= =

r

r

_j

0

，故在附體座標上j 點的加 速度為

(

)

b b b b b b b

j o j j

R

&&

r

=

R

&&

r

+

ω

r

&

×

r

r

+

ω

r

×

ω

r

×

r

r

…(2.22)

再由座標轉換關係可獲得j 點在導航座標上的加速度

(

)

(

(

)

)

n n n b b n b b b

j o b j b j

R

&&

r

=

R

&&

r

+

C

ω

r

&

×

r

r

+

C

ω

r

×

ω

r

×

r

r

…(2.23)

若把加速規放置於j 點，考慮加速規會量測到運動體的線性加速度 Fo及重力加速 度g， 故有以下的關係式 b b b o o o

R

&&

r

=

F

r

+

g

r

因此放置於j 點的加速規其感測方向為

η

r

_j，其量測值為

(

)

(

)

(

)

j

(

(

)

)

n n b b j j j j j b b b b b b b b o o j j j b T b b b T b T b b b j j b b j j o o

A

R

R

F

g

r

r

r

r

F

g

η

η

ω

ω

ω

η

ω

η

η

η

ω

ω

=

⋅

=

⋅

=

+

+

×

+

×

×

⋅

⎡

⎤

⎡

⎤

=

_⎣

×

_⎦

_⎢

_⎥

+

×

×

+

⎣

⎦

r

_r

r

_r

&&

&&

r

_r

_r

_&

_r

_r

_r

_r

_r

&

…(2.24) 若在一個運動物體上擺放各種不同位置、不同感測方向的加速規，則每個加 速規的輸出可以寫成下式

(

)

(

)

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ × × ⋅ + ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ ⋅ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ M M & M M j b b b T j b b b j r a J A ω η ω ω …(2.25)

(

)

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ × = M M T j b T j b j b_r J η η ，j=1,2,3y,4,5,6,3x …(2.26) 如果 J 是可逆的(invertible)，則可求得運動物體的微分方程式與各個不同位 置的加速規量測的關係，如下所示

) )) ( ( ( 1 1 6 ⎥⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ × × ⋅ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ = ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ ₋ × _M M M M & j b b b T j b j b b r A J a η ω ω ω (2.27) 加速規經由適當配置如表(2.1) Accel. Number Location

[

]

T j j j b_{r = cosβ sinβ 0} Sensing direction

[

]

[

]

⎪⎩ ⎪ ⎨ ⎧ = T T j j j b y 1 0 0 , 6 ~ 4 0 sin cos , 3 ~ 1 α α η

1 [ cos(-0.5π/9) sin(-0.5π/9) 0 ] [ cos(4π/9) sin(4π/9) 0 ] 2 [ cos(4π/9) sin(4π/9) 0 ] [ cos(8.5π/9) sin(8.5π/9) 0 ] 3x [ cos(8.5π/9) sin(8.5π/9) 0 ] [ cos(-5π/9) sin(-5π/9) 0 ] 3y [ cos(-5π/9) sin(-5π/9) 0 ] [ cos(-0.5π/9) sin(-0.5π/9) 0 ]

4 [ cos(1π/9) sin(1π/9) 0 ] [ 0 0 1 ] 5 [ cos(7π/9) sin(7π/9) 0 ] [ 0 0 1 ] 6 [ cos(13π/9) sin(13π/9) 0 ] [ 0 0 1 ] 表（2.1） 加速規配置位置

1

2

3y

z

3x

5

4

6

x

y

r

o

圖（2.10） 加速規配置圖 因此可由(2.12)獲得有關於運動體的角加速度。

[ ]

(

(

)

)

⎟⎟ ⎟ ⎟ ⎠ ⎞ ⎜⎜ ⎜ ⎜ ⎝ ⎛ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ × × ⋅ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ = = = − × 6 ~ 1 6 ~ 1 1 1 3 (1:3,:) j j b b b T j b j j b _{J A r} M M M M & η ω ω ω …(2.28) 接著也可求得運動體的線性加速度(ba 及上述之bF)

[ ]

(

(

)

)

⎟⎟ ⎟ ⎟ ⎠ ⎞ ⎜⎜ ⎜ ⎜ ⎝ ⎛ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ × × ⋅ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ = = = − × 6 ~ 1 6 ~ 1 1 1 3 (4:6,:) j j b b b T j b j j b_{a J A r} M M M M ω ω η …(2.29) 觀察上式(2.24)，由於ba 含有角速度 ω 項，我們希望透過設計加速規放置位 置與感測方向，讓ba 不含有角速度 ω 項。

假設編號 1~3y 的加速規放置位置為 brj=[cosβj sinβj 0]’，感測方向為

b_η j=[cosαj sinαj 0]’，因此我們可以將加速規輸出的方程式描述成： ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + − − − = ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + + + ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = M M M M M M M M & M M M M M M ) sin( cos cos sin sin ) sin( sin cos 1 2 2 2 2 1 3 ~ 1 j j j j j j j j y x z y z x z y b x b j j y j j H H a a A α β α β α β β α ω ω ω ω ω ω ω α α …(2.30) 為了消去角速度項，我們找出H1的零核空間(null space)U 2 ) sin cos ( , 0 1 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⋅ M M M M j j U rank H U 　　　　 α α y j j j j y b x b A U U a a 3 ~ 1 1 ) sin cos ( = − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ = ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ M M M M M M α α …(2.31) 如此我們就可以由平面加速規的輸出來得到 bax 和 bay，不需要知道角速度的資 訊。同理，假設編號 4~6 的加速規放置位置為 brj=[cosαj sinαj 0]’，感測方向 為bηj=[0 0 1]’，因此我們可以將加速規輸出的方程式描述成：

[ ]

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = M M M M M M M M & & M M M M j j j j z y z x y x z b j j H H a A β β β β ω ω ω ω ω ω sin cos cos sin 1 2 2 6 ~ 4 _…(2.32) 為了消去角速度項，我們找出H2的零核空間(null space)V 6 ~ 4 1 ) 1 1 1 ( = − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ ⋅ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ = j j z b_{a V V A} M M …(2.33) 如此我們的平面式之純加速規慣性量測單元，可以取代三軸加速規，搭配估測器 的設計與磁場感應器，可以估測到三軸加速度與三軸角速度。

第三章：卡爾曼估測器

—Kalman Filter 、EKF、AFKF

3-1 Kalman Filter

於1960 年，由 R.E. Kalman 所發表一篇著名的論文中，利用遞迴(recursive)

來解決離散資料的線性濾波問題。由於此時電腦數值計算正蓬勃的發展，因此卡 爾曼估測器在控制與導航系統領域中被大量的研究及應用。

卡爾曼估測器是一種最佳化的估測器，它可以間接從不準確及不確定的量測 值來獲得系統的狀態變數，尤其對於雜訊來源為高斯雜訊時，卡爾曼估測器可以 得到最小化的均方誤差(mean square error)。

3-2 Extended Kalman Filter

由於卡爾曼估測器只適用於線性的系統，由於此論文的系統為非線性系統， 對於非線性的系統需線性化才能使用，因此對於被線性化的卡爾曼估測器又稱為 擴增卡爾曼估測器(Extended Kalman Filter, EKF)。

考慮以下的非線性系統

(

)

(

( ) ( )

) ( )

(

1

)

(

1,

(

1

)

) (

1

)

, , 1 + + + + = + + = + k w k x k h k z k v k u k x k f k x (3.1) 其中v 是來自於設備的雜訊，w 是量測產生的雜訊。 擴增卡爾曼估測器包括兩個主要的步驟，其中一個步驟為「狀態的預測」， 也就是從這此刻的狀態及輸入來估測下一刻時間的狀態。 Predict equations

(

)

(

(

) ( )

)

(

k k

)

F

( ) (

k P k k

) ( )

F k Q

( )

k P k u k k x k f k k x T ₊ = + = + | | 1 , | ˆ , | 1 ˆ _(3.2) 另一個步驟為“狀態的修正”，利用量測值來修正前一個步驟所預測的狀態， 以獲得較佳估測值。 Correct equations

(

)

(

) (

) (

[

)

(

) (

) (

)

]

1 1 | 1 1 1 1 | 1 1 = + + + + + + + − + T T k H k k P k H k R k H k k P k W

(

)

(

)

(

) (

[

)

(

) (

) (

)

T

]

(

)

T k W k H k k P k H k R k W k k P k k P +1| +1 = +1| − +1 +1 + +1 +1| +1 +1

(

1| 1

) (

ˆ 1|

)

(

1

)

Res

(

1

)

ˆ k+ k+ =x k+ k +W k+ k+ x …(3.3) 其中A、H 矩陣求得如下所示：

( )

( )

( )

(

)

(

)

(k k) x x k k x x x k h k H x k f k F | 1 ˆ | ˆ 1 1 + = = ∂ + ∂ = + ∂ ∂ = (3.4) 因此EKF 流程圖如下所示： State at t_k x(k) Input at t_k u(k) State estimate at t_k (k k) x |ˆ State covariance at t_k (k k) P | State covariance at t_k (k k) P | Evolution of the system (true state) Known input (control or sensor motion) Estimation of the state State covariance computation Evaluation of Jacobians ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = + Evaluation of Jacobians ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = +

State prediction covariance (k k) F( ) (kPk k) ( )Fk Q( )k

P _{+ =} T₊

| |

1

State prediction covariance (k k) F( ) (kPk k) ( )Fk Q( )k P _{+ =} T₊ | | 1 Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Filter gain ( ) ( ) ( ) ( )1 1 1 | 1 1 − + + + = + k S k H k k P k W T Filter gain ( ) ( ) ( ) ( )1 1 1 | 1 1 − + + + = + k S k H k k P k W T

Updated state covariance

( ) ( ) ( ) ( ) ( )T k W k S k W k k P k k P 1 1 1 | 1 1 | 1 + + + − + = + + 　　　　

Updated state covariance

( ) ( ) ( ) ( ) ( )T k W k S k W k k P k k P 1 1 1 | 1 1 | 1 + + + − + = + + 　　　　 State prediction (k k) f(k x(k k) ( )uk) xˆ +1| = ,ˆ | , Measurement prediction (k k) h(k x(k k)) zˆ +1| = +1,ˆ +1| Measurement residual (k 1) (zk 1) (zˆk 1|k) s Re + = + − +

Updated state estimate

( ) ( 1| ) ( 1)Res( 1) ˆ 1 | 1 ˆ + + + + = + + k k W k k x k k x 　 Transition to t_k+1 (k ) f(k x( ) ( )k uk) ( )vk x +1= , , + Measurement at t_k+1 ( ) ( ) ( 1, 1) ( 1) 1 + + + + = + k w k x k h k z 　　 v(k) w(k+1) 圖（3.1） EKF 流程圖

3-3 儲存記憶褪去式卡曼估測器Adaptive Fading Kalman Filter（AFKF）

卡爾曼估測器可以自動偵測出最佳的估測增益值，由曾經發生過的累積誤差 來決定；但若系統有不可預期的瞬間劇烈變化時，因為還有記憶以往所有曾經發

生過的資料，所以追上變化的速度很慢。AFKF 是用來解決卡爾曼估測器的發散 問題，增加了一個忽略係數λ，此係數調整卡爾曼估測器的累積記憶量，尤其對 於有不可預期的劇烈瞬間變化，可以有很好的估測效果。 在計算預估狀態協方差（covariance）處，增加忽略係數 λ：

(

k k

) (

k

) ( ) ( ) ( )

F k P k k F k Q

( )

k P _{+1| =λ +1 |} T _{+ …(3.5)} 更新狀態協方差處，更改為：P

(

k+1|k+1

)

=

(

I−W

(

k+1

) (

H k+1

)

) (

P k+1|k

)

其中忽略係數λ 為：

( )

{

( )

( )

}

(

) ( ) ( ) ( ) (

)

(

) (

) (

)

T

(

) ( ) (

)

T T T k H k Q k H k H k k P k H N k H k F k k P k F k H M M N k 1 1 1 1 | 1 1 | 1 trace trace , 1 max + + − + − + = + + = = λ …(3.6) 因此AFKF 的流程圖如下： State at tk x(k) Input at tk u(k) State estimate at tk (k k) x |ˆ State covariance at tk ( )k k P | State covariance at tk ( )k k P | Evolution of the system (true state) Known input (control or sensor motion) Estimation of the state State covariance computation Evaluation of Jacobians ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = + Evaluation of Jacobians ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = +

State prediction covariance

( ) (k ) ( ) (FkPk k) ( )Fk Q( )k k k P T₊ + = + | 1 | 1 λ Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Filter gain ( ) ( _1| ) ( ₁) ( ₁)1 1 − + + + = + k S k H k k P k W T Filter gain ( ) ( _1| ) ( ₁) ( ₁)1 1 − + + + = + k S k H k k P k W T

Updated state covariance

( ) ( ) ( ) (I Wk Hk ) (Pk k) k k P | 1 1 1 1 | 1 + + + − = + + State prediction (k k) f(kx(k k) ( )uk) x 1| ,ˆ | , ˆ + = Measurement prediction (k k) h(k x(k k)) z 1| 1,ˆ 1| ˆ + = + + Measurement residual (k 1) (zk 1) (zˆk 1|k) s Re + = + − +

Updated state estimate

( ) ( 1| ) ( 1)Res( 1) ˆ 1 | 1 ˆ + + + + = + + k k W k k x k k x 　 Transition to tk+1 (k ) f(kx( ) ( )k uk) ( )vk x +1= , , + Measurement at t_k+1 ( ) ( ) ( 1, 1) ( 1) 1 + + + + = + k w k x k h k z 　　 v(k) w(k+1) ( ) { ( ) ( )} ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )T T T T k H k Q k H k H k k P k H N k H k F k k P k F k H M M N k 1 1 1 1 | 1 1 | 1 trace trace , 1 max + + − + − + = + + = = 　　　　　　　　 λ 圖（3.2） AFKF 流程圖 經過改良後的AFKF 和原本的 EKF，比較如下圖：

圖（3.3） EKF 與 AFKF 的比較，真實值與 AFKF 估測值幾乎重疊 可以很明顯的發現，EKF 對於有瞬間劇烈變化的追跡能力遠遜於 AFKF。

第四章：新的姿態估測方法

4-1 系統模型 本量測單元，希望可以在有運動加速度的狀況下運作，所以我們一開始先假 設系統在三個軸向上都具有運動加速度，但加速度為定值，也就是說，加速度的 變化率為零： ⎪ ⎩ ⎪ ⎨ ⎧ = = = 0 0 0 z n y n x n a a a & & & …(4.1) 雖然實際運動的物體不太可能沒有加速度的變化，加速度都為定值；假設定 值的加速度在某個瞬間有步階的變化，例如原本nax=1，跳到nax=5，但若我們的 估測器可以抓的到加速度從1 跳到 5 的飄移，而這抓到的時間非常短暫，就等同 於加速度有連續變化時，我們也可以估測的到。此部分在第六章「模擬與分析」 中會詳細討論，利用不同的卡爾曼估測器的設計，縮短追上步階變化的時間，若 追上變化的時間很短，也就代表加速度其實可以自由變化，不一定要是定值。 再加上第二章中提到的四元數微分方程式和角加速度微分方程式，即可組成本估 測器之狀態方程組：

(

)

(

)

(

)

⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ⎪⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎧ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ × × ⋅ − ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ = ⋅ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − − − − − = × = = − × × × 0 ) ( :) , 3 : 1 ( 2 1 1 3 6 ~ 1 6 ~ 1 1 1 3 1 3 1 2 1 3 2 3 3 2 1 1 4 a r A J q q n j j b b b T j b j j b b & M M M M & & ω ω η ω ω η ε ε ε η ε ε ε η ε ε ε …(4.2) 離散化後的狀態方程式為：

(

) ( )

(

( )

) ( )

(

)

( )

(

)

(

[ ]

[

(

( )

(

( )

)

)

]

)

(

) ( )

⎪ ⎪ ⎪ ⎩ ⎪⎪ ⎪ ⎨ ⎧ = + × × ⋅ − ⋅ + = + ⋅ + = + × × − k a k a r k k A J dt k k k k q U k q k q n n j b b T j j b b b 1 :, 3 : 1 1 ˆ 2 dt ˆ 1 ˆ 1 6 1 6 1 _{η ω ω} ω ω ω …(4.3) 其中 Δ 是取樣時間。如此組成共有十個狀態的狀態方程式。接著推導量測 方程式。 若令此刻導航座標上的磁場向量為已知。因此可在本量測單元裝上三軸磁場 感測器，量測附體座標上的磁場；加上加速規量測物體所受到的附體加速度；再 加上四元數本身的單範限制條件，我們可以得到以下的量測方程式：

[

]

[

]

⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + + + = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = _× × × 2 3 2 2 2 1 2 1 3 1 3 1 7 0 1 η ε ε ε T b n T z n y n x n b n b b b a C a a a C m a z …(4.4) 但因為總共有10 個狀態，七個量測方程式還不足以估測車所有狀態，因此我們 再加上一個距離測量感應器，

(

)

2 2 2 8 = + + z −9.81 n y n x n_{a a a} z …(4.5) 此距離感應器是偵測系統上一點與地表上固定點間的距離，我們知道距離的資訊 和經過的時間，就可以微分得到速度與加速度，將得到的加速度純量令為z8，其 數學描述如上（4.5）式。 如此就已架構出系統的動態，配合上一章介紹的卡爾曼估測器，就可以完成 本量測單元。 4-2 流程方塊圖

x y

A

A

A

A

A

A

A

₁

　

₂

　

₃

　

₄

　

₅

　

₆

　

₃ Gyroscope-free IMU

Eq.(2.xx)

Kalman Filter

b

_g

b

_m

z

₈ b

_ω

n

_{a q}

1~4 圖（4.1）流程方塊圖 本估測器所需之感應器計有：平面式純加速規慣性量測單元、磁場感測器、單點 位置感測器(z8)。估測器運作順序為：先由平面式純加速規慣性量測單元測量到 的資訊，轉化成bax~z後，再搭配磁場感測器與單點位置感測器的資訊，當作卡 爾曼估測器的輸出方程式值，經過估測器估測後，就可以估測出精準的附體角速 度、導航加速度與系統姿態。

第五章：穩定性分析

本章將證明本論文所提出的裝置具備穩定性，而本設計元件為一姿態估測器，最 重要的就是系統之觀察性，假設一非線性系統： Measurement equation：z = h(x,t)+w(t) 若系統具有n 個狀態，m 個量測值，此非線性系統具有觀察性之條件為： n x z x z n m n m ≥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ ∂ ∂ ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ ∂ ∂ × × ) ( rank M & 因此為何本論文需比其他舊式估測器增加一「距離感應器」，就是為了滿足系統 觀察性，因為當沒有距離感應器(z8)時，系統之量測方程式為

[

]

[

]

⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + + + = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = _× × × 2 3 2 2 2 1 2 1 3 1 3 1 7 0 1 η ε ε ε T b n T z n y n x n b n b b b a C a a a C m a z 欲估測之狀態有nax~z、q1~4等共7 個狀態值，則此時之

(

)

(

)

(

)

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ + + − − − + + + − + − − − + + − − ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ + ⋅ ⋅ + ⋅ − ⋅ + ⋅ − ⋅ + ⋅ ⋅ − ⋅ ⋅ + ⋅ ⋅ + ⋅ ⋅ − ⋅ ⋅ − ⋅ ⋅ − ⋅ − ⋅ + ⋅ ⋅ + ⋅ − ⋅ + ⋅ + ⋅ ⋅ + ⋅ + ⋅ − ⋅ − ⋅ + ⋅ − ⋅ + ⋅ + ⋅ ⋅ − ⋅ − ⋅ ⋅ + ⋅ + ⋅ ⋅ + ⋅ + ⋅ − ⋅ + ⋅ − ⋅ ⋅ + ⋅ − ⋅ ⋅ − ⋅ − ⋅ ⋅ + ⋅ + ⋅ ⋅ + ⋅ + ⋅ − = ∂ ∂ 0 0 0 0 0 0 0 0 0 0 0 0 2 / 2 / 2 / 2 2 2 3 2 2 2 1 1 3 2 2 3 1 1 3 2 2 2 3 2 2 2 1 3 2 1 2 3 1 3 2 1 2 2 3 2 2 2 1 3 2 1 3 1 2 3 1 2 2 3 1 2 3 1 3 1 2 3 1 2 3 2 1 3 2 3 1 2 1 3 2 3 2 1 2 1 3 1 3 1 2 1 3 2 1 3 2 , η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε ε ε ε η ε ε η ε ε ε η ε η ε ε ε η ε ε ε ε ε η ε ε ε η ε ε ε ε η ε ε η ε ε η ε ε η ε ε ε ε ε η ε ε η ε ε η ε ε η ε ε ε ε ε η ε ε b a a b a b b a a b b a b a a b a b b a b a a b a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a x z z n y n x n x n y n z n y n x n z n z n x n y n x n y n z n z n y n x n z n x n y n y n x n z n y n x n z n z n x n y n z n y n x n x n y n z n a qn

且 ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛ ∂ ∂ a qn x z , & 不會增加矩陣秩數，因此 n x z x z n m n m < ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ ∂ ∂ ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ ∂ ∂ × × ) ( rank M & _{，系統不可觀察。} 因此我們為了達到系統觀察性，增加了一「距離感應器」：z8 狀態方程式為：

( )

⎪ ⎪ ⎪ ⎩ ⎪⎪ ⎪ ⎨ ⎧ = ⋅ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − − − − − = × × × 0 2 1 1 3 1 3 1 2 1 3 2 3 3 2 1 1 4 a q q n b & & ω η ε ε ε η ε ε ε η ε ε ε …(5.1) 量測方程式：

[

]

[

]

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − + + + + + = ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = _× × × 2 2 2 2 3 2 2 2 1 2 8 1 3 1 3 1 8 ) 81 . 9 ( 0 1 z n y n x n T b n T z n y n x n b n b b a a a b a C a a a C z m a z ε ε ε η …(5.2)

(

)

(

)

(

)

(

)

⎥⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ − + + − − + + − + + + + − − − + + + − + − − − + + − − ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⋅ + ⋅ ⋅ + ⋅ − ⋅ + ⋅ − ⋅ + ⋅ ⋅ − ⋅ ⋅ + ⋅ ⋅ + ⋅ ⋅ − ⋅ ⋅ − ⋅ ⋅ − ⋅ − ⋅ + ⋅ ⋅ + ⋅ − ⋅ + ⋅ + ⋅ ⋅ + ⋅ + ⋅ − ⋅ − ⋅ + ⋅ − ⋅ + ⋅ + ⋅ ⋅ − ⋅ − ⋅ ⋅ + ⋅ + ⋅ ⋅ + ⋅ + ⋅ − ⋅ + ⋅ − ⋅ ⋅ + ⋅ − ⋅ ⋅ − ⋅ − ⋅ ⋅ + ⋅ + ⋅ ⋅ + ⋅ + ⋅ − = ∂ ∂ = 2 2 2 2 2 2 2 2 2 2 2 3 2 2 2 1 1 3 2 2 3 1 1 3 2 2 2 3 2 2 2 1 3 2 1 2 3 1 3 2 1 2 2 3 2 2 2 1 3 2 1 3 1 2 3 1 2 2 3 1 2 3 1 3 1 2 3 1 2 3 2 1 3 2 3 1 2 1 3 2 3 2 1 2 1 3 1 3 1 2 1 3 2 1 3 2 , ) 81 . 9 ( 81 . 9 ) 81 . 9 ( ) 81 . 9 ( 0 0 0 0 0 0 0 0 0 0 0 0 2 / 2 / 2 / 0 0 0 0 2 z y x z z y x y z y x x z n y n x n x n y n z n y n x n z n z n x n y n x n y n z n z n y n x n z n x n y n y n x n z n y n x n z n z n x n y n z n y n x n x n y n z n a q a a a a a a a a a a a a b a a b a b b a a b b a b a a b a b b a b a a b a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a a x z H n η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε η ε ε ε ε ε ε η ε ε η ε ε ε η ε η ε ε ε η ε ε ε ε ε η ε ε ε η ε ε ε ε η ε ε η ε ε η ε ε η ε ε ε ε ε η ε ε η ε ε η ε ε η ε ε ε ε ε η ε ε …(5.3) 經過Matlab 檢查，rank(H)=7，即系統可觀察，四元數與加速度等七個狀態 一定可以估測的到。接著，為讓系統可以套用純平面式無陀螺儀慣性量測單元， 增加狀態方程式（eq2.28），且新增三個狀態 ωx~z，量測方程式不變。因此系統 運作上是先利用量測到的資訊加上狀態方程式，使得四元數以及加速度等七個狀 態收斂後，再藉由狀態方程式的關係，估測出ωx~z等三個狀態。 也因此我們可以預期，在這十個狀態估測中，角速度這三個狀態會是最慢收 斂的，其估測的精準程度也要仰賴其他狀態收斂的成果。

第六章：模擬結果與討論

為了驗證前述之理論，故用MATLAB 進行模擬，利用第四章介紹的新的姿 態估測方法，搭配擴增卡爾曼估測器和適應性忽略記憶卡爾曼估測器，令取樣頻 率為1000HZ，利用電腦產生包含高斯分布的隨機雜訊，其平均值為零。給予加 速規的雜訊標準差為0.0001 m/sec^2，而磁場感測器雜訊標準差 0.0001gauss，並 設計各種運動方式來了解演算法之特性。 假設輸入尤拉角φ、θ、ψ 變化皆為弦波訊號， φ=Amp1×cos(2πf1t) θ=Amp2×sin(2πf2t) ψ=Amp3×sin(2πf3t)

其中，Amp1=π/6，Amp2=π/2，Amp3=π/7；f1=5，f2=3，f3=2。

假設當地磁場強度為[0.5 0 0.7]T。以電腦程式分三種狀況模擬，沒有加速度 時、有運動加速度時、以記憶褪去式卡爾曼估測器追跡。

6.1 運動加速度為定值時的姿態估測

[Case1] 我們所提出的新姿態估測方法，最大的不同點在於有沒有運動加速 度的條件，在我們的系統中，假設加速度的變化率為零，即加速度為定值，以這 樣的條件去估測，首先我們先嘗試若加速度如我們假設般為定值時的模擬狀態， n_{a=[0 0 9.8]。} 由假設的輸入尤拉角變化狀況與na，我們可以得到平面式純加速規慣性量測 單元的量測值，再由假設的磁場狀態，可以得到磁場感測器的測量值。圖（6.1） 是系統感測器量到的訊號，將之輸入於卡爾曼估測器後，可以得到如圖（6.1） ～（6.4）的估測姿態值和真實值相比之圖。將四元數、角速度、加速度等十個 系統狀態的誤差收斂情形繪於圖（6.5），可以發覺除了角速度之外，其他幾個系 統狀態都收斂得很快，其實這樣的情形可以在measurement equation 中略知一 二，本系統的八個量測方程式中，都沒有測量到角速度項，所以角速度要倚靠四 元數（姿態）收斂後，才可以靠著state equation 的關係收斂，若未來希望改善這 樣的情形，可以加上陀螺儀來提高系統精度。

0 0.5 1 1.5 2 2.5 3 -10 -5 0 5 10 time(sec) m/ s e c 2

3-axis accelerometer and 3-axis magnetometer measured signal

b_a x b a_y b a_z 0 0.5 1 1.5 2 2.5 3 -1 -0.5 0 0.5 1 time(sec) g aus s b m_x b_m y b_m z 圖（6.1）三軸加速規與磁場感應器訊號圖 0 1 2 3 0.5 1 1.5 2 time(sec)

estimated state and true state (Quaternion)

η η_e 0 1 2 3 -3 -2 -1 0 1 time(sec) ε₁ ε₁(e) 0 1 2 3 -1 -0.5 0 0.5 1 1.5 time(sec) ε₂ ε₂(e) 0 1 2 3 -1 -0.5 0 0.5 time(sec) ε₃ ε₃(e) 圖（6.2）姿態四元數的真實值與估測值(幾乎重疊)

0 0.5 1 1.5 2 2.5 3 -10 -5 0 5 10 time(sec) m/ s e c 2

estimated state and true state (n_a)

c₁ c₁(e) 0 0.5 1 1.5 2 2.5 3 -10 -5 0 5 10 time(sec) m/ s e c 2 c₂ c₂(e) 0 0.5 1 1.5 2 2.5 3 -5 0 5 10 15 time(sec) m/ s e c 2 c₃ c₃(e) 圖（6.3）導航座標加速度的真實值與估測值(幾乎重疊) 0 0.5 1 1.5 2 2.5 3 -50 0 50 100 time(sec) rad/ s ec

estimated state and true state (b_ω)

b_ω x b_ω x(e) 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 time(sec) rad/ s ec b_ω y b_ω y(e) 0 0.5 1 1.5 2 2.5 3 -50 0 50 time(sec) rad/ s ec b_ω z b_ω z(e) 圖（6.4）附體座標角速度的真實值與估測值

0 0.5 1 1.5 2 2.5 3 -2

0 2 4

error = true state - estimated state ; (Quaternion & b_{ω &} n_a)

time(sec) error 1(η) error 2(ε₁) error 3(ε2) error 4(ε₃) 0 0.5 1 1.5 2 2.5 3 -50 0 50 time(sec) rad/ s ec error 5(bω_x) error 6(bω_y) error 7(bω_z) 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 time(sec) m/ s e c 2 error 8(c₁) error 8(c₂) error 10(c₃) 圖（6.5） 10 個估測狀態之誤差收斂情形 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degr ee time(sec)

estimated and true Eular Angle (φ,θ,Ψ)

roll roll_e 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degr ee time(sec) pitch pitch_e 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degr ee time(sec) yaw yaw_e 圖（6.6）尤拉角的真實值與估測值

0 0.5 1 1.5 2 2.5 3 -150 -100 -50 0 50 100 150 degr ee time(sec)

error covergence of Eular Angle (φ,θ,Ψ)

error(φ) error(θ) error(Ψ) 圖（6.7）尤拉角誤差收斂情形 圖（6.6）（6.7)是將四元數資訊轉換成尤拉角，估測值與實際值的比較，和 誤差收斂情形，因為尤拉角相較於四元數在平常看時較有物理意義。圖中的突起 處，是因為四元數轉換成尤拉角時的三角函數關係，

(

)

(

)

(

)

(

)

⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ − − + + = + − = ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ + − − + = − − − 2 3 2 2 2 1 2 3 2 1 1 2 3 1 1 2 3 2 2 2 1 2 1 3 2 1 2 tan 2 sin 2 tan ε ε ε η ηε ε ε ψ ηε ε ε θ ε ε ε η ηε ε ε φ φ 和 ψ 是透過tan-1轉換的，所以會有一些突起，θ 是經過sin-1轉換的，就不會有 這樣的問題，不過實際上的姿態估測，從四元數處觀察，是確實有估測到的。 [Case2] 同前述所有條件，若將加速規和磁場感測器的雜訊標準差提高到 0.01m/sec^2 和 0.01gauss，模擬結果如下。從圖（6.8）～（6.10）的實際值和估 測值比較中，還看不太出差異，但觀察圖（6.11）誤差收斂情形時，可以發覺當 系統感應器的雜訊增加時，角速度的收斂狀況會變差，其他加速度與四元數的收 斂狀況就還可以，角速度受到的影響最大。要改善這樣的問題，除了可以增加陀 螺儀外，在6.3 節中會以不同的估測器設計方法來改良這樣的問題。

0 1 2 3 -0.5 0 0.5 1 time(sec)

estimated state and true state (Quaternion)

η η_e 0 1 2 3 -1 -0.5 0 0.5 1 time(sec) ε₁ ε₁(e) 0 1 2 3 -1 -0.5 0 0.5 1 time(sec) ε₂ ε₂(e) 0 1 2 3 -0.5 0 0.5 1 time(sec) ε₃ ε₃(e) 圖（6.8）姿態四元數的真實值與估測值 0 0.5 1 1.5 2 2.5 3 -50 0 50 100 time(sec) m/ s e c 2

estimated state and true state (n_a)

c₁ c₁(e) 0 0.5 1 1.5 2 2.5 3 -20 0 20 40 60 time(sec) m/ s e c 2 c₂ c₂(e) 0 0.5 1 1.5 2 2.5 3 -60 -40 -20 0 20 time(sec) m/ s e c 2 c₃ c 3(e) 圖（6.9）導航座標加速度的真實值與估測值

0 0.5 1 1.5 2 2.5 3 -40 -20 0 20 40 time(sec) rad/ s ec

estimated state and true state (b_ω)

b_ω x b_ω x(e) 0 0.5 1 1.5 2 2.5 3 -40 -20 0 20 40 time(sec) rad/ s ec b_ω y b_ω y(e) 0 0.5 1 1.5 2 2.5 3 -20 -10 0 10 20 time(sec) rad/ s ec b_ω z b_ω z(e) 圖（6.10）附體座標角速度的真實值與估測值 0 0.5 1 1.5 2 2.5 3 -1 -0.5 0 0.5 1

error = true state - estimated state ; (Quaternion & b_{ω &} n_a)

time(sec) error 1(η) error 2(ε₁) error 3(ε₂) error 4(ε₃) 0 0.5 1 1.5 2 2.5 3 -5 0 5 time(sec) rad/ s ec error 5(bω_x) error 6(bωy) error 7(bω_z) 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 time(sec) m/ s e c 2 error 8(c₁) error 8(c 2) error 10(c₃) 圖（6.11）10 個估測狀態之誤差收斂情形

0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degr ee time(sec)

estimated and true Eular Angle (φ,θ,Ψ)

roll roll e 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degr ee time(sec) pitch pitch e 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degr ee time(sec) yaw yaw_e 圖（6.12）尤拉角的真實值與估測值 6.2 運動加速度非定值時的姿態估測 [Case3] 在我們所提出的新姿態估測方法中，為了增加估測加速度，在狀態 方程式中假設na=0 & ，但若估測器的性能夠好，可以在加速度有變化時追上的 話，而且追上的時間夠短，本系統就可以成功在加速度非定值下運作。以下測試 n_a y在1 秒、1.5 秒、2 秒時，各有一次步階變化，如圖（6.15）所示。可以觀察 到，在第一次步階變化時，追上的速度較慢，但第二刺、第三次就很快追上了， 這是因為卡爾曼估測器可以累記之前發生過的雜訊或者誤差，做出更好的判斷。 圖（6.13）是系統感測器量到的訊號，將之輸入於卡爾曼估測器後，可以得到 如圖（6.14）～（6.16）的估測姿態值和真實值相比之圖。將四元數、角速度、 加速度等十個系統狀態的誤差收斂情形繪於圖（6.17），圖（6.18）是將四元數 資訊轉換成尤拉角，估測值與實際值的比較。

0 0.5 1 1.5 2 2.5 3 -15 -10 -5 0 5 10 15 time(sec) m/ s e c 2

3-axis accelerometer and 3-axis magnetometer measured signal

b_a x b a_y b a_z 0 0.5 1 1.5 2 2.5 3 -1 -0.5 0 0.5 1 time(sec) g aus s b m_x b_m y b_m z 圖（6.13）三軸加速規與磁場感應器訊號圖 0 1 2 3 0 0.5 1 1.5 time(sec)

estimated state and true state (Quaternion)

η η_e 0 1 2 3 -0.4 -0.2 0 0.2 0.4 0.6 time(sec) ε₁ ε₁(e) 0 1 2 3 -1 -0.5 0 0.5 1 time(sec) ε₂ ε₂(e) 0 1 2 3 -1 -0.5 0 0.5 1 time(sec) ε₃ ε₃(e)

0 0.5 1 1.5 2 2.5 3 -10 -5 0 5 10 time(sec) m/ s e c 2

estimated state and true state (n_a)

n_a x n_a x(e) 0 0.5 1 1.5 2 2.5 3 0 5 10 time(sec) m/ s e c 2 n_a y n_a y(e) 0 0.5 1 1.5 2 2.5 3 -5 0 5 10 15 time(sec) m/ s e c 2 n_a z n_a z(e) 圖（6.15）導航座標加速度的真實值與估測值 0 0.5 1 1.5 2 2.5 3 -50 0 50 time(sec) rad/ s e c

estimated state and true state (b_ω)

b_ω x b_ω x(e) 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 time(sec) ra d /s e c b_ω y b_ω y(e) 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 time(sec) rad/ s ec b_ω z b_ω z(e) 圖（6.16）附體座標角速度的真實值與估測值

0 0.5 1 1.5 2 2.5 3 -0.5

0 0.5 1

error = true state - estimated state ; (Quaternion & bω & na)

time(sec) error 1(η) error 2(ε₁) error 3(ε₂) error 4(ε₃) 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 time(sec) rad/ s ec error 5(bω_x) error 6(bω_y) error 7(bω_z) 0 0.5 1 1.5 2 2.5 3 -20 -10 0 10 20 time(sec) m/ s e c 2 error 8(c₁) error 8(c₂) error 10(c₃) 圖（6.17）10 個估測狀態之誤差收斂情形 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degree time(sec)

estimated and true Eular Angle (φ,θ,Ψ)

roll roll_e 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 deg ree time(sec) pitch pitch_e 0 0.5 1 1.5 2 2.5 3 -100 -50 0 50 100 degree time(sec) yaw yaw_e