This section discusses a further restriction on the types of problems to be consid- ered, and relates to the issue of model consistency. A factor graph could be con- structed with any random variables and likelihood potentials between them, but
Figure 3-13: Illustration of inconsistent modeling, where assumed model as gray bell curve is a poor probability density estimate of actual value shown by Dirac spike (black arrow) on the right. The bottom figure shows a different multi-modal likelihood which does support probability mass around the true value which is considered a consistent likelihood model for the true event.
is not representative of any true sequence of events. Alternatively, we could model a system of variables with extremely wide uncertainties, resulting in a correct but uninformative posterior distribution.
By means of an example, consider a two pose system where a robot drives 10 units in a one dimensional world. We instantiate a prior position unary factor to the first pose and pairwise odometry measurement likelihood factor to the second pose. The odometry factor can be made inconsistent by specifying an exceedingly unlikely situation. For example, the odometry measurement uncertainty could be taken as normally distributed with mean of 5 units traveled, with only 1 unit standard deviation (σ); resulting in the actual 10 units traveled being extremely unlikely (5σ). We would consider this situation inconsistent, and illustrated graph- ically in the top portion of Fig. 3-13. We contrast a consistent multi-modal likeli- hood belief in the bottom portion of Fig. 3-13.
In the case of loop closures, we note that the odometry drift estimates must be representative of the actual drift accumulated. We regard a loop closure potential between two variables, which by odometry are unlikely to be in close proximity as an inconsistent model. Our inference algorithm will use user supplied mea- surement likelihood functions to propose regions of the state space that must be explored. The posterior solution is found by stacking all likelihood proposals and
resolving the regions of consensus.
3.7
Conclusion
Most, if not all, SLAM systems to date perform inference using squared error cost functions aggregated into a optimization objective function. The squared cost im- plies a normally distributed measurement likelihood model, which we have shown as too restrictive in many situations. The difficult cases arise from ambiguous mea- surements. For example, a normally distributed measurement model biases data association uncertainty, or makes poor use of information when underlying un- certainty is non-Gaussian. Instead, we choose to use kernel density estimation for approximating all marginal beliefs in the system, and naturally encapsulates multi-hypothesis and nonparametric type beliefs that an associated inference pro- cedure can exploit for more representative posterior estimation. This approach allows us to perform inference on a static factor graph structure, without trying to modify the graph structure before or after inference.
This chapter introduces a number of nonparametric measurement likelihood functions by focusing on the importance of defining an on-manifold measurement residual function combined with a consistent probabilistic error distribution. The measurement models presented in this chapter provide a solution for many of the problems faced by current SLAM type measurement factors in use today. We show several measurement likelihood potentials to be used in any combination for con- structing nonparametric navigation-type factor graphs. To our knowledge, none of the current SLAM solvers are able utilize semantic information in a factor graph during inference without modifying the actual structure of the graph. Our ap- proach is able to perform incremental inference over semantically labeled and the ever changing factor graphs, and recover the most dominant modes according to all available measurement information.
The measurement likelihood functions presented are by no means a complete list. In particular, we discussed how inertial odometry factors (inertial sensor preintegrals) play a special role in real-time, high-bandwidth state estimation. We illustrated how longer running computation times required for robust multi- sensor data fusion that can be combined with a smaller duplicate factor graph inference task for a combined fast localization and robust mapping system. Chap- ter 4 investigates the inertial odometry factor in detail and Chapter 5 discusses the
Multi-modal iSAM incremental inference algorithm to user specified nonparamet-
Chapter 4
Inertial Odometry
In Chapter 3, we described new parametric and nonparametric measurement like- lihood models for navigation type factor graphs. This chapter derives and details the new continuous time and second order inertial odometry measurement factor for high-bandwidth, real-time aspects of the navigation-type factor graph descrip- tions. Robust multi-sensor fusion through nonparametric inference with Multi-
modal iSAMis discussed in Chapter 5.
4.1
Introduction
Inertial sensors are a corner-stone of high bandwidth navigation systems. They offer a means to ”black box” dead reckoning, but suffer difficulties in double or triple integration of sensor errors. Inertial sensors capture vehicle dynamics and other effects such as gravity at high rate, but also include measurement errors. For example, a gyroscope bias offset results in accrued orientation error, which in turn results in incorrect gravity compensation of acceleration measurements. Integration of misaligned gravity quickly results in significant positioning error.
Over the past two decades, simultaneous localization and mapping [135] has been a major area of navigation-related research. Sparse factor graph methods have been developed to allow a new perspective on navigation and localization, bringing into question whether Kalman filtering [56] is still the best way of infer- ring inertial sensor calibration parameters.
Fig. 4-1 illustrates how summarized inertial odometry constraints can be used in navigation type factor graphs. The figure shows how leg kinematics, inertial odometry and visual sightings of opportunistic features as well as loop closures can interact in a centralized framework. We note that a pure inertial odometry
Figure 4-1: Factor graph showing pure inertial odometry constraints, aided by for- ward kinematics from the legs and monocular visual feature sightings through the head mounted camera (associated with Section 8.3).
constraint can also be used to predict real-time state estimates based on a robust factor graph inference result, as shown. Future motions can also be optimized by enforcing vehicle model dynamics towards a desired goal.
However, when we try to use inertial measurements as odometry likelihoods in a factor graph formulation, we find that compensation of sensor errors (dynamic calibration) is not trivial. We further find that use of pure inertial sensory infor- mation in factor graph based navigation systems is limited, due to the lack of a clear inertial odometry measurement residual or likelihood model. We identify the need for a computationally tractable inertial odometry measurement likelihood model for easy integration of multiple sensors in factor graph based methods.
Fig. 4-2 conceptually shows how a smooth trajectory is discretized into discrete poses, and how high-rate inertial information summarized into directbi
bj∆xterms.
Our approach follows from the work of Lupton et al. [143], who suggested raw inertial measurements are first integrated and offset compensation is only done later. These directly integrated inertial sensor values are called inertial preintegrals. We emphasize that allocating error to the inertial sensor bias terms is very dif- ferent from just allocating error to position or orientation states, sometimes also called ”bias”. By modeling native inertial sensor bias terms, we introduce a mech- anism to compensate position and orientation errors along the entire trajectory
Figure 4-2: Conceptual overview of using pure inertial odometry constraints be- tween world frame poses at time tiand tj. Any opportunistic constraint would aid
dynamic sensor calibration. Analytical description in Section 4.3.
according to their true sensor measurement errors. Note that by allocating error to inertial sensor biases, we effectively require reintegration of sensor error influ- ences across each affected odometry likelihood model.
In this chapter we improve on the preintegral method by condensing high-rate gyroscope and accelerometer measurements into lossless inertial preintegral terms with a continuous time and exponential parameterization residual model. We de- scribe the accumulation of a second set of values, referred to as inertial odometry compensation gradients, which are simultaneously accumulated at sensor rate. The compensation gradients can then later be used for retroactive estimation of sensor bias terms by means of a residual function, defined in eq. (4.21). We also discuss propagation of the covariance matrix of the inertial odometry terms. The inertial odometry process is illustrated with examples in Section 4.4. Chapter 8 further explores navigation examples with a hand-held inertial with monocular camera localization solution; and concept demonstration on a Boston Dynamics Atlas humanoid robot.
Our work presents a theoretical development of inertial preintegral bias com- pensation models and best relates to work by [143], [103] and [137]. Our work extends that of [103] and [143] by presenting a continuous time analytical deriva- tion of a Taylor expansion of the sensor error terms manifold inside each odom- etry likelihood function. To the best of our knowledge, an analytical continuous time gradient model derivation together with higher order Taylor expansion of
the residual function for retroactive calibration is not yet available, and is the main contribution of this chapter.
This chapter is structured as follows: We present a motivation for our work alongside contributions from other authors. Our development begins with fa- miliar interpose odometry constraints and is then extended into the retroactive inertial sensor calibration model. We discuss computation of the required com- pensation gradients. Finally, validation experiments from synthetic and specific purpose recorded data is discussed at the end of the chapter.