navlie.lib.models.RelativeBodyFrameVelocity¶

class navlie.lib.models.RelativeBodyFrameVelocity(Q1: ndarray, Q2: ndarray)¶

Bases: ProcessModel

The relative body-frame velocity process model is of the form

\[\mathbf{T}_k = \exp(\Delta t \mathbf{u}_{1,k-1}^\wedge) \mathbf{T}_{k-1} \exp(\Delta t \mathbf{u}_{2,k-1}^\wedge)\]

where \(\mathbf{u}_{1,k-1}\) and \(\mathbf{u}_{2,k-1}\) are inputs resolved in the world frame and body frame, respectively.

To be honest, I’m not sure if this is a useful process model for many people. We were using this for a while to model the relative motion of two robots given the body-frame-velocity of each robot.

This class is comptabile with SO2State, SO3State, SE2State, SE3State, SE23State.

Parameters:

Q1 (np.ndarray) – Covariance of first input.
Q2 (np.ndarray) – Covariance of second input.

evaluate(x: MatrixLieGroupState, u: VectorInput, dt: float) → MatrixLieGroupState¶

Evaluate discrete-time process model.

Parameters:

x (MatrixLieGroupState) – Any valid matrix Lie group state.
u (VectorInput) – Stacked input \([\mathbf{u}_1, \mathbf{u}_2]\).
dt (float) – Time step.

Returns:

New state.

Return type:

MatrixLieGroupState

jacobian(x: MatrixLieGroupState, u: VectorInput, dt: float) → ndarray¶

Implementation of the process model Jacobian with respect to the state.

\[\mathbf{F} = \frac{D {f}(\mathcal{X}_{k-1}, \mathbf{u}, \Delta t)}{D \mathcal{X}_{k-1}}\]

Parameters:

x (State) – State at time \(k-1\).
u (Input) – The input value \(\mathbf{u}\) provided as a Input object.
dt (float) – The time interval \(\Delta t\) between the two states.

Returns:

Process model Jacobian with respect to the state \(\mathbf{F}\).

Return type:

np.ndarray

covariance(x: MatrixLieGroupState, u: VectorInput, dt: float) → ndarray¶

Covariance matrix \(\mathbf{Q}_k\) of the additive Gaussian noise \(\mathbf{w}_{k} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)\). If this method is not overridden, the covariance of the process model error is approximated from the input covariance using a linearization procedure, with the input Jacobian evaluated using finite difference.

Parameters:

x (State) – State at time \(k-1\).
u (Input) – The input value \(\mathbf{u}\) provided as a Input object.
dt (float) – The time interval \(\Delta t\) between the two states.

Returns:

Covariance matrix \(\mathbf{Q}_k\).

Return type:

np.ndarray

evaluate_with_jacobian(x: ~navlie.types.State, u: ~navlie.types.Input, dt: float) -> (<class 'navlie.types.State'>, <class 'numpy.ndarray'>)¶: Evaluates the process model and simultaneously returns the Jacobian as its second output argument. This is useful to override for performance reasons when the model evaluation and Jacobian have a lot of common calculations, and it is more efficient to calculate them in the same function call.

input_covariance(x: State, u: Input, dt: float) → ndarray¶

Covariance matrix of additive noise on the input.

Parameters:

x (State) – State at time \(k-1\).
u (Input) – The input value \(\mathbf{u}\) provided as a Input object.
dt (float) – The time interval \(\Delta t\) between the two states.

Returns:

Covariance matrix \(\mathbf{R}_k\).

Return type:

np.ndarray

input_jacobian_fd(x: State, u: Input, dt: float, step_size=1e-06, *args, **kwargs) → ndarray¶: Calculates the input jacobian with finite difference.

jacobian_fd(x: State, u: Input, dt: float, step_size=1e-06, *args, **kwargs) → ndarray¶: Calculates the model jacobian with finite difference.

sqrt_information(x: State, u: Input, dt: float) → ndarray¶

navlie.lib.models.RangeRelativePose

navlie.lib.models.SingleIntegrator