navlie.lib.models.PointRelativePosition¶
- class navlie.lib.models.PointRelativePosition(landmark_position: ndarray, R: ndarray, landmark_id: Any | None = None)¶
Bases:
MeasurementModelMeasurement model describing the position of a known landmark relative to the robot, resolved in the body frame. That is, the state must describe the pose of the robot, and the measurement is the position of the landmark.
\[\mathbf{y} = \mathbf{C}_{ab}^T (\mathbf{r}_\ell - \mathbf{r})\]where \(\mathbf{C}\) is the attitude of the robot, \(\mathbf{r}_\ell\) is the position of the landmark, and \(\mathbf{r}\) is the position of the robot.
This class is comptabile with
SE2State, SE3State, SE23State, IMUState.- Parameters:
landmark_position (np.ndarray) – Position of landmark in body frame.
R (np.ndarray) – Measurement covariance.
landmark_id (Any, optional) – A unique identifier for the landmark that this measurement is of.
- evaluate(x: MatrixLieGroupState) → ndarray¶
Evaluates the measurement model \(\mathbf{g}(\mathcal{X})\).
- jacobian(x: MatrixLieGroupState) → ndarray¶
Evaluates the measurement model Jacobian with respect to the state.
\[\mathbf{G} = \frac{D \mathbf{g}(\mathcal{X})}{D \mathcal{X}}\]
- covariance(x: MatrixLieGroupState) → 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.