Vision Aided Inertial Navigation using Single Video Camera Measurements

1. Vision Aided Inertial Navigation using Single Video Camera Measurements Vasko Sazdovski Faculty of Electrical Engineering, “Goce Delcev” University, Shtip, R.Macedonia Pilot Training Centre, ELMAK Elbit Systems of Macedonia, Airport Petrovec, Skopje, R.Macedonia ROBOTILNICA Intelligent Systems and Robotics, Skopje, R.Macedonia

2. OUTLINE OF THE PRESENTATION • INTRODUCTION • THEORY OF SIMULTANEOUS LOCALIZATION AND MAPPING (SLAM). INERTIAL NAVIGATION AND AUGMENTED STATE VECTOR. • VISION SENSORS • INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION ALGORITHM • INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING • CONCLUSIONS • FUTURE WORK POSSIBILITIES

3. INTRODUCTION Future UAV’s have a need of high levels of autonomy and independence. They are going to become small, very agile and highly maneuverable. • MeCamquadrotor • 14 sensors including side object detectors that enable "perfect and safe hovering”; • Voice control; • Autonomous person following; • Two Morpho autopilots and a video stabilization system; • Autonomous panoramas; • Real-time streaming to mobile devices; • $50 MSRP, potential availability "by the beginning of 2014“. Prof. Raffaello D'Andrea from ETH Zurich at ZURICH.MINDS 2012 showing the early quadrotor designs within its research group…

4. INTRODUCTION • Integrated navigation system design requires selection of: • set of a sensors and • computation power • that provides reliable and accurate navigation parameters (position, velocity and attitude) with high update rates and bandwidth in small and cost effective manner. Navigator SL Cloud Cap Technology USA Modern INS/GPS Navigation System for UAV’s Size: 131 x 57 x 19 mm  Weight: 80 grams Piccolo autopilot used on UAV for collecting meteorological or map data over long distances. Cloud Cap Technology USA.

5. INTRODUCTION • Integrated navigation system design requires selection of: • set of a sensors and • computation power • that provides reliable and accurate navigation parameters (position, velocity and attitude) with high update rates and bandwidth in small and cost effective manner. Always Innovating Cortex-A9 SoC Module which can run from 1.0GHz to 1.5GHz 1GB RAM, SD card, 2.4GHz/5GHz Wi-Fi and Bluetooth connectivity. MeCam quadrotor

6. INTRODUCTION Many of today’s operational navigation systems rely on inertial sensor measurements. Single axisgyro Atomic Inertial Measurement Unit (IMU) for Sparkfun Inc. Three axis accelerometer Inertial navigation is diverging slowly from the real measurements with time.

7. INTRODUCTION Additional sources of aiding information to the navigation parameters ( non interactive aiding information) Ching-Fang Lin “Modern Navigation Guidance and Control Processing” Prentice Hall Inc. 1991

8. INTRODUCTION Additional sources of aiding information to the navigation parameters (interactive aiding information) Vision sensors are must nowadays

9. THEORY OF SIMULTANEOUS LOCALIZATION AND MAPPING (SLAM). INERTIAL NAVIGATION AND AUGMENTED STATE VECTOR. Unknown environment Vehicle state vector Augmented state vector Map points Simultaneous Localization and Mapping (SLAM) uses relative measurements (range and bearing) from the vehicle with respect to the environment to build a map whilst simultaneously using the generated map computes vehicle position.

10. THEORY OF SIMULTANEOUS LOCALIZATION AND MAPPING (SLAM). INERTIAL NAVIGATION AND AUGMENTED STATE VECTOR. Repeated relative measurements of a map point (circular movement) Mapping of an unknown map point Localization using the map point provide valuable information’s of the spatial relationship of the map point and the vehicle

11. VISION SENSORS Single video camera Stereo video camera Vehicle position Nonlinear model Observation models Map point Linear model Measurement noise

12. INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION ALGORITHM When SLAM is performed on aerial vehicles the vehicle state can be represented by Inertial Navigation mechanization equations which give position, velocity and attitude quaternion: As we proposed the approach is to augment the vehicle state with number of map point estimates of the map point: The augmented state vector containing the vehicle state and the map point estimates is denoted as: The augmented process model is of the form where is the control input containing the acceleration and gyro measurements , is the process noise.

13. INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION ALGORITHM The equation for calculation of the map point estimates written in component form is whereis the measurement noise. The method for augmenting the map point estimates and building the covariance matrix is as follows. The new map point estimate is added to the augmented state vector and a new row and column are added to the covariance matrix: where and are Jacobians of function shown above with respect to the augmented state and observation , respectively These equations are repeated until we augment the state vector with “sufficient” number of map point estimates. Through simulations we realized that we need more then three map point estimates.

14. INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION ALGORITHM As the vehicle circles around the map point, repeated measurements are available from the stereo camera. Using these measurements we can start updating the augmented state vector by using each of the map point estimates at a time. Extended Kalman Filter (EKF) can be implemented for the update. The state covariance is propagated first where is the Jacobian of the augmented process model with respect to the augmented state vector. Next using the measurements (observations) we update the augmented state vector and its covariance: where the innovation vector and the Kalman gain are calculated as: is the Jacobian of the observation function with respect to the vehicle state and the map point estimate used in the update.

15. PERFORMANCES OF INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING Map point estimates True estimated and divergent vehicle trajectories Position errors

16. PERFORMANCES OF INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING Determinant of the covariance matrix of the first map point estimate The contour ellipsoid of the covariance matrix of the first map point estimate Map point estimates

17. PERFORMANCES OF INERTIAL NAVIGATION AIDED BY SIMULTANEOUS LOCALIZATION AND MAPPING Determinant of the covariance matrix of the vehicle position The relative distance between the first and second map point estimates The contour ellipsoid of the covariance matrix of the vehicle position

18. INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING Bearing Only Simultaneous Localization and Mapping (BOSLAM) is very attractive these days because it permits the use of single camera as sensor for measuring the bearing i.e. unit direction to the map points. The major drawback of this solution is the problem of map point initialization from a single measurement.

19. INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING Much of the research in BOSLAM is focused towards the problem of initialization of the map points from single camera measurements. In the literature two techniques are proposed to address the problem of map point initialization. The first technique involves delaying the map point initialization until a criterion is fulfilled and sufficient baseline is available from different vehicle positions. 1. Both techniques have their pros and cons The second technique tries to avoid the delay and initialize the map point from a single measurement The fact that after the first observation, the map point lies along on the line from the vehicle to the map point (the projection ray) is used 2. The approach presented before to augment the state vector not only with one map point estimate brings new parameterization of the map point in BOSLAM. The novelty comes from the usage of the certain number of map point estimates for update of the whole augmented state vector together with a combination of repeated measurements and motion in vicinity of the map point. This approach brings delayed initialization of the map points.

20. INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING Discussion on the observability of a system provides insights and understanding of the fundamental limits of the estimation processes. Since observability analysis can give the best achievable performance even before the system is built, it can be considered as tool for computer analysis of many complicated estimation processes. Stationary Vehicle Vehicle moving on a circular path

21. INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING Single video camera Inertial Navigation aided by BOSLAM as well as many other navigation problems are nonlinear and must be linearized (approximated) before applying the popular Kalman-like filtering algorithms. An EKF presents one such approximation. The EKF is very commonly used algorithm and, because of its simplicity, is very often chosen as the ”best” algorithm for implementation. Nonlinear observation model

22. INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING Inertial Navigation aided by BOSLAM as well as many other navigation problems are nonlinear and must be linearized (approximated) before applying the popular Kalman-like filtering algorithms. An EKF presents one such approximation. The EKF is very commonly used algorithm and, because of its simplicity, is very often chosen as the ”best” algorithm for implementation. • Because of numbers of significant problems that appear when implementing the EKF, other algorithms such as: • Iterated Extended Kalman Filter (IEKF) • Unscented Kalman Filter (UKF) • Unscented Particle Filter (UPF) • were implemented, tested and compared with the EKF algorithm.

23. PERFORMANCES OF INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING True estimated and divergent vehicle trajectories

24. PERFORMANCES OF INERTIAL NAVIGATION AIDED BY BEARING ONLY SIMULTANEOUS LOCALIZATION AND MAPPING Map point estimates

25. CONCLUSIONS • Aiding Inertial navigation (IN) by BOSLAM exhibits a high degree of nonlinearity and typically in these applications an EKF introduces large estimation errors. • Other algorithms such as UKF and UPF demonstrate best performance and appear to be efficient estimators for the concept of IN aided by BOSLAM. • The SLAM aided IN and BOSLAM aided IN sensor fusion algorithm present reliable solutions that provide aiding information to IN from vision sensors. These algorithms successfully integrate the inertial and vision sensors with no a priori knowledge of the environment.

26. QUADROTOR RESEARCH ARENA GRASP Lab, University of Pennsylvania, extensive use of motion capture systems Vicon T Series motion capture System

27. QUADROTOR RESEARCH ARENA Flying Machine arena , Institute for Dynamic Systems and Control ETH Zurich

28. QUADROTOR RESEARCH ARENA Localization and mapping done with two Hokuyo lidars and a servo motor. University of Pennsylvania CityFlyer project, Robotics and Intelligent Systems Lab, City College of New York, City University of New York. STARMAC Project in the Hybrid Systems Lab at UC Berkeley

29. QUADROTOR RESEARCH ARENA Robotics Group, CSAIL MIT: • Hokuyo laser range-finder (1), • stereo cameras (2), • monocular color camera (3), • laser-deflecting mirrors for altitude (4), • 1.6GHz Intel Atom-based flight computer (5), • Ascending Technologies internal processor and IMU (6).

30. QUADROTOR RESEARCH ARENA sFly European Project: Visual-Inertial SLAM for a small hexacopter (IROS 2012 video screenshot) Computer Vision Group at TUM Germany: Autonomous Camera-Based Navigation of a Low-Cost Quadrocopter

31. FUTURE WORK POSSIBILITIES In our future research we are working on developing novel approaches i.e. efficient estimators (sensor fusion algorithms) which will provide navigation performances and accuracy to a level close to the Differential GPS using only single video camera and inertial sensors. Our research work is very much focused on the practical experimentation and validation of the before defined problems. Quadrotor UAV is the chosen platform for the practical experiments. Quadrotor Flying Frog ROBOTILNICA Intelligent Systems and Robotics Custom modified quadrotor from KKMulticopter South Korea

32. FUTURE WORK POSSIBILITIES We are fascinated by insect based navigation. Flying insects especially inspire much of our research on autonomous navigation systems. We are very much interested in how the flying insects adept at maneuvering in complex, unconstrained, hostile and hazardous environments.

33. FUTURE WORK POSSIBILITIES Our approach with the practical experiments presents a low cost approach. Low cost Inertial Measurement Unit (IMU) from Sparkfun Electronics provides the angular rates (gyro) measurements and accelerometer measurements. These inertial sensors data together with the video from a smartphone camera are transferred over WiFi and/or 3G to a remote server/PC for processing. The video processing is performed on the remote PC using OpenCV libraries, together with the sensor fusion algorithms. The results are transferred back to the quadrotor for guidance and control of the vehicle for performing the required maneuvers (motion) and the vehicle task itself.

34. FUTURE WORK POSSIBILITIES As with the case of singe UAV, simply passing or flying by a map point with no manoeuvre will not help much for autonomous navigation for swarm of UAV’s. Coordination and cooperation between the UAV’s performing manoeuvres over same environment features (map points) are needed. This investigation is expected to address the issues when low accuracy vision sensors are used on UAV’s. In this scenario the UAV’s can take measurements not just of the environment but also of each other. It is expected that these measurements can accelerate the convergence of the novel algorithms under experimentations

35. THANK YOU FOR YOUR ATTENTION ? QUESTIONS ARE WELCOMED