catheter_ukf package

catheter_ukf package#

Submodules#

catheter_ukf.riemannian_ukf module#

This module implements the Kalman equations for a UKF on a riemannian manifold.

For reference, see:
Unscented Kalman Filtering on Riemannian Manifolds

by

Soren Hauberg + Prancois Lauze + Kim Steenstrup Pedersen

class catheter_ukf.riemannian_ukf.RiemannianUKF(statespace, unscented)#

Bases: object

Keep track of parameters used for applying an unscented Kalman filter in a Riemannian setting.

These parameters are:

statespace: Knows how to manipulate states in local and global coords. unscented: Knows how to compute sigma points and covariances.

predict(x, P, Q, dt)#

Perform the predict phase of the unscented Kalman filter.

Args:

x: The a-priori state estimate. P: The a-priori error covariance, based at x. Q: The transition covariance, based at x. dt: The timestep.

Returns:

x: The predicted state estimate. P: The predicted error covariance.

update(x, P, R, z)#

Perform the update phase of the unscented Kalman filter.

Note that this makes the assumption that the observation space is a vector space.

Args:

x: The predicted state estimate. P: The predicted error covariance, based at x. R: The observation covariance. z: The observation.

Returns:

x: The a-posteriori state estimate. P: The a-posteriori error covariance.

catheter_ukf.riemannian_ukf.predict(x, P, Q, dt, statespace, unscented)#

Perform the predict phase of the unscented Kalman filter.

Args:

x: The a-priori state estimate. P: The a-priori error covariance, based at x. Q: The transition covariance, based at x. dt: The timestep. statespace: Knows how to manipulate states in local and global coords. unscented: Knows how to compute sigma points and covariances.

Returns:

x: The predicted state estimate. P: The predicted error covariance.

catheter_ukf.riemannian_ukf.update(x, P, R, z, statespace, unscented)#

Perform the update phase of the unscented Kalman filter.

Note that this makes the assumption that the observation space is a vector space.

Args:

x: The predicted state estimate. P: The predicted error covariance, based at x. R: The observation covariance. z: The observation. statespace: Knows how to manipulate states in local and global coords. unscented: Knows how to compute sigma points and covariances.

Returns:

x: The a-posteriori state estimate. P: The a-posteriori error covariance.

catheter_ukf.statespace module#

This module implements various state manipulation functions for the UKF.

States are represented by 3+3+3+3+3+3 = 18 parameters. These are:

x: In R3. The position of the midpoint between the coils. v: In R3. The velocity of x. a: In R3. The acceleration of x. q: In S2 (as a subset of R3). The direction from x to the catheter’s tip. w: In R2 (as a subset of R3). The (rotational) velocity of q. u: In R2 (as a subset of R3). The (rotational) acceleration of q.

If q is a point on the unit sphere, then w and u are tangent to the sphere at q.

Roll of the catheter is not represented.

catheter_ukf.statespace.Exp(base, v)#
catheter_ukf.statespace.Log(base, p)#
catheter_ukf.statespace.Rot(base, v)#
class catheter_ukf.statespace.States(coil_distance, tip_distance)#

Bases: object

Keep track of parameters used for state transition and observation.

These parameters are:

coil_distance: The distance between the coils in mm. tip_distance: The distance from the tip to the tip-adjacent coil in mm.

evolve_state(s, dt)#

Calculate the evolution of the given state after the given duration.

Args:

s: The starting state. dt: The duration.

Returns the evolved state.

global_to_local(global_base, global_coordinate)#
local_identity(global_base)#
local_to_global(global_base, local_coordinate)#
local_transition_cov(s, Q)#
observe_state(s)#
tip_from_state(s)#
catheter_ukf.statespace.evolve_state(s, dt)#
catheter_ukf.statespace.global_to_local(base, g)#
catheter_ukf.statespace.local_to_global(base, l)#
catheter_ukf.statespace.local_transition_cov(s, Q)#
catheter_ukf.statespace.observe_state(s, coil_offset)#
catheter_ukf.statespace.pack_global_state(x, v, a, q, w, u)#
catheter_ukf.statespace.pack_local_state(x, v, a, q, w, u)#
catheter_ukf.statespace.pack_observation(a, b)#
catheter_ukf.statespace.tip_from_state(s, tip_offset)#
catheter_ukf.statespace.unpack_observation(o)#
catheter_ukf.statespace.unpack_state(s)#

catheter_ukf.ukf module#

class catheter_ukf.ukf.UKF(coil_distance=7.8, tip_distance=9.0, h=0.0001)#

Bases: object

Keeps track of the parameters required to use the unscented Kalman filter designed for tracking our catheter.

Args:

coil_distance: The distance between the coils in mm. tip_distance: The distance from the tip to the tip-adjacent coil in mm. h: Determines the spread in the sigma points. If the filter seems to

diverge consider reducing this value.

estimate_initial_state(dist_coord, prox_coord)#
filter(x, P, z, dt)#

Perform a combination predict/update step using this unscented Kalman filter.

Args:

x: The a-priori state estimate. P: The a-priori error covariance, based at x. z: The observation. dt: The timestep.

Returns:

x: The a-posteriori state estimate. P: The a-posteriori error covariance.

predict(x, P, dt)#

Perform the predict phase of the unscented Kalman filter.

Args:

x: The a-priori state estimate. P: The a-priori error covariance, based at x. dt: The timestep.

Returns:

x: The predicted state estimate. P: The predicted error covariance.

tip_and_coils(x)#
update(x, P, z)#

Perform the update phase of the unscented Kalman filter.

Args:

x: The predicted state estimate. P: The predicted error covariance, based at x. z: The observation.

Returns:

x: The a-posteriori state estimate. P: The a-posteriori error covariance.

catheter_ukf.unscented module#

This module implements the unscented transform required for the UKF.

class catheter_ukf.unscented.Unscented(h)#

Bases: object

Keep track of parameters required for the unscented transform.

These parameters are:

h: Determines the spread in the sigma points.

sigmas_from_stats(x, P)#

Compute sigma points and weights from the given parameters.

Args:

x: The mean of the sigma points. P: The covariance of the sigma points.

Returns:

sigmas: The calculated sigma points. weights: The calculated weights.

stats_from_sigmas(sigmas, weights)#

Compute the mean and covariance of the given sigma points.

Args:

sigmas: The sigma points. weights: The weights.

Returns:

x: The weighted mean of the sigma points. P: The weighted covariance of the sigma points.

catheter_ukf.unscented.sigmas_from_stats(x, P, h)#

Compute sigma points and weights from the given parameters.

Compute a set of sigma points and weights representing the given mean and covariance.

Args:

x: The mean of the sigma points. P: The covariance of the sigma points. h: The spread of the sigma points. Controls distance between sigmas.

Returns:

sigmas: The calculated sigma points. weights: The calculated weights.

catheter_ukf.unscented.stats_from_sigmas(sigmas, weights)#

Compute the mean and covariance of the given sigma points.

Compute the mean and covariance represented by a set of sigma points and weights.

Args:

sigmas: The sigma points. weights: The weights.

Returns:

x: The weighted mean of the sigma points. P: The weighted covariance of the sigma points.

Module contents#

This module contains sub-modules related to applying a UKF to our catheter tracking problem.

Most of the stuffs found within are “implementation details”. For users the most relevant things are exported here: UKF.

Contents