navlie.lib.models.InvariantMeasurement

class navlie.lib.models.InvariantMeasurement(meas: Measurement, direction='auto', model=None)

Bases: Measurement

Given a Measurement object, the class will construct a left- or right-invariant innovation ready to be fused into a state estimator.

If a right-invariant innovation is chosen then the following will be formed.

\[ \begin{align}\begin{aligned}\mathbf{z} &= \bar{\mathbf{X}}(\mathbf{y} - \bar{\mathbf{y}})\\&= \bar{\mathbf{X}}(\mathbf{g}(\mathbf{X}) + \mathbf{v} - \mathbf{g}(\bar{\mathbf{X}}))\\&\approx \bar{\mathbf{X}}( \mathbf{g}(\bar{\mathbf{X}}) + \mathbf{G}\delta \mathbf{\xi} + \mathbf{v} - \mathbf{g}(\bar{\mathbf{X}}))\\&= \bar{\mathbf{X}}\mathbf{G}\delta \mathbf{\xi} + \bar{\mathbf{X}}\mathbf{v}\end{aligned}\end{align} \]

and hence \(\bar{\mathbf{X}}\mathbf{G}\) is the Jacobian of \(\mathbf{z}\), where \(\mathbf{G}\) is the Jacobian of \(\mathbf{g}(\mathbf{X})\). Similarly, if a left-invariant innovation is chosen,

\[ \begin{align}\begin{aligned}\mathbf{z} &= \bar{\mathbf{X}}^{-1}(\mathbf{y} - \bar{\mathbf{y}})\\&\approx \bar{\mathbf{X}}^{-1}\mathbf{G}\delta \mathbf{\xi} + \bar{\mathbf{X}}^{-1}\mathbf{v}\end{aligned}\end{align} \]

and hence \(\bar{\mathbf{X}}^{-1}\mathbf{G}\) is the Jacobian of \(\mathbf{z}\).

Parameters:

• meas (Measurement) – Measurement value

• direction ("left" or "right" or "auto") – whether to form a left- or right-invariant innovation, by default “auto”. If “auto” is chosen, the direction will be chosen to be the opposite of the direction of the state.

• model (MeasurementModel, optional) – a measurement model that directly returns the innovation and Jacobian and covariance of the innovation. If none is supplied, the default InvariantInnovation will be used, which computes the Jacobian of the innovation indirectly via chain rule.

value

Container for the measurement value

Type:

numpy.ndarray

stamp

Timestamp

Type:

float

model

measurement model associated with this measurement.

Type:

navlie.types.MeasurementModel

state_id

Optional, ID of the state this measurement is associated.

Type:

Any

minus(y_check: ndarray) → ndarray

Evaluates the difference between the current measurement and a predicted measurement.

By default, assumes that the measurement is a column vector, and thus, the minus operator is simply vector subtraction.

previous

navlie.lib.models.Gravitometer

next

navlie.lib.models.InvariantPointRelativePosition