EXTENDED KALMAN FILTER AND STEREOSCOPIC VISION BASED AUTONOMOUS FLYING SYSTEM FOR QUADCOPTERS Amila Sandaruwan Somasiri (149364C) Degree of Master of Science Department of Electrical Engineering University of Moratuwa Sri Lanka June 2018 EXTENDED KALMAN FILTER AND STEREOSCOPIC VISION BASED AUTONOMOUS FLYING SYSTEM FOR QUADCOPTERS Jahapu Appuhamilage Amila Sandaruwan Somasiri (149364C) Dissertation submitted in partial fulfilment of the requirements for the degree Master of Science in Industrial Automation Department of Electrical Engineering University of Moratuwa Sri Lanka June 2018 i DECLARATION OF THE CANDIDATE & SUPERVISORS I declare that this is my own investigation and this dissertation does not incorporate without acknowledgement any material previously submitted for a Degree or Diploma in any other University or institute of higher learning and to the best of my knowledge and belief it does not contain any material previously published or written by another person except where the acknowledgement is made in the text. Also, I hereby grant to University of Moratuwa the non-exclusive right to reproduce and distribute my thesis/dissertation, in whole or in part in print, electronic or other medium. I retain the right to use this content in whole or part in future works (such as articles or books). ……………………….. ………………... J.A.A.S Somasiri Date The above candidate has carried out research for the Masters Dissertation under our supervision. ……………………….. ……….............. Dr. D.P. Chandima Date ……………………….. ………………… Dr. A.G.B.P Jayasekara Date ii ACKNOWLEDGMENTS First of all, I would like to express my deepest gratitude to my research supervisors Dr. D. P Chandima and Dr. A. G. B. P Jayasekara for all the outstanding guidance, encouragement, advices and support, throughout the MSc thought course and related research. I would like to express my deep appreciation to Head of the Department and academic staff of Department of Electrical Engineering, Faculty of Engineering, University of Moratuwa for their kind support and the guidance throughout the course. Also, I owe special thanks to Dr. Sanjeeva Maithripala Senior Lecturer, Department of Mechanical Engineering, Faculty of Engineering, University of Peradeniya for his kind guidance throughout my master degree. My sincere thanks also goes to my colleagues who are working with me and CodeGen (Pvt) Ltd for generous support throughout my master degree. I would also like to thanks non-academic staff of Department of Electrical Engineer- ing, Faculty of Engineering, University of Moratuwa for their kind support. Furthermore, I would express my heartiest thanks my beloved parents, my beloved wife and beloved family members for their continuing love and support during my master degree. iii Abstract This thesis can be divided into two main modules. First module is implementation of an Ex- tended Kalman filter and introduce into existing flight control algorithm which is used to con- trol multi-rotor unmanned vehicles. Purpose of this implementation is to improve flight per- formance and reliability of the system. Second module is implementation of an obstacle avoid- ance system based on stereo vision and fuzzy logic for same flight control algorithm to avoid crashes and avoid obstacles during navigation. In this thesis Chapter 1 introduce basic modules of this implementations and explain about flight control algorithm and its major components which is used in here. This chapter also explains the theory behind the Extended Kalman Fil- ters, stereo vision systems and fuzzy logic. Chapter 2 described literature survey about existing implementation of Extended Kalman filters on multi-rotor platforms, stereo vision system im- plementations and related obstacle avoidance implementations like artificial potential field and fuzzy logic. First section of chapter 3 focused into implementation details and experimenting results of Extended Kalman filter and also explained how Extended Kalman filter outputs are combined to Attitude and Position controllers of flight control algorithm. Second section of chapter 3 focused into implementation and experimenting results of the stereo vision system. This section explained detail implementation of stereo vision system like stereo camera cali- bration, image rectification, disparity map generation and depth calculation. Mainly OpenCV was used in this implementation. Third section of chapter 3 focused into explained implemen- tation of fuzzy decision-making system. In here described deciding of fuzzy inputs and outputs using depth image, creation of fuzzy inference system, selection of membership functions and combined fuzzy decision-making system with flight control algorithm. Flight testing and ex- perimental results of Extended Kalman filter and obstacle avoidance system were described in chapter 4, both systems were tested on outdoor environments and improvement of the perfor- mance and reliability was discussed in this chapter. Chapter 5 is the final chapter of this thesis and it includes conclusion of the thesis, recommendations and further works. Keywords: Quadcopters, Kalman Filters, Obstacle Avoidance, Stereo Vision, Fuzzy Logic. iv TABLE OF CONTENTS DECLARATION OF THE CANDIDATE & SUPERVISORS i ACKNOWLEDGMENTS ii Abstract iii TABLE OF CONTENTS iv LIST OF FIGURES vi LIST OF ABBREVIATIONS ix LIST OF APPENDICES x INTRODUCTION 1 Flight control algorithm 2 Hardware Abstraction Layer 2 State estimation 3 Attitude control & Position control 3 Communication protocol 3 Key features of the flight control algorithm 5 Kalman filter 6 Extended Kalman filter 7 Stereoscopic Vision 8 Camera calibration 8 Image rectification 10 Disparity 11 Pixel correlation (Block matching) 12 Disparity calculation (Stereo correspondence) algorithms 12 Depth calculation 14 Fundamental problems in stereo correspondence 15 Fuzzy Logic 16 Membership functions 16 Fuzzy inference system 16 LITERATURE REVIEW 17 Related quadcopter states estimator implementations 17 Related stereo vision implementations 17 Related obstacle avoidance implementations 18 METHODOLOGY 19 Development of the Extended Kalman filter 19 Inputs and outputs of the filter 19 v Extended Kalman filter equations 20 Hardware selection and assembling of the Quadcopter 25 Experimental results of the filter 29 Introduce Extended Kalman filter outputs to flight control algorithm 34 Development of the stereo vision system 36 Selection of the stereo camera 36 Stereo camera calibration 38 Image rectification 40 Disparity calculation 40 Manual calibration of the depth image 44 Testing of stereo vision system on outdoor environment 45 Development of the fuzzy decision-making system 47 Analyzing the Depth Image 47 Calculation of size of sub regions 47 Calculation of normalize depth values of sub regions 49 Determination of fuzzy inputs and outputs 49 Determination of fuzzy rules 50 Selection of input and output membership functions 52 Selection of defuzziffication method 53 Introduce fuzzy inference system output to flight control algorithm 54 Testing of the fuzzy system on outdoor environment 55 EXPERIMENTAL RESULTS AND ANALYSIS 58 Flight test and result analyzing of Extended Kalman filter 58 Hardware setup 58 Experimental procedure 58 Results 59 Position controller error variation with Extended Kalman filter 64 Problems encountered during flight tests 67 Flight tests and results analyzing of obstacle avoidance system 68 Hardware setup 68 Experimental procedure 68 Results 69 Problems encountered during flight tests 70 CONCLUSIONS 71 RECOMMENDATIONS AND FURTHER WORKS 72 REFERENCES 74 APPENDICES 76 vi LIST OF FIGURES Figure 1.1 Basic components of simple Multi-copter 4 Figure 1.2 MAVLink communication protocol block diagram 4 Figure 1.3 Graphical user interface use to communicate with quadcopter 5 Figure 1.4 Radial distortion 8 Figure 1.5 Tangential distortion 9 Figure 1.6 Rectification of a stereo pair. 10 Figure 1.7 The Tsukuba stereo image pair: 11 Figure 1.8 Relationship between depth of a point and disparity of same point 15 Figure 1.9 Block diagram of a fuzzy inference system. 16 Figure 3.1 Navio2 flight shield connected with Raspberrypi 3 computer board. 26 Figure 3.2 Selected 850KV brushless dc motor and 20Amp ESC 26 Figure 3.3 Selected transmitter and receiver 27 Figure 3.4 Selected Radio telemetry 27 Figure 3.5 Selected lithium-polymer battery 28 Figure 3.6 Actual hardware set-up of the system 28 Figure 3.7 Connections of the hardware components 29 Figure 3.8 GPS position vs EKF position for Latitude direction 30 Figure 3.9 Zoom version of Figure 3.8 30 Figure 3.10 GPS position vs EKF position for Longitude direction 31 Figure 3.11 Zoom version of Figure 3.10 31 Figure 3.12 GPS velocity vs EKF velocity for Latitude direction 32 Figure 3.13 GPS velocity vs EKF velocity for Longitude direction 32 Figure 3.14 Barometer height vs EKF height 33 Figure 3.15 Barometer climb rate vs EKF climb rate 34 Figure 3.16 Attitude control of quadcopter 35 Figure 3.17 Position control of quadcopter 35 Figure 3.18 Normal USB web camera. 36 Figure 3.19 Stereo camera build using two normal USB web camera 36 Figure 3.20 ZED stereo camera with Nvidia Jetson TX1 computing platform. 38 Figure 3.21 Stereo calibration using a 7 × 10 chessboard pattern 39 vii Figure 3.22 Raw images (Top) Rectified images (Bottom) 40 Figure 3.23 User interface for block matching algorithm 42 Figure 3.24 Effect of changing number of disparities. 42 Figure 3.25 Disparity maps for different SAD window sizes 43 Figure 3.26 Input raw images and corresponding depth map 44 Figure 3.27 Moment of manual calibration of depth image 45 Figure 3.28 Quality of the depth image in different scenarios 46 Figure 3.29 Depth map’s division in nine windows. 47 Figure 3.30 Visible image plane of the stereo camera 48 Figure 3.31 Dimension of each sub regions 49 Figure 3.32 Navigation direction deciding based on depth image. 50 Figure 3.33 Input membership funcition of desired velocity input (𝑉𝑖𝑛). 52 Figure 3.34 Input membership funcition of normalize depth value of one region (𝐷𝑙𝑢 ) 52 Figure 3.35 Output membership funcitions of 𝑉𝑥, 𝑉𝑧 53 Figure 3.36 Output membership funcition of 𝑉𝑦 53 Figure 3.37 Fuzzy inference system 53 Figure 3.38 Combination of stereo vision system, fuzzy inference system with flight control algorithm 54 Figure 3.39 Code architecture runs on Nvidia Jetson TX1 55 Figure 3.40 Outdoor testing of fuzzy decision-making system. 56 Figure 3.41 Variation of depth values of fuzzy decision-making system. 56 Figure 3.42 Variation of velocities of fuzzy decision-making system. 57 Figure 4.1 Final assembled quadcopter 58 Figure 4.2 Quadcopter flying on open field 59 Figure 4.3 Barometer measured height vs EKF estimated height 60 Figure 4.4 Barometer measured climb rate vs EKF estimated climb rate 60 Figure 4.5 GPS measured Latitude position vs EKF estimated Latitude position 61 Figure 4.6 GPS measured Longitude position vs EKF estimated 62 Figure 4.7 Zoom version of GPS measured Longitude position vs EKF estimated Longitude position between data point 6600 and 67000 62 Figure 4.8 GPS measured Latitude velocity vs EKF estimated Latitude velocity 63 viii Figure 4.9 GPS measured Longitude velocity vs EKF estimated 63 Figure 4.10 Actual height variation vs desired height variation 64 Figure 4.11 Height error variation 65 Figure 4.12 Actual position variation vs desired position variation in 65 Figure 4.13 Position error variation in Latitude direction 66 Figure 4.14 Actual position variation vs desired position variation in Longitude direction 66 Figure 4.15 Position error variation in Longitude direction 67 Figure 4.16 Overall hardware setup including stereo camera. 68 Figure 4.17 Quadcopter fly over tree 69 Figure 4.18 Quadcopter fly to a wall. 70 ix LIST OF ABBREVIATIONS Abbreviation Description BM Block matching CPU Central processing unit EKF Extended Kalman Filter ESC Electronic speed control FIS Fuzzy Inference system GCS Ground control station GPU Graphical processing unit GPS Global Positioning system GUI Graphical user interface HAL Hardware Abstraction Layer MEMS Microelectromechanical systems NCC Normalized cross correlation OpenCV Open source Computer Vision PWM Pulse Width Modulation RTOS Real-time operating system SGBM Semi-Global block matching SAD Sum of absolute difference SSD Sum of square difference UAV Unmanned Aerial Vehicle UDP User Datagram Protocol USB Universal Serial Bus x LIST OF APPENDICES Appendix Description Appendix - A MATLAB Symbolic Implementation of Extended Kalman Filter. Appendix - B C++ Implementation of Extended Kalman Filter. Appendix - C C++ Implementation of Stereo Vision System. Appendix - D C++ Implementation of Fuzzy Decision-making System. Appendix - E Extended Kalman filter ground test data. Appendix - F Extended Kalman filter flight test data. Appendix - G Obstacle avoidance system ground test data. Appendix - H Video evidence of system test. Note: Appendices are available on the provided compact disk (CD).