Core Classes

Error

class inekf.ERROR(value)

Type of invariant error. Has options for left or right.

Attributes:

`LEFT`	Left error
`RIGHT`	Right error

LEFT = 0: Left error

RIGHT = 1: Right error

Invariant Extended Kalman Filter

class inekf.InEKF(pModel, x0, error)

The Invariant Extended Kalman Filter

Parameters

pModel (ProcessModel) – Process model
state (inekf.LieGroup) – Initial state, must be of same group that process model uses and must be uncertain
error (ERROR) – Right or left invariant error

Methods:

`addMeasureModel`(name, m)	Add measurement model to the filter.
`predict`(u[, dt])	Prediction Step.
`update`(name, m)	Update Step.

Attributes:

state

Current state estimate.

addMeasureModel(name, m)

Add measurement model to the filter.

Parameters

name (str) – Name of measurement model.
m (MeasureModel) – A measure model pointer, templated by the used group.

predict(u, dt=1)

Prediction Step.

Parameters

u (control) – Must be same as what process model uses.
dt (float) – Delta t. Used sometimes depending on process model. Defaults to 1.

Returns

State estimate

Return type

inekf.LieGroup

property state

Current state estimate. May be read or wrriten, but can’t be edited in place.

Returns: state
Return type: inekf.LieGroup

update(name, m)

Update Step.

Parameters

name (str) – Name of measurement model.
z (np.ndarray) – Measurement. May vary in size depending on how measurement model processes it.

Returns

State estimate.

Return type

inekf.LieGroup

Measure Model

class inekf.MeasureModel(b, M, error)

Base class measure model. Written to be inherited from, but in most cases this class will be sufficient. More information on inheriting can be seen in Custom Models.

We have overloaded the [] operator to function as a python template. Example of this can be seen in Getting Started.

Templates:

Group State’s group that is being tracked, of type inekf.LieGroup.

Attributes:

`H`	Linearized matrix H.
`H_error`	This is the converted H used in InEKF if it's a right filter with left measurement or vice versa.
`M`	Measurement covariance.
`b`	b vector used in measure model.
`error`	Type of error of the filter (right/left)

Methods:

`calcSInverse`(state)	Calculate inverse of measurement noise S, using H_error.
`calcV`(z, state)	Computes innovation based on measurement model.
`makeHError`(state, iekfERROR)	Sets and returns H_error for settings where filter error type != measurement error type.
`processZ`(z, state)	Process measurement before putting into InEKF.
`setHandb`(b)	Sets measurement vector b and recreates H accordingly.

property H

Linearized matrix H. Will be automatically created from b in constructor unless overriden. May be read or written, but not modified in place.

Returns: np.ndarray

property H_error

This is the converted H used in InEKF if it’s a right filter with left measurement or vice versa. Used in calcSInverse if overriden. Probably won’t need to be overwritten. May be read or written, but not modified in place.

Returns: np.ndarray

property M

Measurement covariance.

Returns: np.ndarray

property b

b vector used in measure model.

Returns: np.ndarray

calcSInverse(state)

Calculate inverse of measurement noise S, using H_error. Called fourth in the update step.

Parameters: state (inekf.LieGroup) – Current state estimate.
Returns: Inverse of measurement noise.
Return type: np.ndarray

calcV(z, state)

Computes innovation based on measurement model. Called third in the update step.

Parameters

z (np.ndarray) – Measurement.
state (inekf.LieGroup) – Current state estimate.

Returns

Truncated innovation.

Return type

np.ndarray

property error

Type of error of the filter (right/left)

Returns: ERROR

makeHError(state, iekfERROR)

Sets and returns H_error for settings where filter error type != measurement error type. Done by multiplying H by adjoint of current state estimate. Called second in update step.

Parameters

state (inekf.LieGroup) – Current state estimate.
iekfERROR (ERROR) – Type of filter error.

Returns

H_error

Return type

np.ndarray

processZ(z, state)

Process measurement before putting into InEKF. Can be used to change frames, convert r/b->x/y, or append 0s. By default is used to append zeros/ones onto it according to b vector set. Called first in update step.

Parameters

z (np.ndarray) – Measurement
state (inekf.LieGroup) – Current state estimate.

Returns

Processed measurement.

Return type

np.ndarray

setHandb(b)

Sets measurement vector b and recreates H accordingly. Useful if vector b isn’t constant.

Parameters: b (np.ndarray) – Measurement model b

Process Model

class inekf.ProcessModel

Base class process model. Must be inheriting from, base class isn’t implemented. More information on inheriting can be seen in Custom Models.

We have overloaded the [] operator to function as a python template. Example of this can be seen in Getting Started.

Templates:

Group State’s group that is being tracked, of type inekf.LieGroup.
U Form of control. Can be either a group of inekf.LieGroup, or a vector. Vectors can be used for example by “Vec3” or 3 for a vector of size 3. -1, “D”, or “VecD” for dynamic control size.

Attributes:

`Q`	Process model covariance.

Methods:

`f`(u, dt, state)	Propagates state forward one timestep.
`makePhi`(u, dt, state)	Make a discrete time linearized process model matrix, with $Phi = exp(ADelta t)$.

property Q

Process model covariance. May be read or written, but not modified in place.

Returns: np.ndarray

f(u, dt, state)

Propagates state forward one timestep. Must be overriden, has no default implementation.

Parameters

u (control) – Control
dt (float) – Delta time
state (inekf.LieGroup) – Current state

Returns

Updated state estimate

Return type

inekf.LieGroup

makePhi(u, dt, state)

Make a discrete time linearized process model matrix, with $Phi = exp(ADelta t)$. Must be overriden, has no default implementation.

Parameters

u (control) – Control
dt (float) – Delta time
state (inekf.LieGroup) – Current state estimate (shouldn’t be needed unless doing an “Imperfect InEKF”)
error (ERROR) – Right or left error. Function should be implemented to handle both.

Returns

Phi

Return type

np.ndarray