Dissertations and Theses 5-2018 Homography-Based State Estimation for Autonomous Exploration in Unknown Environments Andres S. Chavez Armijos Follow this and additional works at: https://commons.erau.edu/edt Part of the Aerospace Engineering Commons Scholarly Commons Citation Armijos, Andres S. Chavez, "Homography-Based State Estimation for Autonomous Exploration in Unknown Environments" (2018). Dissertations and Theses. 396. https://commons.erau.edu/edt/396 This Thesis - Open Access is brought to you for free and open access by Scholarly Commons. It has been accepted for inclusion in Dissertations and Theses by an authorized administrator of Scholarly Commons. For more information, please contact [email protected]. HOMOGRAPHY-BASED STATE ESTIMATION FOR AUTONOMOUS EXPLORATION IN UNKNOWN ENVIRONMENTS A Thesis Submitted to the Faculty of Embry-Riddle Aeronautical University by Andres S. Chavez Armijos In Partial Fulfillment of the Requirements for the Degree of Master of Science in Aerospace Engineering May 2018 Embry-Riddle Aeronautical University Daytona Beach, Florida iii ACKNOWLEDGMENTS I would like to dedicate this thesis to all the people that have supported my work in a directly or indirectly manner, but I would also like to thank all of those who have meant something during my graduate studies. Firstly, I would like to thank my committee members for their support on this thesis, but I would also like to give a special mention to Dr. Richard Prazenica, who has not only been a good mentor and advisor, but has also been a truly good friend that I will always remember. Secondly, I would like to thank my friend Nick Myhre who has aided me through the process of debugging, mathematical reasoning, and flight testing behind this thesis. He has also been a friend that I can truly rely on for the next chapters of my life. I would also like to thank my friends and roommates Erick, Sergio, and Carlos who were an important part of the technical and emotional support that I received during my thesis. Lastly, but not least, as Jane Horward once said ”Call it a clan, call it a network, call it a tribe, call it a family. Whatever you call it, whoever you are, you need one”, this thesis would not have been possible without the support of my family. Therefore, this work is specially dedicated to all of the members of my family with special mention to Alexandra, Jose, Mauricio, and Rosa because this work is the reflection of all the effort that they made so I could follow my dreams. They have always been and always will be my motivation, inspiration, and my source of strength for every piece of my life. Gracias familia por la oportunidad que me dieron. Los amo. iv TABLE OF CONTENTS Page LIST OF TABLES ::::::::::::::::::::::::::::::::: vi LIST OF FIGURES :::::::::::::::::::::::::::::::: vii SYMBOLS :::::::::::::::::::::::::::::::::::: x ABBREVIATIONS :::::::::::::::::::::::::::::::: xi ABSTRACT :::::::::::::::::::::::::::::::::::: xii 1 Introduction :::::::::::::::::::::::::::::::::: 1 1.1 Problem Statement :::::::::::::::::::::::::::: 1 1.2 Purpose ::::::::::::::::::::::::::::::::: 3 2 Literature Review ::::::::::::::::::::::::::::::: 5 2.1 Dead Reckoning (DR) :::::::::::::::::::::::::: 6 2.1.1 Inertial Navigation System (INS) :::::::::::::::: 8 2.2 Signal-Based Navigation ::::::::::::::::::::::::: 8 2.2.1 Radio Navigation :::::::::::::::::::::::: 9 2.2.2 Satellite Navigation ::::::::::::::::::::::: 13 2.3 Vision-Based Navigation (VBN) ::::::::::::::::::::: 14 2.3.1 Algorithms with Model-Based Approach (MBA) :::::::: 15 2.3.2 Algorithms with Appearance-Based Approach (ABA) :::::: 19 3 Projective Geometry :::::::::::::::::::::::::::::: 26 3.1 Geometric Primitives ::::::::::::::::::::::::::: 26 3.2 Homography Relationship :::::::::::::::::::::::: 28 3.2.1 Direct Linear Transformation (DLT) ::::::::::::::: 29 3.2.2 Nonlinear Homography ::::::::::::::::::::: 31 3.2.3 Robust Homography ::::::::::::::::::::::: 33 3.2.4 Homography Decomposition ::::::::::::::::::: 35 4 Simulation ::::::::::::::::::::::::::::::::::: 37 4.1 Quadcopter Dynamic Model ::::::::::::::::::::::: 37 4.2 Sensor Modeling ::::::::::::::::::::::::::::: 41 4.2.1 Inertial Measurements :::::::::::::::::::::: 41 4.2.2 Camera Model :::::::::::::::::::::::::: 43 4.3 Simulation Architecture ::::::::::::::::::::::::: 44 5 Vision-Based State Estimation ::::::::::::::::::::::::: 47 v Page 5.1 Homography-Based Visual Odometry :::::::::::::::::: 48 5.1.1 Previous Normal Comparison :::::::::::::::::: 50 5.1.2 Multiple Homography Comparison ::::::::::::::: 51 5.1.3 Pose Reconstruction ::::::::::::::::::::::: 53 5.2 Homography-Based Kalman Filter :::::::::::::::::::: 56 5.2.1 Process Model :::::::::::::::::::::::::: 57 5.2.2 Measurement Model ::::::::::::::::::::::: 60 5.2.3 Observability Analysis :::::::::::::::::::::: 62 5.2.4 Extended Kalman Filter Model ::::::::::::::::: 65 5.3 Simulation Results :::::::::::::::::::::::::::: 67 5.3.1 Visual Odometry Simulation Results ::::::::::::::: 68 5.3.2 Numerical Observability Analysis Results :::::::::::: 72 5.3.3 Homography-Based Kalman Filter Results :::::::::::: 75 5.3.4 Monte Carlo Simulation ::::::::::::::::::::: 78 6 Experimental Results :::::::::::::::::::::::::::::: 81 6.1 Hardware Description :::::::::::::::::::::::::: 81 6.1.1 Pixhawk Autopilot :::::::::::::::::::::::: 83 6.1.2 3DR GPS Module :::::::::::::::::::::::: 84 6.1.3 GoPro Hero 3+ :::::::::::::::::::::::::: 84 6.2 Vision Processing Algorithm ::::::::::::::::::::::: 85 6.2.1 Camera Calibration ::::::::::::::::::::::: 86 6.2.2 Image Feature Detection ::::::::::::::::::::: 88 6.3 Measurement Integration ::::::::::::::::::::::::: 91 6.3.1 Median Filtering (MF) :::::::::::::::::::::: 92 6.3.2 Innovation Filtering (IF) ::::::::::::::::::::: 95 6.4 Results :::::::::::::::::::::::::::::::::: 98 6.4.1 Test 1 :::::::::::::::::::::::::::::: 100 6.4.2 Test 2 :::::::::::::::::::::::::::::: 103 7 Hybrid State Estimation :::::::::::::::::::::::::::: 106 7.1 Landmark-Based Measurement ::::::::::::::::::::: 107 7.2 Navigation Filter Architecture :::::::::::::::::::::: 108 7.3 Experimental Results ::::::::::::::::::::::::::: 111 8 Conclusion ::::::::::::::::::::::::::::::::::: 115 9 Recommendations ::::::::::::::::::::::::::::::: 118 References ::::::::::::::::::::::::::::::::::::: 120 vi LIST OF TABLES Table Page 5.1 Simulation sensor parameters :::::::::::::::::::::::: 68 5.2 RMS error computation for Visual Odometry sample test :::::::::: 71 5.3 Initial Error Covariance for Observability Analysis ::::::::::::: 73 5.4 Singular Values for the Observability Gramian Matrix :::::::::::: 74 5.5 Error Covariance Rate of Change in the Standard Deviation ::::::::: 76 5.6 RMS Error Homography EKF Simulation :::::::::::::::::: 77 6.1 3DR Iris Quadcopter specifications ::::::::::::::::::::: 82 6.2 RMS Error Test 1 :::::::::::::::::::::::::::::: 101 6.3 RMS Error Test 2 :::::::::::::::::::::::::::::: 104 7.1 RMS Error Hybrid Navigation Comparison Test ::::::::::::::: 113 vii LIST OF FIGURES Figure Page 2.1 Dead Reckoning Drift Accumulation Process (Groves, 2008) :::::::: 7 2.2 Strapdown Inertial Navigation System Diagram (Titterton & Weston, 2004) : 9 2.3 Radio Direction Triangulation (Indutiveload, 2008) ::::::::::::: 10 2.4 Hyperbolic Ranging Position Localization (Polturi, 2007) :::::::::: 11 2.5 Frequency shift of a moving object (Lehtinen, 2001) ::::::::::::: 12 2.6 GNSS Positioning Description :::::::::::::::::::::::: 14 2.7 Vision bearing system (Groves, 2013) :::::::::::::::::::: 16 2.8 Map feature comparison (Pink, 2008) :::::::::::::::::::: 18 2.9 Vision-based Terrain-Referenced Navigation (D. Lee et al., 2012) :::::: 19 2.10 Two-View Projective Epipolar Geometry Representation (Ma et al., 2004) : 21 2.11 Optical Flow Differential and Energy-Based Potential Field Representation (Roberts et al., 2009) ::::::::::::::::::::::::::::: 22 2.12 Relationship Between Process Information Matrix and Covariance Matrix in SLAM EKF (Frese & Hirzinger, 2001) ::::::::::::::::::: 25 3.1 3D Point Projection Representation (Stolfi, 2009) :::::::::::::: 26 3.2 Planar Homography Relationship (Ma et al., 2004) ::::::::::::: 28 4.1 Representation of Applied Forces and Torques on the Vehicle (Mahony et al., 2012) ::::::::::::::::::::::::::::::::::::: 38 4.2 Pinhole Camera Model (Hartley & Zisserman, 2003) :::::::::::: 43 4.3 Simulation Architecture Diagram :::::::::::::::::::::: 45 4.4 Simulation ROS Topic Tree Graph :::::::::::::::::::::: 45 4.5 Gazebo Environment Visualization :::::::::::::::::::::: 46 5.1 Two-Frame Homography Over a Planar Set of Points :::::::::::: 49 5.2 Three-Frame Homography Over a Planar Set of Points ::::::::::: 52 5.3 EKF Architecture Diagram :::::::::::::::::::::::::: 57 viii Figure Page 5.4 Quadcopter Simulated 3D Trajectory for Visual Odometry ::::::::: 69 5.5 Visual Odometry Solution Position Comparison ::::::::::::::: 69 5.6 Visual Odometry Solution Attitude Comparison ::::::::::::::: 70 5.7 3D Trajectory for Numerical Observability Analysis ::::::::::::: 72 5.8 Simulated Camera Sample Tracking ::::::::::::::::::::: 75 5.9 Quadcopter Simulated 3D Trajectory for Homography-Based EKF ::::: 78 5.10 Homography-Based EKF Simulation Results :::::::::::::::: 79 5.11 Monte Carlo Simulation Results ::::::::::::::::::::::: 80 6.1 3DR Iris Quadcopter Testbed :::::::::::::::::::::::: 82 6.2 Pixhawk Autopilot :::::::::::::::::::::::::::::: 83 6.3 3DR GPS External Module ::::::::::::::::::::::::: 84 6.4 GoPro HERO 3+ ::::::::::::::::::::::::::::::: 85 6.5 Vision Processing Workflow Diagram ::::::::::::::::::::