Improving Underwater State Estimation Through Novel Initialization Techniques
With this project, we investigate how initialization techniques can be used for improving the state estimation, specifically for autonomous underwater vehicles.
Why does initialization matter?
Answering this question requires a fundamental understanding of how we performed state estimation to begin with. In this project, we pose the state estimation task as a smoothing problem, as opposed to a filtering based one.
Smoothing, in the context of state estimation in robotics, is the technique that uses all observations over a time period in order to estimate the state over those time periods. Formally, it can be detoted as maximizing the posterior distribution P(X|Z), where X = {X0, X1, …, Xt} are the states from time 0 to time t, and Z = {Z1, …, Zt} are the observations made from time 0 to time t. This is the problem of maximum a-posteriori (MAP).
Solving MAP requires careful design of how we represent our state and variables. For this project, we use factor graphs and use the library Georgia Tech Smoothing and Mapping (GTSAM) for implementing our factor graph.
The maximization of the posterior distribution is posed as an optimization problem!
So now we get to why initialization matters: finding the MAP is a nonlinear optimization problem! The objective function is nonlinear, and almost always non-convex. This arises from the fact that the measurement models that are used are nonlinear (i.e. pinhole camera models, odometry models). So, if we have a non-convex and nonlinear function that we are trying to minimize, we employ nonlinear programming (NLP) techniques that use analytical gradient-based methods.
For each of these NLP methods, we have to start from some point on the objective function that we are minimizing: that is our initialization. Rather than starting from a random initial point, we say: start from an initial point that is possibly close to a good local minima of the nonlinear function.
Our idea for initialization is that given a new observation or measurement from a sensor, we run a predict-correct step of an Invariant Kalman Filter and use that as the initialized state before running the MAP optimization!
This allows us to leverage a novel filtering based approach as a highly informative prior on a nonlinear optimization process, and we show that we can match the performance of baseline methods!
Primer on MAP
The maximization of the posterior distribution is posed as an optimization problem!
By using Bayes’ Rule, we can compute what the unnormalized posterior distribution: P(X|Z) ∝ P(Z|X)P(X).
The prior, P(X), is typically known, and its inclusion in the maximization of the posterior makes the problem a maximum aposteriori (MAP).
The likelihood, P(Z|X), represents the likelihood of observing observation Z given the state X. Through a measurement model, we can quantify this probability: the actual observation is compared to the output of the measurement model and an error is computed. By minimizing the negative of this error, we maximize the posterior — because they are proportional! (remember: maximizing X is the same as minimizing -X).
By maximizing the combination of the prior and the likelihood, we then achieve a result that gives us the maximum of the posterior distribution!
