Real time Localization of Mobile Robotic Platform via fusion of Inertial and Visual Navigation System

Azhar Mahmood
School of Electrical Engineering &Computer Science National University of Sciences and Technology Islamabad, Pakistan 11mscsamahmood@seecs.edu.pk

Asim Baig, Qaisar Ahsan

Department of Computer Engineering Muhammad Ali Jinnah University Islamabad, Pakistan

Abstract Inertial Navigation System (INS) is one of the most important component of a mobile robotic platform, be it ground or air based. It is used to localize the mobile robotic platform in the real world and identify its location in terms of latitudes and longitudes or other related coordinate systems. Highly accurate and precise INS is quite expensive and is therefore not suitable for more general purpose applications. It is, therefore, a standard approach in mobile robotics to use a low grade commercial INS coupled with another navigation device to provide a more accurate triangulation. Generally, INS and Global Positioning System (GPS) are integrated using Kalman Filters to provide accurate localization information about the mobile robots. Although, in certain scenarios, the mobile robot is not able to acquire a GPS fix for long durations of time especially when navigating in indoor environments or in areas with inadequate GPS satellite coverage. In such cases, an additional source of location fix is required. This paper describes an accurate and stable data fusion filter which integrates the position of a mobile robot from a Visual Navigation System (VNS) with the position from an INS to accurately localize the robot in absence of GPS data. This research proposes a seven error states model and uses it in Kalman Filter for data fusion. The filter is tuned and tested using dynamic and static data from INS and VNS. Simulation and experimentation results show that the seven error states model based Kalman Filter provides a good balance between accuracy, robustness and processing efficiency for a real time implementation. Experiments also show that in absence of GPS data only a couple of fixes from the VNS are sufficient to quickly correct the position of the mobile robotic platform and three fixes at different times are sufficient for velocity correction of INS. Keywords- Kalman Filter, Localization, Mobile Robot, INS, Visual Navigation, Data Fusion

operates continuously and provides a high bandwidth output at low noise. It provides effective attitude, angular rates and acceleration measurements as well as position and velocity. However, its navigational accuracy degrades with time as the noise and the biases on its inertial instrument outputs are integrated through the navigation equations [1]. For INS the accelerometer errors grow quadratically and gyro errors grow cubically with time [2]. This means that low cost INS can only provide precise readings for a couple of seconds [3]. On the other hand, GPS provides an accurate fix with a very small error window covering any part of the world during day and night [4]. However, GPS cannot provide attitude data, also at times GPS fix cannot be acquired for a long period of time and it can be jammed quite easily. In fact, the knowledge required to jam GPS is becoming public and the hardware to achieve this is basic at best [5]. Similarly, VNS can provide very accurate fix with accuracy of within a couple of meters but it cannot provide the fix continuously. The fix from VNS is acquired only at certain locations with enough features to produce a good result. Therefore, it is a general practice to use a fusion approach to develop an integrated navigation system. An integrated navigation system utilizes the complementary characteristics of different navigational devices to improve the overall precision of navigation. In addition, the system becomes more reliable and robust to noise due to the existence of the redundant readings which allows for detection and removal of bad sensor data. This is the reason why a lot of research is being done in this area. In [6] the authors conducted an observability analysis to show that GPS/INS integration can constrain positional drift. Hong et. al. in [7] perform a more indepth analysis of a loosely coupled INS/GPS integrated system to show that in addition to constraining the positional drift and under certain maneuvers it is possible recover the attitudes of the UAV as well. In [8] the author outlines a seven state Kalman filter based integration of INS and GPS for micro air vehicle navigation. In [9] the authors develop a non-linear INS error model using a Gaussian Process Regression. The proposed system acquires the data during GPS availability to estimate the error between GPS and INS. In the absence of GPS the error estimates are used as input to a Kalman filter to correct for the INS drift. Recently, INS and VNS integration has attracted a lot of interest. In [10] the authors use 10 state Kalman filter to integrate measurements from INS and VNS systems. They implemented their system on Yamaha Rmax unmanned

I.

INTRODUCTION

Navigation is one of the fundamental aspects of an autonomous mobile robotic platform e.g. UAV (for simplicity sake term UAV will be used throughout this paper instead of the more tedious mobile robotic platform). The aim of a good navigation system is to identify the location of a UAV in a know coordinate system and to make sure that it follows the predetermined path closely to the destination. A wide range of devices are used to achieve this task including Inertial Navigation System (INS) or Inertial Measurement Unit (IMU), Global Positioning System (GPS) and Visual Navigation System (VNS). Each of the above mentioned systems have their own strengths and weaknesses. For Example, INS

helicopter. In [11] Dusha and Majias integrate GPS and VNS to estimate not only the positional drift but also the attitude error of the vehicle. In this paper the authors present a seven state Kalman filter model to integrate the measurements of INS and VNS. The proposed integrated system is tested using simulated data acquired by conducting flights on small single propeller planes. The test results clearly show that without the availability of a GPS a small number of fixes from the VNS can correct the positional error in the overall system. In fact, the positional error was reduced from around 300m to just above 50m with a single set of fixes from the VNS. The paper is structured as follows. Section 2 outlines the notations used in the paper as well briefly defining the workings of the VNS used in this proposed system. Which is followed by the outlines the fusion approach and the relative equations are derived. Section 3 outlines the experimental setup with section 4 providing the results and analysis. Finally, the conclusions are provided in section 5. II. THE PROPOSED APPROACH

loss in precision. In fact, our experimentation shows once the system starts working the non-diagonal nature of the P and Qk actually compensates for the roll and pitch errors and there is almost no loss in precision . The seven error state vector used in the proposed system is given below. =

(1)

These states are for yaw, north, east and down velocities, Latitude, Longitude and height. This reduction in error states also changes the error dynamics matrices. The velocity and distance error dynamics matrices remain the same but the attitude error dynamics matrix is changed significantly. These matrices are provided below.
vN vD v 2( sin L + E tan L) R R R vE vE 1 Fvv = 2 sin L + tan L (v N tan L + v D ) 2 cos L + R R R 2v N vE 2( cos L ) 0 R R

(2)

Following notations are used throughout this paper to define and derive the integrated system. x Ven L G f F K P Qd R Error state vector Attitude error vector Vehicle velocity w.r.t. Earth (North, East, Down) Geodetic Longitude Angular rate vector Design matrix Force vector Dynamics matrix Kalman gain matrix Covariance matrix of state vector Spectral density matrix Covariance matrix of measurement error vec.

vE v E (2 cos L + ) 2 R cos L v vE Fvr = 2(v N cos L v D sin L) + N 2 R cos L 2v E sin L

0 0 0

2 (v E tan L v D v N ) R vE 2 (v N tan L + v D ) R 1 2 2 (v N + v E ) 2 R 1
2

(3)

0 v tan L Frr = E R cos L 0

0 0 0

R vE 2 R cos L 0 vN
2

(4)

One of the most important component of the whole integrated system is the VNS. A standard VNS for a UAV works by acquiring an image from a downward looking camera and matching it with a geo-referenced satellite image of the same areas thus allowing the UAV to correctly localize itself. For a ground based mobile robotic platform the image is generally acquired from a forward looking camera and is compared with a reference image with pre-marked location of known landmarks. In either case, the output of the VNS is a positional fix of the vehicle within a know frame of reference. One of the most important factors in achieving the best fix from a VNS is the proper acquisition of the image. Ideally, the image should be acquired when the vehicle is in stable state i.e. having zero roll or pitch, this is generally not possible. Therefore it is generally attempted to acquire the image when the vehicle is at minimum possible roll and pitch. Generally, a Kalman filter with 9 states is used for integration of INS and VNS [9], but by considering the above restriction on the imaging we can ignore the two states for roll and pitch, as they are very small when image is acquired. This reduces the Kalman filter states to 7 without any significant

= 0

1 R Frv = 0 0

0 1 R cos L 0

The overall error dynamics matrix is given as

0 0 1

(5)

(6) (7)

n in Fev n F = f Fvv o Frv

Fer Fvr Frr

(8)

Similarly the forcing matrix u with white noise is given as

u = [f x

b b b f y f z ibx iby ibz ]T

(9)

The measurement from the VNS is considered to be the input and since the measurements from the INS are latitude, longitude and height. The straightforward formulation of the measurement equation can be written as: = (11)

2 2 2 2 2 2 = ( 2 )

Finally, the noise covariance matrix is given to be (10)

actual error as soon as the system receives a couple of good fixes from the VNS. An important point to note is that the VNS fix only depends on the image acquired and the reference thus it is not influences in any way by the INS errors. In the same vain figure 3 shows the errors in terms of the actual and integrated north velocities and figure 4 shows the errors in terms of the actual and integrated east velocities. The figures show that it takes a lot more fixes from the VNS to converge to the actual velocity errors. In fact, it was observed that it takes the system upwards of 10 or more fixes from the VNS to reduce the error to within 0.2m/s. An interesting thing to note is that the difference between the actual and the integrated velocity errors isnt too big to begin with but it is further reduced as soon as the fixes from VNS start arriving. V. CONCLUSION

The height for VNS is acquired from an altimeter. Altimeter is an integral part of a VNS system and is always present as its readings are required to process the acquired image. The reduced set of equations given above increase the update speed and reduce the processing time. On the other hand, the number of states is high enough to provide a robust and precise system as the results and analysis section outlines. III. THE EXPERIMENTAL SETUP

The experimentation was conducted on a plane as opposed to a helicopter as in [10]. The VNS system used in the experimentatl setup was based on the work presented in [12]. The camera for VNS was attached to the belly of the plane and the INS was attached inside the plane exactly on top of the camera. The image from the camera and the INS readings were read into a laptop, synced and stored in real-time. Once the plane had landed correlation based matching was performed using a code written in C/C++. The INS and VNS readings were fused using a seven state Kalman filter code written in MATLAB. The whole data acquisition system was activated only once the plane reached over the top of test area to preserve the laptops battery life. It is important to note that the system works as loosely coupled open system, which means that the INS is not directly corrected and if no reading is available from the VNS the integrated output will follow the INS reading. IV. RESULTS AND ANALYSIS

In this paper we present a seven state Kalman filter based fusion approach to integrate INS and VNS for a precise and robust positional fix of any mobile robotic platform. The authors designed the input and error dynamics matrices for the proposed system along with the noise covariance matrix. The tests were conducted in a plane and the results show that the proposed system is not only robust and precise but it also converges very quickly. REFERENCES
J.A. Farrell. Aided Navigation: GPS with High Rate Sensors. McGraw Hill, 2008. [2] P. D. Groves. Principles of GNSS, Inertial, and Multi-Sensor Integrated Navigation Systems. Artech House, 2007. [3] S. Godha. Performance evaluation of low cost mems-based imu integrated with gps for land vehicle navigation application. Master's thesis, University of Calgary, 2006. [4] Kaplan, E. Understanding GPS principles and applications. Artech House., 1996 [5] P. D. Groves, R.. J. Handley, Optimizing the Integration of Terrain Referenced Navigation with INS and GPS. Published by Royal Institure of Navigation with permission from QinetiQ Ltd. 2004 [6] I. Rhee, M.F. Abdel-Hafez, and J.L.Speyer. Observability of an integrated GPS/INS during maneuvers. IEEE Transactions on Aerospace and Electronic Systems, 40(2), 2004. [7] S. Hong, M. H. Lee, H. Chun, S. Kwon, and J.L. Speyer. Observability of error states in GPS/INS integration. IEEE Transactions on Vehicular Technology, 54(2), 2005. [8] A. M. Malik, S. Riaz, Single Seven State Discrete Time Extended Kalman Filter for Micro Air Vehicle. World Congress of Engineering, 2010. [9] M. Atia, A. Noureldin, M. Korenberg, Enhanced Kalman Filter for RISS/GPS Integrated Navigation using Gaussian Process Regression, Proceedings of the 2012 International Technical Meeting of The Institute of Navigation, 2012. [10] G. Conte, P. Doherty, A Visual Navigation System for UAS based on Geo-referenced Imagery. Conference on Unmanned Aerial Vehicle in Geometics. 2011. [11] D. Dusha, L. Mejias, Attitude observability of a loosely coupled GPS/Visual Odometry Integrated Navigation Filter. Australasian Conference on Robotics and Automation (ACRA 2010), 2010. [12] A. Baig, A. Mahmood, M. A. Chaudhry, Local normalized cross correlation for geo-registration. 9th International Bhurban Conference on Applied Sciences (IBCAST) , 2011 [1]

The following graphs show the results of the test performed on the above mentioned experimental setup. As noted above, the proposed system works in a loosely coupled formation. Since such a system works by compensating for the error of the INS when a data fix is available, it makes sense to evaluate a system like this by comparing the actual error of the INS with the estimate provided by the Kalman filter when a data fix is available. Figure 1 shows the errors of latitude in terms of the actual and integrated reading during a portion of the flight. It was made sure that the portion of the flight selected in these graphs was where the VNS provided a set of fixes and that it was not the start or the end portion of the flight. It is interesting to note that the error converges to the actual very quickly whenever the reading from the VNS is available. In fact, the error difference becomes almost negligible within three to four readings from the VNS. Similarly, Figure 2 shows the error of longitude in terms of the actual and integrated readings. The figure clearly shows that the integrated results converge to the

Figure 1. Latitude Error between actual and estimated values

Figure 2. Longitude Error between actual and estimated values

Figure 3. North Velocity Error between actual and estimated values

Figure 4. East Velocity Error between actual and estimated values