tools.RigidMotionEKF

tools.RigidMotionEKF#

class bartorch.tools.RigidMotionEKF#

Bases:

Constant-velocity extended Kalman filter over registered rigid poses.

The state is the pose and its rate of change, 2 * dof entries with dof three in 2D and six in 3D. The process noise is that of a piecewise-constant acceleration of variance process_noise per pose coordinate; the measurement is the pose a registration returns. Angle innovations and angles are wrapped to \([-\pi, \pi)\).

Parameters:

  • registration (RigidRegistration) – Measurement model used by step().

  • dimension ({2, 3}, default=3) – Spatial dimensions.

  • process_noise (float or sequence of float, default=0.0001) – Acceleration variance, one value or one per pose coordinate.

  • measurement_noise (float or sequence of float, default=0.01) – Measurement variance, one value or one per pose coordinate, used when a measurement carries no covariance.

  • initial_covariance (float, default=1.0) – Diagonal of the state covariance at initialization.

state#

Pose followed by its rate.

Type:

numpy.ndarray

covariance#

State covariance.

Type:

numpy.ndarray

initialized#

Whether a measurement has been fused; the first one initializes the state rather than updating it.

Type:

bool

property pose#

Filtered pose, with its covariance and the last measurement’s metric.

property velocity#

Angular and translational rates, in pose order, per unit of dt.

reset()#

Return to the uninitialized state, or initialize at initial.

predict()#

Propagate the state by dt and return the predicted pose.

Parameters:

dt (float, default=1.0) – Time step, in the unit the noise variances are stated per.

update()#

Fuse a measured pose and return the filtered pose.

The covariance update is the Joseph form.

Parameters:

  • measurement (RigidMotionEstimate or array_like) – Measured pose; bare parameters are taken about the filter’s centre.

  • covariance (array_like, default=None) – Measurement covariance; that of measurement, or measurement_noise on the diagonal, otherwise.

step()#

Predict, register moving to fixed from the prediction, and update.

Parameters: