Skip to main content
SpringerLink
Log in

A Constrained Kalman Filter for Rigid Body Systems with Frictional Contact

  • Conference paper
  • First Online:
Algorithmic Foundations of Robotics XIII (WAFR 2018)

Part of the book series: Springer Proceedings in Advanced Robotics ((SPAR,volume 14))

Included in the following conference series:

Abstract

Contact interactions are central to robot manipulation and locomotion behaviors. State estimation techniques that explicitly capture the dynamics of contact offer the potential to reduce estimation errors from unplanned contact events and improve closed-loop control performance. This is particularly true in highly dynamic situations where common simplifications like no-slip or quasi-static sliding are violated. Incorporating contact constraints requires care to address the numerical challenges associated with discontinuous dynamics, which make straightforward application of derivative-based techniques such as the Extended Kalman Filter impossible. In this paper, we derive an approximate maximum a posteriori estimator that can handle rigid body contact by explicitly imposing contact constraints in the observation update. We compare the performance of this estimator to an existing state-of-the-art Unscented Kalman Filter designed for estimation through contact and demonstrate the scalability of the approach by estimating the state of a 20-DOF bipedal robot in realtime.

This is a preview of subscription content, log in via an institution to check access.

Access this chapter

Log in via an institution
Chapter
USD 29.95
Price excludes VAT (USA)
  • Available as PDF
  • Read on any device
  • Instant download
  • Own it forever
eBook
USD 169.00
Price excludes VAT (USA)
  • Available as EPUB and PDF
  • Read on any device
  • Instant download
  • Own it forever
Softcover Book
USD 219.99
Price excludes VAT (USA)
  • Compact, lightweight edition
  • Dispatched in 3 to 5 business days
  • Free shipping worldwide - see info
Hardcover Book
USD 219.99
Price excludes VAT (USA)
  • Durable hardcover edition
  • Dispatched in 3 to 5 business days
  • Free shipping worldwide - see info

Tax calculation will be finalised at checkout

Purchases are for personal use only

Institutional subscriptions

References

  1. Wahba, G.: A least squares estimate of satellite attitude. SIAM Rev. 7, 409–409 (1965)

    Article  Google Scholar 

  2. Simon, D.: Kalman filtering with state constraints: a survey of linear and nonlinear algorithms. IET Control Theory Appl. 4, 1303–1318 (2010)

    Article  MathSciNet  Google Scholar 

  3. Stewart, D.E., Trinkle, J.C.: An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and coulomb friction. Int. J. Numer. Meth. Eng. 39(15), 2673–2691 (1996)

    Article  MathSciNet  Google Scholar 

  4. Lowrey, K., Dao, J., Todorov, E.: Real-time state estimation with whole-body multi-contact dynamics: a modified UKF approach. In: Proceedings of the IEEE-RAS International Conference on Humanoid Robots (2016)

    Google Scholar 

  5. Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Yokoi, K., Hirukawa, H.: Biped walking pattern generation by a simple three-dimensional inverted pendulum model. Adv. Robot. 17, 131–147 (2003)

    Article  Google Scholar 

  6. Stephens, B.J.: State estimation for force-controlled humanoid balance using simple models in the presence of modeling error. In: 2011 IEEE International Conference on Robotics and Automation, pp. 3994–3999, May 2011

    Google Scholar 

  7. Wang, S., Shi, Y., Wang, X., Jiang, Z., Yu, B.: State estimation for quadrupedal using linear inverted pendulum model. In: 2017 2nd International Conference on Advanced Robotics and Mechatronics (ICARM), pp. 13–18, August 2017

    Google Scholar 

  8. Bloesch, M., Hutter, M., Hoepflinger, M.A., Leutenegger, S., Gehring, C., Remy, C.D., Siegwart, R.: State estimation for legged robots - consistent fusion of leg kinematics and IMU (2012)

    Google Scholar 

  9. Rotella, N., Bloesch, M., Righetti, L., Schaal, S.: State estimation for a humanoid robot. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 952–958, September 2014

    Google Scholar 

  10. Xinjilefu, X., Feng, S., Atkeson, C.G.: Dynamic state estimation using quadratic programming. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 989–994, September 2014

    Google Scholar 

  11. Kuindersma, S., Deits, R., Fallon, M., Valenzuela, A., Dai, H., Permenter, F., Koolen, T., Marion, P., Tedrake, R.: Optimization-based locomotion planning, estimation, and control design for Atlas. Auton. Robots 40(3), 429–455 (2016)

    Article  Google Scholar 

  12. Hartley, R., Mangelson, J., Gan, L., Jadidi, M.G., Walls, J.M., Eustice, R.M., Grizzle, J.W.: Legged robot state-estimation through combined forward kinematic and preintegrated contact factors. arXiv:1712.05873 [cs], December 2017

  13. Singh, S.P.N., Waldron, K.J.: A hybrid motion model for aiding state estimation in dynamic quadrupedal locomotion. In: Proceedings of the 2007 IEEE International Conference on Robotics and Automation, pp. 4337–4342, April 2007

    Google Scholar 

  14. Nori, F., Kuppuswamy, N., Traversaro, S.: Simultaneous state and dynamics estimation in articulated structures. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3380–3386, September 2015

    Google Scholar 

  15. Koval, M.C., Pollard, N.S., Srinivasa, S.S.: Pose estimation for planar contact manipulation with manifold particle filters. Int. J. Robot. Res. 34, 922–945 (2015)

    Article  Google Scholar 

  16. Koval, M.C., Klingensmith, M., Srinivasa, S.S., Pollard, N.S., Kaess, M.: The manifold particle filter for state estimation on high-dimensional implicit manifolds, pp. 4673–4680. IEEE, May 2017

    Google Scholar 

  17. Lowrey, K., Kolev, S., Tassa, Y., Erez, T., Todorov, E.: Physically-consistent sensor fusion in contact-rich behaviors. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1656–1662, September 2014

    Google Scholar 

  18. Baraff, D.: Fast contact force computation for nonpenetrating rigid bodies, pp. 23–34. ACM Press (1994)

    Google Scholar 

  19. Lemke, C.E., Howson Jr., J.T.: Equilibrium points of bimatrix games. J. Soc. Ind. Appl. Math. 12(2)

    Google Scholar 

  20. Ferris, M.C., Munson, T.S.: Interfaces to PATH 3.0: design, implementation and usage. Comput. Optim. Appl. 12, 207–227 (1999)

    Article  MathSciNet  Google Scholar 

  21. Anitescu, M., Potra, F.A.: Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems. Nonlinear Dyn. 14(3), 231–247 (1997)

    Article  MathSciNet  Google Scholar 

  22. Bullet Collision Detection & Physics Library. http://www.bulletphysics.org/

  23. Smith, R.: Open dynamics engine (2007). http://www.ode.org/

  24. DART: Dynamic animation and robotics toolkit. https://dartsim.github.io/

  25. Gurobi Optimization, Inc.: Gurobi optimizer reference manual. Technical report (2014)

    Google Scholar 

  26. Tedrake, R., The Drake Development Team: Drake: a planning, control, and analysis toolbox for nonlinear dynamical systems (2016)

    Google Scholar 

  27. Yu, K.-T., Bauza, M., Fazeli, N., Rodriguez, A.: More than a million ways to be pushed: a high-fidelity experimental dataset of planar pushing. arXiv:1604.04038 [cs], April 2016

  28. Fazeli, N., Zapolsky, S., Drumwright, E., Rodriguez, A.: Fundamental limitations in performance and interpretability of common planar rigid-body contact models. arXiv:1710.04979 [cs], October 2017

  29. Fazeli, N., Kolbert, R., Tedrake, R., Rodriguez, A.: Parameter and contact force estimation of planar rigid-bodies undergoing frictional contact. Int. J. Robot. Res. 36, 1437–1454 (2017)

    Article  Google Scholar 

Download references

Acknowledgements

This work was supported by a Draper Internal Research and Development grant and the National Science Foundation (Grant Number IIS-1657186). Any opinion, findings, and conclusions or recommendations expressed in this material are those of the authors and do not necessarily reflect the views of the National Science Foundation.

Author information

Authors and Affiliations

  1. Harvard University, Cambridge, MA, 02138, USA

    Patrick Varin & Scott Kuindersma

Authors
  1. Patrick Varin
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Scott Kuindersma
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Patrick Varin .

Editor information

Editors and Affiliations

  1. Departamento de Sistemas Digitales, Instituto Tecnológico Autónomo de México, México, Mexico

    Marco Morales

  2. Department of Computer Science, University of New Mexico, Albuquerque, NM, USA

    Lydia Tapia

  3. Universidad Politécnica de Yucatán, Yucatán, Mexico

    Gildardo Sánchez-Ante

  4. Georgia Tech, Atlanta, GA, USA

    Seth Hutchinson

Rights and permissions

Reprints and permissions

Copyright information

© 2020 Springer Nature Switzerland AG

About this paper

Check for updates. Verify currency and authenticity via CrossMark

Cite this paper

Varin, P., Kuindersma, S. (2020). A Constrained Kalman Filter for Rigid Body Systems with Frictional Contact. In: Morales, M., Tapia, L., Sánchez-Ante, G., Hutchinson, S. (eds) Algorithmic Foundations of Robotics XIII. WAFR 2018. Springer Proceedings in Advanced Robotics, vol 14. Springer, Cham. https://doi.org/10.1007/978-3-030-44051-0_28

Download citation

Publish with us

Policies and ethics

Search

Navigation