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 timestate (
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 timestate (
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
) – Measurementstate (
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
) – Measurementstate (
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).