SE2 Models

See the Victoria Park example to see these classes in usage.

Odometry Process Model

class inekf.OdometryProcess(*args: Any, **kwargs: Any)

Bases: inekf._inekf.ProcessModel_SE2_1_0_SE2_1_0

Odometry process model with single column.

There’s a number of available constructors, see InEKF::OdometryProcess for a list of all of them.

Methods:

`f`(u, dt, state)	Overriden from base class.
`makePhi`(u, dt, state)	Overriden from base class.
`setQ`(q)	Set Q from a variety of sources

f(u, dt, state)

Overriden from base class. Propagates the model $X_{t+1} = XU$

Parameters

u (inekf.SE2) – Rigid body transformation of vehicle since last timestep.
dt (float) – Delta time
state (inekf.SE2) – Current state

Returns

Updated state estimate

Return type

inekf.SE2

makePhi(u, dt, state)

Overriden from base class. If right, this is the identity. If left, it’s the adjoint of U.

Parameters

u (inekf.SE2) – Rigid body transformation of vehicle since last timestep.
dt (float) – Delta time
state (inekf.SE2) – 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

setQ(q)

Set Q from a variety of sources

Parameters: q (np.ndarray or float) – Can be a float, 3-vector, or 3x3-matrix. Sets the covariance Q accordingly.

Dynamic Odometry Process Model

class inekf.OdometryProcessDynamic(*args: Any, **kwargs: Any)

Bases: inekf._inekf.ProcessModel_SE2_D_0_SE2_1_0

Odometry process model with variable number of columns, for use in SLAM on SE2.

There’s a number of available constructors, see InEKF::OdometryProcessDynamic for a list of all of them.

Methods:

`f`(u, dt, state)	Overriden from base class.
`makePhi`(u, dt, state)	Overriden from base class.
`setQ`(q)	Set Q from a variety of sources

f(u, dt, state)

Overriden from base class. Propagates the model $X_{t+1} = XU$. Landmarks are left as is.

Parameters

u (inekf.SE2) – Rigid body transformation of vehicle since last timestep.
dt (float) – Delta time
state (inekf.SE2[-1,0]) – Current state

Returns

Updated state estimate

Return type

inekf.SE2[-1,0]

makePhi(u, dt, state)

Overriden from base class. If right, this is the identity. If left, it’s the adjoint of U. Landmark elements are the identity in both versions of Phi.

Parameters

u (inekf.SE2) – Rigid body transformation of vehicle since last timestep.
dt (float) – Delta time
state (inekf.SE2[-1,0]) – 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

setQ(q)

Set Q from a variety of sources

Parameters: q (np.ndarray or float) – Can be a float, 3-vector, or 3x3-matrix. Sets the covariance Q accordingly.

GPS

class inekf.GPSSensor(*args: Any, **kwargs: Any)

Bases: inekf._inekf.MeasureModel_SE2_D_0

GPS Sensor for use in SE2 SLAM model.

Parameters: std (float) – The standard deviation of a measurement.

Methods:

processZ(z, state)

Overriden from the base class.

processZ(z, state)

Overriden from the base class. Needed to fill out H/z with correct number of columns based on number of landmarks in state.

Parameters

z (np.ndarray) – Measurement
state (inekf.SE2[-1,0]) – Current state estimate.

Returns

Processed measurement.

Return type

np.ndarray

Landmark Sensor

class inekf.LandmarkSensor(*args: Any, **kwargs: Any)

Bases: inekf._inekf.MeasureModel_SE2_D_0

Landmark sensor used in SLAM on SE2

Parameters

std_r (float) – Range measurement standard deviation
std_b (float) – Bearing measurement standard deviation

Methods:

`calcMahDist`(state)	Calculates Mahalanobis distance of having seen a certain landmark.
`calcSInverse`(state)	Overriden from base class.
`makeHError`(state, iekfERROR)	Overriden from base class.
`processZ`(z, state)	Overriden from base class.
`sawLandmark`(state)	Sets H based on what landmark was recently seen.

calcMahDist(state)

Calculates Mahalanobis distance of having seen a certain landmark. Used for data association.

Parameters

z (np.ndarray) – Range and bearing measurement
state (inekf.SE2[-1,0]) – Current state estimate

Returns

Mahalanobis distance

Return type

float

calcSInverse(state)

Overriden from base class. If using RInEKF, takes advantage of sparsity of H to shrink matrix multiplication. Otherwise, operates identically to base class.

Parameters: state (inekf.SE2[-1,0]) – Current state estimate.
Returns: Inverse of measurement noise.
Return type: np.ndarray

makeHError(state, iekfERROR)

Overriden from base class. Saves filter error for later use, then calls base class.

Parameters

state (inekf.SE2[-1,0]) – Current state estimate.
iekfERROR (ERROR) – Type of filter error.

Returns

H_error

Return type

np.ndarray

processZ(z, state)

Overriden from base class. Converts r,b -> x,y coordinates and shifts measurement covariance. Then fills out z accordingly.

Parameters

z (np.ndarray) – Measurement
state (inekf.SE2[-1,0]) – Current state estimate.

Returns

Processed measurement.

Return type

np.ndarray

sawLandmark(state)

Sets H based on what landmark was recently seen.

Parameters

seen. (idx Index of landmark recently) –
landmarks. (state Current state estimate. Used for # of) –