ICS: Incremental Constrained Smoothing for State Estimation

May 2020

ICS: Incremental Constrained Smoothing for State Estimation

Authors:

Paloma Sodhi, Sanjiban Choudhury, Joshua G. Mangelson, and Michael Kaess
Conference Paper
Proceedings of (ICRA) International Conference on Robotics and Automation

Abstract:

A robot operating in the world constantly receives information about its environment in the form of new measurements at every time step. Smoothing-based estimation methods seek to optimize for the most likely robot state estimate using all measurements up till the current time step. Existing methods solve for this smoothing objective efficiently by framing the problem as that of incremental unconstrained optimization. However, in many cases observed measurements and knowledge of the environment is better modeled as hard constraints derived from real-world physics or dynamics. A key challenge is that the new optimality conditions introduced by the hard constraints break the matrix structure needed for incremental factorization in these incremental optimization methods. Our key insight is that if we leverage primal-dual methods, we can recover a matrix structure amenable to incremental factorization. We propose a framework ICS that combines a primal-dual method like the Augmented Lagrangian with an incremental Gauss Newton approach that reuses previously computed matrix factorizations. We evaluate ICS on a set of simulated and real-world problems involving equality constraints like object contact and inequality constraints like collision avoidance.
@conference{Sodhi-2020-125350,
author = {Paloma Sodhi And Sanjiban Choudhury And Joshua G. Mangelson And Michael Kaess},
title = {ICS: Incremental Constrained Smoothing for State Estimation},
booktitle = {Proceedings of (ICRA) International Conference on Robotics and Automation},
year = {2020},
month = {May},
pages = {279 - 285},
}
Copyright notice: This material is presented to ensure timely dissemination of scholarly and technical work. Copyright and all rights therein are retained by authors or by other copyright holders. All persons copying this information are expected to adhere to the terms and constraints invoked by each author's copyright. These works may not be reposted without the explicit permission of the copyright holder.