IMU Sensor Calibration System. More...

#include <IMUCalibration.hpp>

Collaboration diagram for fsw::calibration::IMUCalibration:

Classes
struct	CalibrationParams
	Calibration parameters. More...
struct	RawReading
	Raw sensor reading. More...
struct	CalibratedReading
	Calibrated reading. More...
struct	CalibrationPoint
	Calibration data point. More...

Public Types
enum class	SensorType { ACCELEROMETER , GYROSCOPE , MAGNETOMETER }
	Sensor type. More...
enum class	CalibrationStatus { NOT_CALIBRATED , CALIBRATING , CALIBRATED , DRIFT_DETECTED , RECALIBRATION_REQUIRED , FAILED }
	Calibration status. More...

Public Member Functions
	IMUCalibration (SensorType type)
	~IMUCalibration ()
void	initialize (const CalibrationParams &initial_params=CalibrationParams{})
	Initialize calibration.
void	addCalibrationPoint (const CalibrationPoint &point)
	Add calibration data point.
bool	calibrate ()
	Perform calibration.
bool	calibrateAccelerometer (const std::vector< RawReading > &readings)
	Calibrate accelerometer using static positions.
bool	calibrateGyroscope (const std::vector< RawReading > &readings, const std::vector< double > &reference_rates={})
	Calibrate gyroscope using rotation rates.
bool	calibrateMagnetometer (const std::vector< RawReading > &readings, const Eigen::Vector3d &reference_field)
	Calibrate magnetometer using known field.
CalibratedReading	calibrateReading (const RawReading &raw) const
	Apply calibration to raw reading.
CalibrationParams	getCalibrationParams () const
	Get calibration parameters.
CalibrationStatus	getStatus () const
	Get calibration status.
bool	isValid () const
	Check if calibration is valid.
void	reset ()
	Reset calibration.

Private Member Functions
void	estimateBias (const std::vector< RawReading > &readings)
void	estimateScaleAndMisalignment (const std::vector< CalibrationPoint > &points)
void	estimateTemperatureCoeff (const std::vector< RawReading > &readings, const std::vector< double > &temperatures)
bool	calibrateAccelStatic (const std::vector< RawReading > &readings)
bool	calibrateGyroZeroVelocity (const std::vector< RawReading > &readings)
bool	calibrateGyroRotation (const std::vector< RawReading > &readings, const std::vector< double > &reference_rates)
bool	calibrateMagEllipsoidFit (const std::vector< RawReading > &readings)
double	computeCalibrationQuality () const
bool	detectDrift (const RawReading &reading) const

Private Attributes
SensorType	sensor_type_
CalibrationStatus	status_
CalibrationParams	params_
std::vector< CalibrationPoint >	calibration_data_
std::mutex	mutex_
std::vector< RawReading >	recent_readings_

Static Private Attributes
static constexpr size_t	MAX_RECENT_READINGS = 100

Detailed Description

IMU Sensor Calibration System.

Calibrates accelerometers, gyroscopes, and magnetometers using:

Bias estimation
Scale factor estimation
Misalignment correction
Temperature compensation

Definition at line 23 of file IMUCalibration.hpp.

Member Enumeration Documentation

◆ CalibrationStatus

enum class fsw::calibration::IMUCalibration::CalibrationStatus

strong

Calibration status.

Enumerator
NOT_CALIBRATED
CALIBRATING
CALIBRATED
DRIFT_DETECTED
RECALIBRATION_REQUIRED
FAILED

Definition at line 33 of file IMUCalibration.hpp.

◆ SensorType

enum class fsw::calibration::IMUCalibration::SensorType

strong

Sensor type.

Enumerator
ACCELEROMETER
GYROSCOPE
MAGNETOMETER

Definition at line 28 of file IMUCalibration.hpp.

Constructor & Destructor Documentation

◆ IMUCalibration()

fsw::calibration::IMUCalibration::IMUCalibration ( SensorType type )

Definition at line 10 of file IMUCalibration.cpp.

References params_, sensor_type_, and status_.

◆ ~IMUCalibration()

fsw::calibration::IMUCalibration::~IMUCalibration ( )

default

Member Function Documentation

◆ addCalibrationPoint()

void fsw::calibration::IMUCalibration::addCalibrationPoint ( const CalibrationPoint & point )

Add calibration data point.

Parameters

point Calibration data point

Definition at line 27 of file IMUCalibration.cpp.

References calibration_data_, lock, and mutex_.

◆ calibrate()

bool fsw::calibration::IMUCalibration::calibrate ( )

Perform calibration.

Returns: true if successful

Definition at line 32 of file IMUCalibration.cpp.

References ACCELEROMETER, calibrateAccelerometer(), CALIBRATED, calibrateGyroscope(), calibrateMagnetometer(), CALIBRATING, calibration_data_, computeCalibrationQuality(), FAILED, GYROSCOPE, lock, MAGNETOMETER, mutex_, params_, sensor_type_, status_, and success.

◆ calibrateAccelerometer()

bool fsw::calibration::IMUCalibration::calibrateAccelerometer ( const std::vector< RawReading > & readings )

Calibrate accelerometer using static positions.

Requires measurements at multiple orientations (typically 6+ positions) where gravity is the only acceleration.

Definition at line 76 of file IMUCalibration.cpp.

References calibrateAccelStatic(), and estimateBias().

Referenced by calibrate().

◆ calibrateAccelStatic()

bool fsw::calibration::IMUCalibration::calibrateAccelStatic ( const std::vector< RawReading > & readings )

private

Definition at line 185 of file IMUCalibration.cpp.

References estimateScaleAndMisalignment(), g, fsw::calibration::IMUCalibration::CalibrationPoint::raw, fsw::calibration::IMUCalibration::CalibrationPoint::reference, fsw::calibration::IMUCalibration::CalibrationPoint::reference_uncertainty, and fsw::calibration::IMUCalibration::RawReading::value.

Referenced by calibrateAccelerometer().

◆ calibrateGyroRotation()

bool fsw::calibration::IMUCalibration::calibrateGyroRotation	(	const std::vector< RawReading > &	readings,
		const std::vector< double > &	reference_rates )

private

Definition at line 233 of file IMUCalibration.cpp.

References estimateBias(), and params_.

Referenced by calibrateGyroscope().

◆ calibrateGyroscope()

bool fsw::calibration::IMUCalibration::calibrateGyroscope	(	const std::vector< RawReading > &	readings,
		const std::vector< double > &	reference_rates = {} )

Calibrate gyroscope using rotation rates.

Requires measurements during known rotations or zero-velocity periods.

Definition at line 90 of file IMUCalibration.cpp.

References calibrateGyroRotation(), and calibrateGyroZeroVelocity().

Referenced by calibrate().

◆ calibrateGyroZeroVelocity()

bool fsw::calibration::IMUCalibration::calibrateGyroZeroVelocity ( const std::vector< RawReading > & readings )

private

Definition at line 214 of file IMUCalibration.cpp.

References diff(), estimateBias(), and params_.

Referenced by calibrateGyroscope().

◆ calibrateMagEllipsoidFit()

bool fsw::calibration::IMUCalibration::calibrateMagEllipsoidFit ( const std::vector< RawReading > & readings )

private

Definition at line 256 of file IMUCalibration.cpp.

References estimateBias(), and params_.

Referenced by calibrateMagnetometer().

◆ calibrateMagnetometer()

bool fsw::calibration::IMUCalibration::calibrateMagnetometer	(	const std::vector< RawReading > &	readings,
		const Eigen::Vector3d &	reference_field )

Calibrate magnetometer using known field.

Requires measurements at multiple orientations in known magnetic field.

Definition at line 106 of file IMUCalibration.cpp.

References calibrateMagEllipsoidFit().

Referenced by calibrate().

◆ calibrateReading()

CalibratedReading fsw::calibration::IMUCalibration::calibrateReading ( const RawReading & raw ) const

Apply calibration to raw reading.

Parameters

raw	Raw sensor reading

Returns: Calibrated reading

Definition at line 118 of file IMUCalibration.cpp.

References CALIBRATED, calibrated, lock, mutex_, params_, status_, fsw::calibration::IMUCalibration::RawReading::temperature, and fsw::calibration::IMUCalibration::RawReading::value.

Referenced by computeCalibrationQuality().

◆ computeCalibrationQuality()

double fsw::calibration::IMUCalibration::computeCalibrationQuality ( ) const

private

Definition at line 315 of file IMUCalibration.cpp.

References calibrated, calibrateReading(), calibration_data_, and count().

Referenced by calibrate().

◆ detectDrift()

bool fsw::calibration::IMUCalibration::detectDrift ( const RawReading & reading ) const

private

Definition at line 343 of file IMUCalibration.cpp.

References diff(), MAGNETOMETER, r, recent_readings_, sensor_type_, and fsw::calibration::IMUCalibration::RawReading::value.

◆ estimateBias()

void fsw::calibration::IMUCalibration::estimateBias ( const std::vector< RawReading > & readings )

private

Definition at line 173 of file IMUCalibration.cpp.

References params_.

Referenced by calibrateAccelerometer(), calibrateGyroRotation(), calibrateGyroZeroVelocity(), and calibrateMagEllipsoidFit().

◆ estimateScaleAndMisalignment()

void fsw::calibration::IMUCalibration::estimateScaleAndMisalignment ( const std::vector< CalibrationPoint > & points )

private

Definition at line 276 of file IMUCalibration.cpp.

References b, and params_.

Referenced by calibrateAccelStatic().

◆ estimateTemperatureCoeff()

void fsw::calibration::IMUCalibration::estimateTemperatureCoeff	(	const std::vector< RawReading > &	readings,
		const std::vector< double > &	temperatures )

private

◆ getCalibrationParams()

IMUCalibration::CalibrationParams fsw::calibration::IMUCalibration::getCalibrationParams ( ) const

Get calibration parameters.

Definition at line 149 of file IMUCalibration.cpp.

References lock, mutex_, and params_.

◆ getStatus()

CalibrationStatus fsw::calibration::IMUCalibration::getStatus ( ) const

Get calibration status.

Definition at line 154 of file IMUCalibration.cpp.

References lock, mutex_, and status_.

Referenced by isValid().

◆ initialize()

void fsw::calibration::IMUCalibration::initialize ( const CalibrationParams & initial_params = CalibrationParams{} )

Initialize calibration.

Parameters

initial_params Initial calibration parameters (optional)

Definition at line 19 of file IMUCalibration.cpp.

References calibration_data_, lock, mutex_, NOT_CALIBRATED, params_, recent_readings_, and status_.

◆ isValid()

bool fsw::calibration::IMUCalibration::isValid ( ) const

Check if calibration is valid.

Definition at line 159 of file IMUCalibration.cpp.

References CALIBRATED, and getStatus().

◆ reset()

void fsw::calibration::IMUCalibration::reset ( )

Reset calibration.

Definition at line 163 of file IMUCalibration.cpp.

References calibration_data_, lock, mutex_, NOT_CALIBRATED, params_, recent_readings_, and status_.

Member Data Documentation

◆ calibration_data_

std::vector<CalibrationPoint> fsw::calibration::IMUCalibration::calibration_data_

private

Definition at line 182 of file IMUCalibration.hpp.

Referenced by addCalibrationPoint(), calibrate(), computeCalibrationQuality(), initialize(), and reset().

◆ MAX_RECENT_READINGS

size_t fsw::calibration::IMUCalibration::MAX_RECENT_READINGS = 100

staticconstexprprivate

Definition at line 187 of file IMUCalibration.hpp.

◆ mutex_

std::mutex fsw::calibration::IMUCalibration::mutex_

mutableprivate

Definition at line 183 of file IMUCalibration.hpp.

Referenced by addCalibrationPoint(), calibrate(), calibrateReading(), getCalibrationParams(), getStatus(), initialize(), and reset().

◆ params_

CalibrationParams fsw::calibration::IMUCalibration::params_

private

Definition at line 181 of file IMUCalibration.hpp.

Referenced by calibrate(), calibrateGyroRotation(), calibrateGyroZeroVelocity(), calibrateMagEllipsoidFit(), calibrateReading(), estimateBias(), estimateScaleAndMisalignment(), getCalibrationParams(), IMUCalibration(), initialize(), and reset().

◆ recent_readings_

std::vector<RawReading> fsw::calibration::IMUCalibration::recent_readings_

private

Definition at line 186 of file IMUCalibration.hpp.

Referenced by detectDrift(), initialize(), and reset().

◆ sensor_type_

SensorType fsw::calibration::IMUCalibration::sensor_type_

private

Definition at line 179 of file IMUCalibration.hpp.

Referenced by calibrate(), detectDrift(), and IMUCalibration().

◆ status_

CalibrationStatus fsw::calibration::IMUCalibration::status_

private

Definition at line 180 of file IMUCalibration.hpp.

Referenced by calibrate(), calibrateReading(), getStatus(), IMUCalibration(), initialize(), and reset().

The documentation for this class was generated from the following files:

daq-server/diablo_server/lib/include/calibration/IMUCalibration.hpp
daq-server/diablo_server/lib/src/calibration/IMUCalibration.cpp

Classes

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Enumeration Documentation

◆ CalibrationStatus

◆ SensorType

Constructor & Destructor Documentation

◆ IMUCalibration()

◆ ~IMUCalibration()

Member Function Documentation

◆ addCalibrationPoint()

◆ calibrate()

◆ calibrateAccelerometer()

◆ calibrateAccelStatic()

◆ calibrateGyroRotation()

◆ calibrateGyroscope()

◆ calibrateGyroZeroVelocity()

◆ calibrateMagEllipsoidFit()

◆ calibrateMagnetometer()

◆ calibrateReading()

◆ computeCalibrationQuality()

◆ detectDrift()

◆ estimateBias()

◆ estimateScaleAndMisalignment()

◆ estimateTemperatureCoeff()

◆ getCalibrationParams()

◆ getStatus()

◆ initialize()

◆ isValid()

◆ reset()

Member Data Documentation

◆ calibration_data_

◆ MAX_RECENT_READINGS

◆ mutex_

◆ params_

◆ recent_readings_

◆ sensor_type_

◆ status_