navlie.lib.preintegration.LinearIncrement¶

class navlie.lib.preintegration.LinearIncrement(input_covariance: ndarray, state_matrix: Callable[[VectorInput, float], ndarray], input_matrix: Callable[[VectorInput, float], ndarray], dof: int, bias: ndarray | None = None, state_id: Any | None = None)¶

Bases: RelativeMotionIncrement

This class preintegrates any process model of the form

\[\mathbf{x}_{k} = \mathbf{A}_{k-1} \mathbf{x}_{k-1} + \mathbf{B}_{k-1}(\mathbf{u}_{k-1} + \mathbf{w}_{k-1})\]

where \(\mathbf{w}_{k-1} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{k-1})\). By directly interating this equation, it can be shown that

\[\mathbf{x}_j - \left(\prod_{k=i}^{j-1} \mathbf{A}_k \right) \mathbf{x}_i + \sum_{k=i}^{j-1} \left(\prod_{\ell=k+1}^{j-1} \mathbf{A}_\ell\right) \mathbf{B}_k \mathbf{u}_k\]

which can be rewritten as

\[\mathbf{x}_j = \mathbf{A}_{ij} \mathbf{x}_i + \Delta \mathbf{u}_{ij} + \mathbf{w}_{ij}\]

This class will compute the preintegrated quantities \(\mathbf{A}_{ij}, \Delta \bar{\mathbf{u}}_{ij}, \mathbf{Q}_{ij}\) where \(\mathbf{w}_{ij} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{ij})\).

Parameters:

input_covariance (numpy.ndarray) – Covariance associated with the input. If a bias is also supplied, then this should also contain the covariance of the bias random walk.
state_matrix (Callable[[VectorInput, float], numpy.ndarray]) – The state transition matrix, supplied as a function of the input and time interval dt.
input_matrix (Callable[[VectorInput, float], numpy.ndarray]) – The input matrix, supplied as a function of the input and time interval dt.
dof (int) – the total dof of the state
bias (numpy.ndarray, optional) – If provided, this bias will be subtracted from the input values. Furthermore, the covariance associated with this RMI will also be augmented to include a bias state.
state_id (Any, optional) – Optional container for other identifying information, by default None.