Justificación

We discuss odometry measurements as a time separated form of kinematic re- lations. Odometry generally involves solving an ordinary differential equation (ODE) from velocity or acceleration measurements. Interpose odometry mea- surements are treated similarly to any other measurement model, as suggested in eq. (3.5), while using the kinematic relation similar to eq. (3.16). The difference in ODE-type kinematics is that the interpose measurementi

j∆ ˜X is created by in-

tegrating platform velocities or accelerations.

Conditional: ODE-type Kinematic (Odometry)

Noisy body relative velocity measurements biv, such as vehicle wheel odometry_˜

Figure 3-3: Factor Group II: Shaded factor graph illustration from Fig. 3-1 for high- bandwidth and real-time inertial navigation aspects of a humanoid robot state es- timation process. A more elaborate example is shown in Fig. 3-6.

bi

bj∆ ˜Xsince the last pose

w biX, bi bjR = tj Y t=ti Z t _b ω× dτ b_bi_j∆ ˜Xpos = Z tj ti bi b R b_{v dτ,˜ (3.18)} where bi

b R represents the vehicle body rotation in the previous pose

bi_{(·) frame.}

Rotation could be computed from angular rotation ratesb_{ω. This expression uses}

Lie algebra_b

ω× ∈ so(n) to mechanize the rotation manifold.

However, in preparation for nonparametric measurement model discussion, consider a situation with robot wheel slip, which we will model with non- Gaussian, multi-modal or nonparametric distribution.

Conditional and Prior: Body Relative Velocity (Doppler Velocity Log)

Underwater autonomous vehicles (AUVs) generally use a Doppler velocity log combined with an inertial measurement unit to perform sea floor relative navi- gation [56]. We show a separate factor graph illustration in Fig. 3-4 for using body relative velocities as a navigation aid.

Figure 3-4: Example of a navigation-type factor graph for autonomous underwa- ter vehicle navigation. Green nodes represent vehicle poses relative to the world frame, separated body frame velocities shown as V1, V2. Integrated Doppler mea-

surements between poses are shown in light red, as well as absolute velocity snap- shots as priors on body frame velocity states. This illustration also shows a land- mark feature of interest being estimated from nonparametric-type acoustic factors.

Fig. 3-4 illustrates an AUV near the sea floor, where an onboard inertial mea- surement unit, acoustic Doppler velocity log and acoustic ranging measurements are available. Inertial odometry factors are in blue along with integrated velocity measurements from a Doppler velocity log in light red. Mapping of an acoustic feature landmark measurement is also shown, and we note an example a nonpara- metric acoustic measurement is shown in Fig. 3-9.

The factor graph description shown in Fig. 3-4 is grounded by a marginal prior on the pose X0, which is based on the previous event leading up to the estimated

position of the vehicle at X0. The factor graph segment shown spans the time

between the marginal prior and real-time indicated on the right. We can think of this factor graph segment as a fixed lag smoothing framework, whereby the marginal prior is updated and moved forward simultaneous to new poses being introduced on the right, as real-time progresses. We use the inertial odometry factor to project the real-time state estimate from the last pose in the factor graph. We discuss the time scales of computation and inertial odometry factor in the next section.

Conditional: Real-time Inertial Odometry (Preintegration)

In this section, we provide an overview of using inertial odometry factors in con- cert with all other measurement likelihoods. Chapter 2 describes conventional methods for processing and fusing inertial sensory data into a navigation solu- tion, generally using Kalman filtering techniques. These methods have become a crucial component of all high-bandwidth, real-time state estimation systems to date [229], and note that factor graph style inclusion of inertial measurements will play an increasingly vital role in all future agile robots.

Sensor bias and scale factor errors result in significant dead reckoning drift when integrated without dynamics calibration and compensation. These errors are a main driver developing dynamical calibration and retroactive compensation models particularly for inertial measurement units (IMUs). Calibration is done in concert with other, possibly lower bandwidth, navigational information. In- ertial sensor measurement errors have the advantages of being well understood, strongly unimodal, and accurately modeled with a Gauss-Markov process. In- ertial sensor noise input terms are normally distributed, resulting in highly uni- modal relative odometry trajectory segments. The strong unimodal nature of iner- tial measurements has an additional advantage in multi-modal inference schemes, which search for consensus among various sensor data sources. Strong unimodal conditional likelihoods may still transmit multi-modal belief, but quickly help cull out low likelihood modes from other ambiguous measurements, resulting in a re- duced computational burden.

The intricate manifold on which the gyroscope and accelerometer bias terms influence an long interpose residual function is not yet been well understood. Con- sider long interpose integration times with rapidly varying orientation of the plat- form. Knowing how much to correct the velocity, position, and orientation dimen- sions for a change in each of the sensor error terms becomes intricate.

A major contribution of this thesis is the analytic derivation of an efficiently computable residual function for pure inertial odometry with retroactive sensor bias compensation, shown as the blue factors in Fig. 3-3. This residual function approx- imates the sensor error manifold with a higher order Taylor expansion. The newly developed inertial odometry measurement likelihood model can then be used as a generic measurement factor in any combination with other factors in a navigation- type factor graph. Chapter 4 is dedicated entirely to the analysis of the new inertial odometry-type factor (measurement preintegration) and defer the reader there for an in depth discussion.

Figure 3-5: 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.

measured trajectory—using inertial navigation-type equations in preintegration form—between two poses Xi, Xi+1 to calculate a relative and noisy preintegral

measurement bi

bi+1∆ ˜X. Through the process described in Chapter 4, we define a

prediction function to exploit the sensor error parameter manifolds,

bi

bi+1∆ ˆX = (f ◦ fB) (

w

bXi,wbXi+1) . (3.19)

The combination of preintegral measurement and prediction produces a residual function between the two poses

δ (w_bXi,wbXi+1;wg) =b_bi_i+1∆ ˜X − (f ◦ fB) (wb Xi,wbXi+1) . (3.20)

The inertial odometry residual is a linear map embedding similar to the kine- matic relations in eq. (3.18). The residual function contains all high bandwidth platform motion dynamics, as well as measured gravityw_{gand sensor biases.}

We also note that the nonparametric factor graph solution may result in multi- modal estimates of sensor calibration terms, even though the inertial odometry is itself generally combined with a multivariate normal error distribution.

Any number of inertial odometry factors can be assembled from separate IMU measurements in the same factor graph for centralized inference. We specifically note the possibility of placing IMUs on each of the robot’s feet, hands or head for

better joint estimation for the extremities. The ensemble of IMUs is then individ- ually calibrated from the aggregate of all sensor information.

Inertial odometry factors give statistical equivalence between all navigation measurements in the centralized factor graph inference scheme. The strong uni- modal Gaussian nature of inertial measurements is a great boon for finding con- sensus amongst other nonparametric, multi-modal sensor measurements.

The inertial odometry measurement likelihoods, as shown in Fig. 3-3, play two vital parts in the overall data fusion process. The inertial odometry factor intro- duces a dynamic calibration-type factor, where sensor error terms are retroactively estimated in concert with all other available sensory information in the data fu- sion inference process, while supplying a strong unimodal odometry measure- ment likelihood.

Before addressing the second advantage of real-time state estimation, we note the principle of feed-forward sensor calibration. The topic is more thoroughly dis- cussed in Section 4.5, but we briefly mention the feed forward aspect here. The residual function in eq. (3.20) is approximated relative to the true rotation rates and accelerations the platform is subjected to. By slowly moving the bias estimates out of the factor graph and into the sensor processing, we can asymptotically improve the approximated residual function accuracy. We assume feed forward corrections are being applied to the real-time state estimate discussion in the next section.

Prior: Intrapose Boundary Marginal (Real-time)

The second major advantage of inertial odometry factors is the ability to obtain a real-time state estimate. We can use the most current inertial odometry preintegral to make real-time state estimate predictions relative to one of the recent poses. We indicate the real-time dead reckoning with an inertial odometry factor and arrow in Fig. 3-3. Using precisely the same inertial odometry residual function, we can produce an algebraically equivalent state estimate solution to a conventional inertial navigation system:

bi b δx = δ bi_x 0,i, 0;wg
, wbx = wbix ⊕ bi b δx, (3.21) wherebi_x

0,i keeps only the bias estimate from pose Xi and zeros all other terms.

The residual function performs gravity and bias compensation to produce bi

b δx

relative to pose w

biXi. The ⊕ operation stands off, or projects, the current inertial

odometry derived trajectory segment to the real-time state estimate. We select the reference pose location from a marginal belief estimatew

bix = argmaxx

_w

biXi| Z.

are the preintegral equivalent to previous recursive inertial navigation schemes. We emphasize that the real-time prediction does not need to consist of a purely inertial odometry factor.

Figure 3-6: Timescales illustration of combining real-time state estimate with ro- bust inference. The top part of the figure illustrates robust inference on a large factor graph, with the most recent inference result for poses Xk, k = 1, 2, ..., n on

the left. A smaller duplicate solution beyond pose Xn, grounded by two boundary

marginals, is shown on the bottom part. The real-time state estimate is achieved with most recent inertial odometry factor. This arrangement allows for both slower robust computation times and fast hard real-time state estimation. Information between the SLAM server and local inference task travels across a limited commu- nication layer.

Fig. 3-6 illustrates the real-time state estimation process further. We can choose to project the current inertial odometry preintegration residual from a recently inferred SLAM pose. We can also make a local duplicate copy, shown in the bottom part of the figure, which fuses the most recent sensor data. A previous suggested approach, concurrent filtering and smoothing [117], advocates a serial mechanism

where a smaller local factor graph solves variables first and then pass factors and variables over to the larger solution.

In contrast, we suggest duplicating a portion of the factor graph in two copies and solving simultaneously, while ”grounding” the smaller graph against the in- trapose boundary marginal prior from the larger solution. We call the marginal belief on the intermediate pose the intrapose boundary marginal. A smaller si- multaneous factor graph can immediately duplicate all new factors and variables beyond the intrapose boundary marginal, even though the larger solution will still be computing on the previous factor graph snapshot. The replicated small factor graph structure is solved much faster and in parallel to the larger factor graph. The combination of large robust factor graph, small local duplicate factor graph, and inertial odometry residual function allows great flexibility and longer computation time, while maintaining real-time state estimation capability. This duplication pro- cess between local shorter and larger long term factor graphs was recently shown to also occur in biological systems: Research in memory mechanisms shows that mammals simultaneously create both long and short term memories [124], and do not transfer short term to long term memory as many researchers had thought.

The larger factor graph takes longer to compute, but shares the most recent inferred pose marginals to the smaller local factor graph solution as new marginals become available. Once a new marginal prior is available from the larger solution, we insert a new boundary condition on the local duplicate. The smaller local factor graph is then reduced in size and the process continues. Note the current inertial odometry preintegral is grounded against the most stable pose in recent history to give a real-time state estimate.