ICS: Incremental Constrained Smoothing for State Estimation

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.

Authors

• Paloma Sodhi
• Sanjiban Choudhury
• Joshua G. Mangelson
• Michael Kaess

Keywords

• Optimization
• Smoothing methods
• Time measurement
• Integrated circuits
• Simultaneous localization and mapping
• Time Step
• Matrix Factorization
• Inequality Constraints
• Equality Constraints
• Current Step
• Environmental Knowledge
• Interior Point Method
• Unconstrained Optimization
• Hard Constraints
• Environment In The Form
• Object Contact
• Previous Step
• Nonlinear Programming
• Nonlinear Least Squares
• Taylor Expansion
• Maximum A Posteriori
• KKT Conditions
• Constraint Violation
• Upper Triangular
• Factor Graph
• Adaptive Step Size
• Normal Equations
• Smooth Problems
• Constraint Factor
• Odometry
• Loop Closure
• Collision-free Trajectory
• Contact Sensors