A Normalized Measurement Vector Model for Enhancing Localization Performance of 6-DoF Bearing-only SLAM

  • Sukchang Yun
  • Yeonjo Kim
  • Byoungjin Lee
  • Sangkyung Sung
Regular Paper Robot and Applications


This study proposes a novel bearing measurement model in order to improve the localization performance of 6-DoF SLAM (six degree-of-freedom simultaneous localization and mapping). The main limitation of the existing measurement model for 6-DoF bearing-only SLAM using feature points was first analyzed, and a bearing measurement normalization method was then presented in order to cope with this limitation. The existing measurement model has a vulnerability in that the bearing measurement has different error levels depending on the feature point position, and thus the validity of the model is degraded as the feature point moves closer to the origin in the image. This problem can cause the innovation vector to become abnormally large in extended Kalman filter (EKF)- based navigation filters, resulting in divergence of the navigation filter. The normalization method proposed in this study makes the measurement error level constant. The new measurement model was derived using this method, and a bearing-only SLAM consisting of an inertial measurement unit (IMU) and bearing sensors was constructed in the EKF framework. The validity of this measurement model was analyzed by checking the innovation vectors in the navigation filter, and the performance of the system was verified through simulations by comparing with the navigation solution based on the existing measurement model.


Feature points model validation SLAM vision-based navigation 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. [1]
    H. Durrant-whyte and T. Bailey, “Simultaneous localization and mapping: part I,” IEEE Robot. Autom. Mag., vol. 13, no. 2, pp. 99–108, 2006. [click]CrossRefGoogle Scholar
  2. [2]
    M. Bosse, P. Newman, H. Leonard, and S. Teller, “Simultaneous localization and map building in large-scale cyclic environments using the atlas framework,” Int. J. Rob. Res., vol. 23, no. 12, pp. 1113–1139, Dec. 2004.CrossRefGoogle Scholar
  3. [3]
    J. A. Castellanos, J. M. Martinez, J. Neira, and J. D. Tardos, “Experiments in multisensor mobile robot localiztion and map building,” Proc. of IFAC Symposium on Intelligent Autonomous Vehicles, vol. 1, pp. 173–178, 1998.Google Scholar
  4. [4]
    K. S. Chong and L. Kleeman, “Feature-based mapping in real, large scale environments using an ultrasonic array,” Int. J. Rob. Res., vol. 18, no. 1, pp. 3–19, 1999.Google Scholar
  5. [5]
    A. J. Davison, Y. G. Cid, and N. Kita, “Real-time 3D SLAM with wide-angle vision,” Proc. of IFAC/EURON Symposium on Intelligent Autonomous Vehicles, 2004.Google Scholar
  6. [6]
    J. Folkesson and H. Christensen, “Graphical SLAM - a self-correcting map,” Proc. of IEEE International Conference on Robotics and Automation, vol 1., 2004, pp. 383–390, 2004.Google Scholar
  7. [7]
    J. E. Guivant and E. M. Nebot, “Optimization of the simultaneous localization and map-building algorithm for realtime implementation,” IEEE Trans. Robot. Autom., vol. 17, no. 3, pp. 242–257, 2001. [click]CrossRefGoogle Scholar
  8. [8]
    R. Eustice, H. Singh, J. Leonard, and M. R. Walter, “Visually mapping the RMS titanic: conservative covariance estimates for SLAM information filters,” Int. J. Robot. Res., vol. 25, no. 12, pp. 1223–1242, 2006.CrossRefGoogle Scholar
  9. [9]
    P. Newman and J. Leonard, “Pure range-only sub-sea SLAM,” Proc. of IEEE International Conference on Robotics and Automation, pp. 1921–1926, 2003. [click]Google Scholar
  10. [10]
    S. Sukkarieh and H. F. Durrant-whyte, “Towards the development of simultaneous localisation and map building for an unmanned air vehicle,” Proc. of International Conference of Field and Service Robotics, pp. 93–201, 2001.Google Scholar
  11. [11]
    J. Kim and S. Sukkarieh, “Airborne simultaneous localisation and map building,” Proc. of IEEE International Conference on Robotics and Automation, pp. 406–411, 2003. [click]Google Scholar
  12. [12]
    J. Kim and S. Sukkarieh, “Autonomous airborne navigation in unknown terrain environments,” IEEE Trans. Aerosp. Electron. Syst., vol. 40, no. 3, pp. 1031–1045, 2004.CrossRefGoogle Scholar
  13. [13]
    I.-K. Jung and S. Lacroix, “High resolution terrain mapping using low attitude aerial stereo imagery,” Proceedings Ninth IEEE International Conference on Computer Vision, pp. 0–5, 2003.Google Scholar
  14. [14]
    G. Klein and D. Murray, “Parallel tracking and mapping for small ARworkspaces,” Proc. of 6th IEEE ACM Int. Symp. Mix. Augment. Real., pp. 1–10, Nov. 2007.Google Scholar
  15. [15]
    G. Klein and D. Murray, “Parallel tracking and mapping on a camera phone,” Proc. of 8th IEEE Int. Symp. Mix. Augment. Real., pp. 83–86, Oct. 2009.Google Scholar
  16. [16]
    J. Ventura, C. Arth, G. Reitmayr, and D. Schmalstieg, “Global localization from monocular SLAM on a mobile phone.,” IEEE Trans. Vis. Comput. Graph., vol. 20, no. 4, pp. 531–539, Apr. 2014.CrossRefGoogle Scholar
  17. [17]
    M.W. Achtelik, S. Lynen, S. Weiss, L. Kneip, M. Chli, and R. Siegwart, “Visual-inertial SLAM for a small helicopter in large outdoor environments,” Proc. of IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2651–2652, 2012.Google Scholar
  18. [18]
    J. Engel, J. Sturm, and D. Cremers, “Camera-based navigation of a low-cost quadrocopter,” Proc. of IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2815–2821, 2012.Google Scholar
  19. [19]
    J. Engel and D. Cremers, “Accurate figure flying with a quadrocopter using onboard visual and inertial sensing,” Proc. of IEEE/RJS International Conference on Intelligent Robot Systems, 2012.Google Scholar
  20. [20]
    M. Li, B. H. Kim, and A. I. Mourikis, “Real-time motion tracking on a cellphone using inertial sensing and a rollingshutter camera,” Proc. of IEEE International Conference on Robotics and Automation, pp. 4712–4719, 2013.Google Scholar
  21. [21]
    G. Chowdhary, E. N. Johnson, D. Magree, A. Wu, and A. Shein, “GPS-denied indoor and outdoor monocular vision aided navigation and control of unmanned aircraft,” J. F. Robot., vol. 30, no. 3, pp. 415–438, May 2013.CrossRefGoogle Scholar
  22. [22]
    C.-L. Wang, T.-M. Wang, J.-H. Liang, Y.-C. Zhang, and Y. Zhou, “Bearing-only visual SLAM for small unmanned aerial vehicles in GPS-denied environments,” Int. J. Autom. Comput., vol. 10, no. 5, pp. 387–396, May 2013. [click]CrossRefGoogle Scholar
  23. [23]
    M. Bryson and S. Sukkarieh, “Observability analysis and active control for airborne SLAM,” IEEE Transactions on Aerospace and Electronic Systems, vol. 44, no. 1, pp. 261–280, 2008.CrossRefGoogle Scholar
  24. [24]
    S. Chun, D. H. Won, M.-B. Heo, and Y. J. Lee, “Performance analysis of an INS/SLAM integrated system with respect to the geometrical arrangement of multiple vision sensors,” Int. J. Control. Autom. Syst., vol. 10, no. 2, pp. 288–297, Apr. 2012. [click]CrossRefGoogle Scholar
  25. [25]
    D. H. Won, S. Chun, S. Sung, Y. J. Lee, J. Cho, J. Joo, and J. Park, “INS/vSLAM system using distributed particle filter,” Int. J. Control. Autom. Syst., vol. 8, no. 6, pp. 1232–1240, Jan. 2010. [click]CrossRefGoogle Scholar
  26. [26]
    A. E. Oguz and H. Temeltas, “On the consistency analyzing of A-SLAM for UAV navigating in GNSS denied environment,” Acta Polytech. Hungarica, vol. 10, no. 4, pp. 119–132, 2013.Google Scholar
  27. [27]
    M. W. M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-whyte, and M. Csorba, “A solution to the simultaneous localization and map building (SLAM) problem,” IEEE Trans. Robot. Autom., vol. 17, no. 3, pp. 229–241, 2001. [click]CrossRefGoogle Scholar
  28. [28]
    P. S. Maybeck, Stochastic Models, Estimaton and Control, Volume 1, Navtech Book, 1994.Google Scholar
  29. [29]
    W. Ding, Optimal Integration of GPS with Inertial Sensors: Modelling and Implementation, PhD Thesis, University of New South Wales, 2008.Google Scholar
  30. [30]
    “Camera Calibration Toolbox for Matlab.” [Online]. Available: htmls/parameters.html.Google Scholar
  31. [31]
    C. Tomasi and T. Kanade, “Detection and tracking of point features,” Technical Report CMU-CS-91-132, no. April, Carnegie Mellon University, 1991.Google Scholar
  32. [32]
    T. Bailey, “Constrained initialisation for bearing-only SLAM,” Proc, of IEEE International Conference on Robotics and Automation, pp. 1966–1971, 2003.Google Scholar
  33. [33]
    J. Civera, A. J. Davison, and J. Montiel, “Inverse depth parametrization for monocular SLAM,” IEEE Transactions on Robotics, vol. 24, no. 5, pp. 932–945, 2008. [click]CrossRefGoogle Scholar

Copyright information

© Institute of Control, Robotics and Systems and The Korean Institute of Electrical Engineers and Springer-Verlag GmbH Germany, part of Springer Nature 2018

Authors and Affiliations

  • Sukchang Yun
    • 1
  • Yeonjo Kim
    • 1
  • Byoungjin Lee
    • 1
  • Sangkyung Sung
    • 1
  1. 1.Department of Aerospace Information EngineeringKonkuk UniversitySeoulKorea

Personalised recommendations