Inertial Models
See the Underwater Inertial example to see these classes in usage.
Inertial Process Model
-
class InEKF::InertialProcess : public InEKF::ProcessModel<SE3<2, 6>, Eigen::Matrix<double, 6, 1>>
Inertial process model. Integrates IMU measurements and tracks biases. Requires “Imperfect InEKF” since biases don’t fit into Lie group structure.
Public Functions
-
InertialProcess()
Construct a new Inertial Process object.
-
inline ~InertialProcess()
Destroy the Inertial Process object.
-
SE3<2, 6> f(Eigen::Vector6d u, double dt, SE3<2, 6> state) override
Overriden from base class. Integrates IMU measurements.
- Parameters
u – Control. First 3 are angular velocity, last 3 are linear acceleration.
dt – Delta time
state – Current state
- Returns
Integrated state
-
MatrixCov makePhi(const Eigen::Vector6d &u, double dt, const SE3<2, 6> &state, ERROR error) override
Overriden from base class. Since this is used in an “Imperfect InEKF”, both left and right versions are slightly state dependent.
- Parameters
u – Control
dt – Delta time
state – Current state estimate (shouldn’t be needed unless doing an “Imperfect InEKF”)
error – Right or left error. Function should be implemented to handle both.
- Returns
Phi
-
void setGyroNoise(double std)
Set the gyro noise. Defaults to 0 if not set.
- Parameters
std – Gyroscope standard deviation
-
void setAccelNoise(double std)
Set the accelerometer noise. Defaults to 0 if not set.
- Parameters
std – Accelerometer standard deviation
-
void setGyroBiasNoise(double std)
Set the gryo bias noise. Defaults to 0 if not set.
- Parameters
std – Gyroscope bias standard deviation
-
void setAccelBiasNoise(double std)
Set the accelerometer bias noise. Defaults to 0 if not set.
- Parameters
std – Accelerometer bias standard deviation
-
InertialProcess()
Depth Sensor
-
class InEKF::DepthSensor : public 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.
Public Functions
-
DepthSensor(double std = 1)
Construct a new Depth Sensor object.
- Parameters
std – The standard deviation of the measurement.
-
inline ~DepthSensor()
Destroy the Depth Sensor object.
-
virtual VectorB processZ(const Eigen::VectorXd &z, const SE3<2, 6> &state) override
Overriden from the base class. Inserts psuedo measurements for the x and y value to fit the invariant measurement.
- Parameters
z – Measurement
state – Current state estimate.
- Returns
Processed measurement.
-
virtual MatrixS calcSInverse(const SE3<2, 6> &state) override
Overriden from base class. Calculate inverse of measurement noise S, using the Woodbury Matrix Identity.
- Parameters
state – Current state estimate.
- Returns
Inverse of measurement noise.
-
void setNoise(double std)
Set the measurement noise.
- Parameters
std – The standard deviation of the measurement.
-
DepthSensor(double std = 1)
Doppler Velocity Log
-
class InEKF::DVLSensor : public InEKF::MeasureModel<SE3<2, 6>>
DVL sensor measurement model for use with inertial process model.
Public Functions
-
DVLSensor()
Construct a new DVLSensor object. Assumes no rotation or translation between this and IMU frame.
-
DVLSensor(Eigen::Matrix3d dvlR, Eigen::Vector3d dvlT)
Construct a new DVLSensor object with offset from IMU frame.
- Parameters
dvlR – 3x3 Rotation matrix encoding rotationf rom DVL to IMU frame.
dvlT – 3x1 Vector of translation from IMU to DVL in IMU frame.
-
DVLSensor(SO3<> dvlR, Eigen::Vector3d dvlT)
Construct a new DVLSensor object with offset from IMU frame.
- Parameters
dvlR – SO3 object encoding rotationf rom DVL to IMU frame.
dvlT – 3x1 Vector of translation from IMU to DVL in IMU frame.
-
DVLSensor(SE3<> dvlH)
Construct a new DVLSensor object with offset from IMU frame.
- Parameters
dvlH – SE3 object encoding transformation from DVL to IMU frame.
-
void setNoise(double std_dvl, double std_imu)
Set the noise covariances.
- Parameters
std_dvl – Standard deviation of DVL measurement.
std_imu – Standard deviation of gyropscope measurement (needed b/c we transform frames).
-
virtual VectorB processZ(const Eigen::VectorXd &z, const SE3<2, 6> &state) override
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 – DVL/IMU measurement.
state – Current state estimate.
- Returns
Processed measurement.
-
DVLSensor()