navlie.lib.models.RangePoseToPose¶
- class navlie.lib.models.RangePoseToPose(tag_body_position1, tag_body_position2, state_id1, state_id2, R)¶
Bases:
MeasurementModelRange model given two absolute poses of rigid bodies, each containing a ranging tag.
Compatible with
SE2State, SE3State, SE23State, IMUState.- Parameters:
tag_body_position1 (np.ndarray) – Position of tag in body frame of Robot 1.
tag_body_position2 (np.ndarray) – Position of tag in body frame of Robot 2.
state_id1 (Any) – State ID of Robot 1.
state_id2 (Any) – State ID of Robot 2.
R (float or np.ndarray with size 1) – Covariance associated with range measurement error.
- evaluate(x: CompositeState) → ndarray¶
Evaluates the measurement model \(\mathbf{g}(\mathcal{X})\).
- jacobian(x: CompositeState) → ndarray¶
Evaluates the measurement model Jacobian with respect to the state.
\[\mathbf{G} = \frac{D \mathbf{g}(\mathcal{X})}{D \mathcal{X}}\]
- covariance(x: CompositeState) → ndarray¶
Returns the covariance \(\mathbf{R}\) associated with additive Gaussian noise.
- evaluate_with_jacobian(x: State) → Tuple[ndarray, ndarray]¶
Evaluates the measurement 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.