Inertial Models

See the Underwater Inertial example to see these classes in usage.

Inertial Process Model

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

Bases: inekf._inekf.ProcessModel_SE3_2_6_Vec6

Inertial process model. Integrates IMU measurements and tracks biases. Requires “Imperfect InEKF” since biases don’t fit into Lie group structure.

Methods:

`f`(u, dt, state)	Overriden from base class.
`makePhi`(u, dt, state)	Overriden from base class.
`setAccelBiasNoise`(std)	Set the accelerometer bias noise.
`setAccelNoise`(std)	Set the accelerometer noise.
`setGyroBiasNoise`(std)	Set the gryo bias noise.
`setGyroNoise`(std)	Set the gyro noise.

f(u, dt, state)

Overriden from base class. Integrates IMU measurements.

Parameters

u (np.ndarray) – 6-Vector. First 3 are angular velocity, last 3 are linear acceleration.
dt (float) – Delta time
state (inekf.SE3[2,6]) – Current state

Returns

Updated state estimate

Return type

inekf.SE3[2,6]

makePhi(u, dt, state)

Overriden from base class. Since this is used in an “Imperfect InEKF”, both left and right versions are slightly state dependent.

Parameters

u (np.ndarray) – 6-Vector. First 3 are angular velocity, last 3 are linear acceleration.
dt (float) – Delta time
state (inekf.SE3[2,6]) – 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

setAccelBiasNoise(std)

Set the accelerometer bias noise. Defaults to 0 if not set.

Parameters: std (float) – Accelerometer bias standard deviation

setAccelNoise(std)

Set the accelerometer noise. Defaults to 0 if not set.

Parameters: std (float) – Accelerometer standard deviation

setGyroBiasNoise(std)

Set the gryo bias noise. Defaults to 0 if not set.

Parameters: std (float) – Gyroscope bias standard deviation

setGyroNoise(std)

Set the gyro noise. Defaults to 0 if not set.

Parameters: std (float) – Gyroscope standard deviation

Depth Sensor

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

Bases: inekf._inekf.MeasureModel_SE3_2_6

Pressure/Depth sensor measurement model for use with inertial process model. Uses pseudo-measurements to fit into a left invariant measurement model.

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

Methods:

`calcSInverse`(state)	Overriden from base class.
`processZ`(z, state)	Overriden from the base class.
`setNoise`(std)	Set the measurement noise

calcSInverse(state)

Overriden from base class. Calculate inverse of measurement noise S, using the Woodbury Matrix Identity

Parameters: state (inekf.SE3[2,6]) – Current state estimate.
Returns: Inverse of measurement noise.
Return type: np.ndarray

processZ(z, state)

Overriden from the base class. Inserts psuedo measurements for the x and y value to fit the invariant measurement.

Parameters

z (np.ndarray) – Measurement
state (inekf.SE3[2,6]) – Current state estimate.

Returns

Processed measurement.

Return type

np.ndarray

setNoise(std)

Set the measurement noise

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

Doppler Velocity Log

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

Bases: inekf._inekf.MeasureModel_SE3_2_6

DVL sensor measurement model for use with inertial process model.

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

Methods:

`processZ`(z, state)	Overriden from base class.
`setNoise`(std_dvl, std_imu)	Set the noise covariances.

processZ(z, state)

Overriden from base class. Takes in a 6 vector with DVL measurement as first 3 elements and IMU as last three and converts DVL to IMU, then makes it the right size and passes it on.

Parameters

z (np.ndarray) – Measurement
state (inekf.SE3[2,6]) – Current state estimate.

Returns

Processed measurement.

Return type

np.ndarray

setNoise(std_dvl, std_imu)

Set the noise covariances.

Parameters

std_dvl (float) – Standard deviation of DVL measurement.
std_imu (float) – Standard deviation of gyropscope measurement (needed b/c we transform frames).