本篇論文探討使用腳部穿戴式，單組與雙組慣性感測器(IMU)的
兩種行人軌跡追綜方法。
我們使用的IMU是EcoIMU，是由一顆MPU9250九軸感測器以及
CC2541微處理器所組成的慣性測量系統，
其特性包含微小化、即時性、無線等等。
將九軸感測器感測的資料透過低耗能藍芽(BLE)無線通訊協定
傳到電腦端轉換成三維空間移動的方向及軌跡。
我們提出的方法，透過Savitzky-Golay可調變維度的多項式濾波器、
慣性姿態演算法、Madgwick方向權重估測等方式，
降低了目前IMU在室內定位誤差累積的問題。

第一組實驗結果顯示，
本篇實驗了單腳單顆IMU以及雙腳雙IMU的模式，
單IMU實驗中證明了最小平方差多項式濾波器、姿態演算法、磁力計補償方向
使方向角度上可以更加精準。
第二組實驗結果顯示，
使用雙腳雙IMU時，我們透過超音波感測器牽制兩隻腳飄移距離的誤差，
可以有效移除單顆IMU在步伐寬度上的漂移誤差。

This thesis investigates two techniques for tracking the trajectory
of a pedestrian using one and two foot-mounted inertial measurement
units (IMU) called EcoIMU. Each EcoIMU transmits data from its 9
degree-of-freedom (9-DoF) inertial sensor, i.e., accelerometer, a
triaxial gyroscope, and a triaxial compass, via Bluetooth Low Energy
(BLE) protocol to a PC for analysis. We propose an algorithm to
calculate the orientation and trajectory in 3D by two-degree
Savitzky-Golay filter (TDSG) with zero-velocity update, and we use
Madgwick algorithm and heading direction method to reduce the error.

First experimental results show that accuracy of orientation and
distance error from double integration can be improved by
TDSG
and Madgiwck methods successfully. Second experimental results show
that by adding ultrasonic sensor to limit the distance between two
IMU on different feet, the drifts on stride widths can be reduced
significantly. Our proposed techniques also achieve
less error disance
compared to the state-of-the-art in indoor localization.

Contents i

Acknowledgments vi

1 Introduction 1
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1.1 Drift of Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.1.2 Accumulated Error of Position . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Contribution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Thesis Organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Related Work 4

3 Background Theory 6
3.1 Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3.1.1 Euler Angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3.1.2 Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.1.3 Rotation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2 Filters for IMU Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2.2 Savitzky-Golay Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2.3 Mahony Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2.4 Madgwick Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

4 System Architecture and Implementation 18
4.1 Node Subsystem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

i


4.2 Host Subsystem of Single IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.2.1 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.2.2 Two-Degree Savitzky–Golay Filter . . . . . . . . . . . . . . . . . . . . . . 20
4.2.3 Madgwick Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.2.4 Heading Direction Detection . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.2.5 Step Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.2.6 Calculation of Linear Displacement . . . . . . . . . . . . . . . . . . . . . . 28
4.2.7 Zero Velocity Mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.3 Dual-EcoIMU Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.3.1 Ultrasonic sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.3.2 Distance Data Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

5 Evaluation 32
5.1 Single IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
5.1.1 Performance with different heading direction estimation . . . . . . . . . . . 33
5.1.2 Performance with different high degree of two degree SG filter . . . . . . . . 35
5.1.3 Performance with different sampling rate . . . . . . . . . . . . . . . . . . . 37
5.2 Dual IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2.1 Performance with EcoIMU and ultrasonic sensor . . . . . . . . . . . . . . . 40

6 Conclusions and Future Work 45
6.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

Appendix 49
Main Code Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

[1] P. Molchanov, S. Gupta, K. Kim, and K. Pulli. Short-range FMCW monopulse radar for hand-
gesture sensing. In 2015 IEEE Radar Conference (RadarCon), pages 1491–1496, May 2015.
doi: 10.1109/RADAR.2015.7131232.

[2] Rudolph Emil Kalman. A new approach to linear filtering and prediction problems. Journal of
basic Engineering, 82(1):35–45, 1960.

[3] Abraham. Savitzky and M. J. E. Golay. Smoothing and differentiation of data by simplified least
squares procedures. Analytical Chemistry, 36(8):1627–1639, 1964. doi: 10.1021/ac60214a047.

[4] Phillip. Barak. Smoothing and differentiation by an adaptive-degree polynomial filter. Analytical
Chemistry, 67(17):2758–2762, 1995. doi: 10.1021/ac00113a006.

[5] R. Mahony, T. Hamel, and J. M. Pflimlin. Nonlinear complementary filters on the special or-
thogonal group. IEEE Transactions on Automatic Control, 53(5):1203–1218, June 2008. ISSN
0018-9286. doi: 10.1109/TAC.2008.923738.

[6] Sabastian O. H. Madgwick, A. J. Harrison, and Ravi Vaidyanathan. Estimation of IMU and
MARG orientation using a gradient descent algorithm. In 2011 IEEE International Conference
on Rehabilitation Robotics, pages 1–7, June 2011. doi: 10.1109/ICORR.2011.5975346.

[7] A. Cavallo, A. Cirillo, P. Cirillo, G. De Maria, P. Falco, C. Natale, and S. Pirozzi. Experimental
comparison of sensor fusion algorithms for attitude estimation. IFAC Proceedings Volumes, 47
(3):7585 – 7591, 2014. ISSN 1474-6670. doi: http://dx.doi.org/10.3182/20140824-6-ZA-1003.
01173. URL http://www.sciencedirect.com/science/article/pii/S1474667016428089. 19th
IFAC World Congress.

[8] W. Kang and Y. Han. SmartPDR: Smartphone-based pedestrian dead reckoning for indoor

47


localization. IEEE Sensors Journal, 15(5):2906–2916, May 2015. ISSN 1530-437X. doi:
10.1109/JSEN.2014.2382568.

[9] H. Guo, M. Uradzinski, and H. Yin. Indoor positioning based on foot-mounted IMU. In Bulletin
of the Polish Academy of Sciences Technical Sciences, pages 629–634, 2015. doi: 10.1515/
bpasts-2015-0074.

[10] G. V. Prateek, R. Girisha, K. V. S. Hari, and P. Händel. Data fusion of dual foot-mounted ins to
reduce the systematic heading drift. In 2013 4th International Conference on Intelligent Systems,
Modelling and Simulation, pages 208–213, Jan 2013. doi: 10.1109/ISMS.2013.46.