You are on page 1of 5

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

Section 3 outlines the experimental setup with section 4 providing the results and analysis. Which is followed by the outlines the fusion approach and the relative equations are derived.e. the conclusions are provided in section 5.t.helicopter. One of the most important factors in achieving the best fix from a VNS is the proper acquisition of the image. Longitude and height. In either case. 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. In this paper the authors present a seven state Kalman filter model to integrate the measurements of INS and VNS. a Kalman filter with 9 states is used for integration of INS and VNS [9]. In [11] Dusha and Majias integrate GPS and VNS to estimate not only the positional drift but also the attitude error of the vehicle. Earth (North. having zero roll or pitch. The paper is structured as follows. 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. 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 ๐‘…๐‘…๐‘๐‘๐‘๐‘๐‘๐‘ 2 ๐ฟ๐ฟ 0๏ฟฝ ๏ฃน 0๏ฃบ ๏ฃบ 0๏ฃบ ๏ฃบ โˆ’ 1๏ฃบ ๏ฃบ ๏ฃป ๐œ๐œ ๐ธ๐ธ (5) 0 ๐‘‰๐‘‰๐ธ๐ธ ๐‘ก๐‘ก๐‘ก๐‘ก๐‘ก๐‘ก๐ฟ๐ฟ ๐‘…๐‘… 2 ๏ฟฝ (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 . East. THE PROPOSED APPROACH loss in precision. Section 2 outlines the notations used in the paper as well briefly defining the workings of the VNS used in this proposed system. 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. The proposed integrated system is tested using simulated data acquired by conducting flights on small single propeller planes. The seven error state vector used in the proposed system is given below. the output of the VNS is a positional fix of the vehicle within a know frame of reference. In fact. this is generally not possible. ฮดx ฮต Ven L ฯ‰ G f F K P Qd R Error state vector Attitude error vector Vehicle velocity w. Latitude. ๐›ฟ๐›ฟ๐‘ฅ๐‘ฅ = ๏ฟฝ๐›ฟ๐›ฟ๐œ“๐œ“ ๐›ฟ๐›ฟ๐œ๐œ ฮ ๐›ฟ๐›ฟ๐œ๐œ ๐ธ๐ธ ๐›ฟ๐›ฟ๐œ๐œ ๐ท๐ท ๐›ฟ๐›ฟ๐ฟ๐ฟ ๐›ฟ๐›ฟ๐œ†๐œ† ๐›ฟ๐›ฟโ„Ž ๏ฟฝ ๐‘‡๐‘‡ (1) These states are for yaw. These matrices are provided below. The velocity and distance error dynamics matrices remain the same but the attitude error dynamics matrix is changed significantly. the image should be acquired when the vehicle is in stable state i. ๏ฃฎ 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 ๏ฃฏ ๏ฃฏ 2โ„ฆv 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. 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. Generally. east and down velocities. In fact. Ideally. Therefore it is generally attempted to acquire the image when the vehicle is at minimum possible roll and pitch. 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. the positional error was reduced from around 300m to just above 50m with a single set of fixes from the VNS.r. 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 . II. This reduction in error states also changes the error dynamics matrices. north. 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. Finally.

Speyer. D. A. 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. Single Seven State Discrete Time Extended Kalman Filter for Micro Air Vehicle. 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. M. Enhanced Kalman Filter for RISS/GPS Integrated Navigation using Gaussian Process Regression. R.L. [7] S. Aided Navigation: GPS with High Rate Sensors. Lee. REFERENCES J. The whole data acquisition system was activated only once the plane reached over the top of test area to preserve the laptops battery life. Mahmood. Malik. 40(2). it was observed that it takes the system upwards of 10 or more fixes from the VNS to reduce the error to within 0. Conference on Unmanned Aerial Vehicle in Geometics. 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. Artech House. Principles of GNSS. The figures show that it takes a lot more fixes from the VNS to converge to the actual velocity errors. and J. V. S. [12] A. H. 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. THE EXPERIMENTAL SETUP The experimentation was conducted on a plane as opposed to a helicopter as in [10]. 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. University of Calgary. Chun. M. The INS and VNS readings were fused using a seven state Kalman filter code written in MATLAB. 2004. CONCLUSION The height for VNS is acquired from an altimeter. 2012. H. Australasian Conference on Robotics and Automation (ACRA 2010). [9] M. 2010. 2005. It is important to note that the system works as loosely coupled open system. Handley. An interesting thing to note is that the difference between the actual and the integrated velocity errors isnโ€™t too big to begin with but it is further reduced as soon as the fixes from VNS start arriving. 2008. Understanding GPS principles and applications. Chaudhry. Figure 2 shows the error of longitude in terms of the actual and integrated readings. In fact.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. [11] D. longitude and height. Riaz. Noureldin. Since such a system works by compensating for the error of the INS when a data fix is available. Inertial. It is interesting to note that the error converges to the actual very quickly whenever the reading from the VNS is available. Published by Royal Institure of Navigation with permission from QinetiQ Ltd. Local normalized cross correlation for geo-registration. 2006. 1996 [5] P.L. Godha. Doherty. Observability of error states in GPS/INS integration. and Multi-Sensor Integrated Navigation Systems. A. Figure 1 shows the errors of latitude in terms of the actual and integrated reading during a portion of the flight. IEEE Transactions on Vehicular Technology. Dusha. The authors designed the input and error dynamics matrices for the proposed system along with the noise covariance matrix. Attitude observability of a loosely โ€ coupled GPS/Visual Odometry Integrated Navigation Filter. [3] S. M. 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. 2004 [6] I. As noted above. 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. Hong. [2] P. Groves. M. The figure clearly shows that the integrated results converge to the . the proposed system works in a loosely coupled formation. The reduced set of equations given above increase the update speed and reduce the processing time. Abdel-Hafez. Groves. P. 2010. Korenberg. 54(2). Altimeter is an integral part of a VNS system and is always present as its readings are required to process the acquired image. synced and stored in real-time.. Optimizing the Integration of Terrain Referenced Navigation with INS and GPS. S. A. Rhee. E. [4] Kaplan. the number of states is high enough to provide a robust and precise system as the results and analysis section outlines. Performance evaluation of low cost mems-based imu integrated with gps for land vehicle navigation application. [8] A. On the other hand.. 2011.A. Proceedings of the 2012 International Technical Meeting of The Institute of Navigation. Mejias. L. D. A Visual Navigation System for UAS based on Geo-referenced Imagery. In fact. Atia. Master's thesis. IV. 2007. Similarly. 2011 [1] The following graphs show the results of the test performed on the above mentioned experimental setup. The image from the camera and the INS readings were read into a laptop. The straightforward formulation of the measurement equation can be written as: ๐ฟ๐ฟ๐‘‰๐‘‰๐‘‰๐‘‰๐‘‰๐‘‰ โˆ’ ๐ฟ๐ฟ๐ผ๐ผ๐‘‰๐‘‰๐‘‰๐‘‰ ๐›ฟ๐›ฟ๐‘Ž๐‘Ž = ๏ฟฝ ๐œ†๐œ†๐‘‰๐‘‰๐‘‰๐‘‰๐‘‰๐‘‰ โˆ’ ๐œ†๐œ†๐ผ๐ผ๐‘‰๐‘‰๐‘‰๐‘‰ ๏ฟฝ ๐ป๐ป๐‘‰๐‘‰๐‘‰๐‘‰๐‘‰๐‘‰ โˆ’ ๐ป๐ป๐ผ๐ผ๐‘‰๐‘‰๐‘‰๐‘‰ (11) 2 2 2 2 2 2 ๐‘„๐‘„๐‘‘๐‘‘ = ๐‘‘๐‘‘๐‘‘๐‘‘๐‘ก๐‘ก๐‘‘๐‘‘(๏ฟฝ๐œŽ๐œŽ๐œ”๐œ”๐œ”๐œ” ๐œŽ๐œŽ๐‘ก๐‘ก๐‘ฅ๐‘ฅ ๐œŽ๐œŽ๐‘ก๐‘ก๐‘Ž๐‘Ž ๐œŽ๐œŽ๐‘ก๐‘ก๐œ”๐œ” ๐œŽ๐œŽ๐ฟ๐ฟ ๐œŽ๐œŽ๐œ†๐œ†2 ๐œŽ๐œŽโ„Ž ๏ฟฝ) Finally. and J. [10] G. Artech House. Observability of an integrated GPS/INS during maneuvers. J. Baig. 9th International Bhurban Conference on Applied Sciences (IBCAST) . 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. Farrell. Kwon.Speyer. M. McGraw Hill. Conte. III.F. The VNS system used in the experimentatl setup was based on the work presented in [12]. IEEE Transactions on Aerospace and Electronic Systems.2m/s. the error difference becomes almost negligible within three to four readings from the VNS. 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. World Congress of Engineering. Once the plane had landed correlation based matching was performed using a code written in C/C++.

Latitude Error between actual and estimated values Figure 2.Figure 1. Longitude Error between actual and estimated values .

East Velocity Error between actual and estimated values .Figure 3. North Velocity Error between actual and estimated values Figure 4.