FixedLagSmoother - GTSAM Docs

This API walkthrough introduces the wrapped classes FixedLagSmoother, FixedLagSmootherResult declared by FixedLagSmoother.h and demonstrates their principal Python operations.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import gtsam
import numpy as np
from gtsam.symbol_shorthand import L, X

Overview¶

The FixedLagSmoother class is the base class for BatchFixedLagSmoother and IncrementalFixedLagSmoother.

It provides an API for fixed-lag smoothing in nonlinear factor graphs. It maintains a sliding window of the most recent variables and marginalizes out older variables. This is particularly useful in real-time applications where memory and computational efficiency are critical.

Mathematical Formulation¶

In fixed-lag smoothing the objective is to estimate the state $\mathbf{x}_t$ given all measurements up to time $t$ , but only retaining a fixed window of recent states. The optimization problem can be expressed as:

\min_{\mathbf{x}_{t-L:t}} \sum_{i=1}^{N} \| \mathbf{h}_i(\mathbf{x}_{t-L:t}) - \mathbf{z}_i \|^2

(1)

where $L$ is the fixed lag, $\mathbf{h}_i$ are the measurement functions, and $\mathbf{z}_i$ are the measurements. In practice, the functions $\mathbf{h}_i$ depend only on a subset of the state variables $\mathbf{X}_i$ , and the optimization is performed over a set of $N$ factors $\phi_i$ instead:

\min_{\mathbf{x}_{t-L:t}} \sum_{i=1}^{N} \| \phi_i(\mathbf{X}_i; \mathbf{z}_i) \|^2

(2)

The API below allows the user to add new factors at every iteration, which will be automatically pruned after they no longer depend on any variables in the lag.

Wrapped class API¶

The header also exposes the following wrapped classes. This executable listing makes their constructors, properties, and methods visible in the installed build: FixedLagSmootherResult.

def public_api(qualified_name):
    value = gtsam
    for component in qualified_name.split('.'):
        value = getattr(value, component, None)
        if value is None:
            return []
    return [name for name in dir(value) if not name.startswith('_')]

print('\nFixedLagSmootherResult')
print(public_api('FixedLagSmootherResult'))