1. ROBOTILNICA Intelligent Systems and Robotics, Skopje, R.Macedonia
Vision Aided Inertial Navigation using Single Video Camera Measurements
Vasko Sazdovski
note
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.
Prof. Raffaello D'Andrea from ETH Zurich at ZURICH.MINDS 2012 showing the early quadrotor designs within its research group…
MeCam quadrotor
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“.

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 axis gyro
Three axis accelerometer
Atomic Inertial Measurement Unit (IMU) for Sparkfun Inc.
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.
Augmented
state vector
Vehicle
Map points
Unknown environment
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
Nonlinear model
Vehicle position
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
where is 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
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.
Single video camera
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 Vicon T Series motion capture System
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?
QUESTIONS ARE WELCOMED