第四章 感測器整合系統
4.3 感測器整合系統模型
4.5.2 加速規與陀螺儀輸入訊號帶有偏壓
以兩顆 GPS 之導航座標系資訊直接帶入估測系統中之輸出系統做為依 據，架構延伸建立一估測狀態則包括四元數、三軸角速度常數偏壓、三軸 加速度常數偏壓、導航座標系三軸 O 點速度、導航座標系三軸 O 點位置即
16 1
b b NED NED T
o o o o
X q dw da V L
相同低以VA oNED/ 、LNEDA o/ 、LNEDo 做為輸出系統，其所帶入之資訊則包含導 航座標系的資訊，其方程式如下
41
NED NED
o o
NED NED
o o
NED NED o o
42
NED NED
o b
43
圖(4.23)四元數對照圖
圖(4.24)陀螺儀訊號偏壓對照圖
44
圖(4.25)加速規訊號偏壓對照圖
圖(4.26)導航座標系 O 點速度對照圖
45
圖(4.27)導航座標系 O 點位置對照圖
將導航座標系資訊直接帶入觀察系統中，如預期所示，可估測精確的 姿態。
4.6 2 顆 GPS+Barometer+IMU
藉由增加可觀察的資訊即增加系統輸出，避免因座標轉換矩陣所造成 四元數估測狀態造成誤判情行，可使觀察性矩陣滿秩，以達到估測系統準 確估測運動物體姿態。在此嘗試以兩顆 GPS 並增加 Barometer 以獲得導航 座標系的高度資訊，建立兩種系統輸入架構並驗證其可行性。其感測元件 擺放位置如下圖(4.28)所示，GPS-A 放置於 A 點，GPS-B 放置於 B 點，而 Barometer 與 IMU 放置於估測位置 O 點。
46
NED NED
o Z o Z
NED NED NED b
A o b A o
NED NED NED b
B o b B o
NED NED
o Z o Z
NED NED
o Z o Z
47
NED NED
o Z o Z
NED NED
o Z o Z
NED NED
o Z o Z
48
NED NED
o b
49
圖(4.29)四元數對照圖
圖(4.30)角速度對照圖
50
圖(4.31)附體座標系 O 點加速度對照圖
圖(4.32)導航座標系 O 點速度對照圖
51
NED NED
o Z o Z
52
NED NED
o Z o Z
NED NED
o Z o Z
53
NED NED
o Z o Z
NED NED
o b
54
圖(4.34)四元數對照圖
圖(4.35)陀螺儀訊號偏壓對照圖
55
圖(4.36)加速規訊號偏壓對照圖
圖(4.37)導航座標系 O 點速度對照圖
56
圖(4.38)導航座標系 O 點位置對照圖
其模擬結果亦如我們先前所預期之結果，觀察性矩陣為滿秩故系統為 可觀察系統，且以 MatLab 模擬結果亦可精準估測運動物體之路徑與姿態。
4.7 3 顆 GPS+IMU
以三顆 GPS 做為系統輸出為一般常見用以估測角度變化之架構，於本 文中亦重新建構其估測系統模型，模擬其估測情況，並以此架構為基準。
估測狀態則包括四元數、三軸角速度常數偏壓、三軸加速度常數偏壓、導 航座標系三軸 O 點速度、導航座標系三軸 O 點位置即
16 1
b b NED NED T
o o o o
X q dw da V L
此時 IMU 所量測之角速度與加速度則為系統輸入，其感測元件擺放位 置如下圖(4.39)所示，GPS-A 放置於 A 點，GPS-B 放置於 B 點，而 GPS-C 與 IMU 放置於 O 點而其 O 點亦為估測位置。
57
NED NED
o b
NED NED
o o
NED NED b
A o b A o
NED NED b
B o b B o
NED NED
o o
58
NED NED
o o
NED NED o o
59
16rank O
觀察性矩陣滿足滿秩的條件，故此架構應為可精確的估測到四元數等 等估測狀態，透過 EKF 估測系統模擬如下：
針對此系觀察系統之 EKF 估測動態架構，模擬估測狀態為
16 1
b b NED NED T
o o o o
X q dw da V L
圖(4.40)四元數對照圖
圖(4.41)陀螺儀訊號偏壓對照圖
60
圖(4.42)加速規訊號偏壓對照圖
圖(4.43)導航座標系 O 點速度對照圖
61
圖(4.44)導航座標系 O 點位置對照圖
由模擬結果發現其估測之角速度偏壓與加速度偏壓之誤差低於百分 之一，亦精確估測到四元數等等之估測狀態，符合預期之結果。
62
第五章 DSP 實現系統訊號整合