#include <imu_ekf.h>

Public Types
typedef Eigen::Matrix< double, 12, 12 >	ProcessNoiseMatrix
Public Member Functions
void	correction (const Observation &observation)
	Correction step of the Extended Kalman Filter.
	ImuEkf ()
	Constructor Default Constructor.
void	initialize (const Eigen::Quaterniond &rot, const Eigen::Vector3d &pos, const Eigen::Quaterniond &calibRot, const Eigen::Vector3d &calibPos, const Eigen::Vector3d &measuredAcceleration, const ImuState::CovarianceMatrix &covMat, bool constrainGravity)
	Initialize the system.
void	objectTransformation (Eigen::Quaterniond &rot, Eigen::Vector3d &trans, Eigen::Vector3d &grav)
	Receive the target frame estimate.
void	propagate (const ImuMeasurement &measurement, double deltaT)
	Propagation of the IMU state using measurements from the IMU.
void	setEstimateAccBias (bool v)
	whether to include the accelerometer bias into the state estimation process
void	setEstimateGravity (bool v)
	whether to include the gravity into the state estimation process
void	setEstimateGyroBias (bool v)
	whether to include the gyro bias into the state estimation process
void	setExtrinsicTransform (const Eigen::Quaterniond &rot, const Eigen::Vector3d &trans)
	Set the extrinsic transformations This sets the transformation from the target frame to the IMU. Note: you only need to do this, if the IMU is not your target frame!
void	setProcessNoise (const ProcessNoiseMatrix &noiseMat)
	setter for the process noise
void	setStateCovariance (const ImuState::CovarianceMatrix &covMat)
	setter for the state covariance
void	setStateCovariance (const ImuState::StateVector &diag)
	setter for the state covariance
void	setVelocityDamping (double v)
	this sets a damping factor for the velocity propagation
const ImuState &	state () const
	access the current state of the filter
ImuState &	state ()
	~ImuEkf ()
	Destructor.
Private Member Functions
void	noiseJacobian (const ImuState &state, const ImuMeasurement &meas, double deltaT, double step, Eigen::Matrix< double, 19, 12 > &G)
void	normalizeOrientationObservation (Eigen::Quaterniond &q)
void	numericalJacobians (const ImuState &state, const ImuMeasurement &measurement, double deltaT, Eigen::Matrix< double, 19, 19 > &F, Eigen::Matrix< double, 19, 12 > &G)
void	numericalJacobians2 (const ImuState &state, const ImuMeasurement &measurement, double deltaT, Eigen::Matrix< double, 19, 19 > &F, Eigen::Matrix< double, 19, 12 > &G)
void	propagateCovariance ()
void	propagateOrientation (Eigen::Quaterniond &qnew, const ImuState &state, const ImuMeasurement &measurement, double deltaT)
void	propagateState (const ImuState &oldState, const ImuMeasurement &meas, double deltaT, ImuState &newState)
void	propagateTranslation (Eigen::Vector3d &tnew, const Eigen::Quaterniond &rot, const ImuState &state, const ImuMeasurement &measurement, double deltaT)
void	propagateVelocity (Eigen::Vector3d &vnew, const Eigen::Quaterniond &rot, const ImuState &state, const ImuMeasurement &measurement, double deltaT)
void	stateJacobian (const ImuState &state, const ImuMeasurement &meas, double deltaT, double step, Eigen::Matrix< double, 19, 19 > &F)
void	updatePerturbationJacobian (const Eigen::Quaterniond &d, double deltaT)
void	updateProcessNoise (const ImuMeasurement &measurment, double deltaT)
Private Attributes
ImuState::CovarianceMatrix	covarianceUpdate_
Eigen::Matrix< double, ImuState::Dimension, ImuState::Dimension >	F_
Eigen::Matrix< double, ImuState::Dimension, 12 >	G_
Eigen::Matrix< double, ImuState::Dimension, 7 >	kalmanGain_
Observation::Vector	measResidual_
ProcessNoiseMatrix	Q_
cv::RNG	rng_
ImuState	state_
ImuState::StateVector	stateUpdate_
double	velocity_damping_

Detailed Description

This class does filtering of IMU measurements using an Extended Kalman Filter Prediction step: propagate the state using acceleration and angular rate measurements Correction step: use any kind of external information to correct the IMU state with an absolute observation of the state

Definition at line 60 of file imu_ekf.h.

Member Typedef Documentation

typedef Eigen::Matrix<double, 12, 12> imu_filter::ImuEkf::ProcessNoiseMatrix

Definition at line 63 of file imu_ekf.h.

Constructor & Destructor Documentation

imu_filter::ImuEkf::ImuEkf ( )

Constructor Default Constructor.

Definition at line 47 of file imu_ekf.cpp.

imu_filter::ImuEkf::~ImuEkf ( )

Destructor.

Definition at line 58 of file imu_ekf.cpp.

Member Function Documentation

void imu_filter::ImuEkf::correction ( const Observation & observation )

Correction step of the Extended Kalman Filter.

This function performs the standard correction equations of an EKF using an Observation from another source.

Parameters:

observation The observation containing the neccessary information to do the correction step

Definition at line 106 of file imu_ekf.cpp.

void imu_filter::ImuEkf::initialize	(	const Eigen::Quaterniond &	rot,
		const Eigen::Vector3d &	pos,
		const Eigen::Quaterniond &	calibRot,
		const Eigen::Vector3d &	calibPos,
		const Eigen::Vector3d &	measuredAcceleration,
		const ImuState::CovarianceMatrix &	covMat,
		bool	constrainGravity
	)

Initialize the system.

Initialize the filter

Parameters:

rot	Initial rotation in world
pos	Initial position in world
calibRot	calibration rotation from target frame to IMU (if the IMU frame is not the target frame)
calibPos	calibration translation from target frame to IMU (if the IMU frame is not the target frame)
measuredAcceleration	initially measured acceleration (used to determine the initial direction of the gravity vector)
covMat	initial covariance
constrainGravity	wheather to constrain the norm of the gravity in the beginning, or to just use the meas. acceleration

Definition at line 146 of file imu_ekf.cpp.

void imu_filter::ImuEkf::noiseJacobian	(	const ImuState &	state,
		const ImuMeasurement &	meas,
		double	deltaT,
		double	step,
		Eigen::Matrix< double, 19, 12 > &	G
	)		`[private]`

Definition at line 412 of file imu_ekf.cpp.

void imu_filter::ImuEkf::normalizeOrientationObservation ( Eigen::Quaterniond & q ) [inline, private]

Definition at line 268 of file imu_ekf.h.

void imu_filter::ImuEkf::numericalJacobians	(	const ImuState &	state,
		const ImuMeasurement &	measurement,
		double	deltaT,
		Eigen::Matrix< double, 19, 19 > &	F,
		Eigen::Matrix< double, 19, 12 > &	G
	)		`[private]`

Definition at line 508 of file imu_ekf.cpp.

void imu_filter::ImuEkf::numericalJacobians2	(	const ImuState &	state,
		const ImuMeasurement &	measurement,
		double	deltaT,
		Eigen::Matrix< double, 19, 19 > &	F,
		Eigen::Matrix< double, 19, 12 > &	G
	)		`[private]`

void imu_filter::ImuEkf::objectTransformation	(	Eigen::Quaterniond &	rot,
		Eigen::Vector3d &	trans,
		Eigen::Vector3d &	grav
	)

Receive the target frame estimate.

Get the current target frame transformation. This will remove the extrinsics again

Parameters:

rot	current rotation
trans	current translation
grav	current gravity vector estimate

Definition at line 192 of file imu_ekf.cpp.

void imu_filter::ImuEkf::propagate	(	const ImuMeasurement &	measurement,
		double	deltaT
	)

Propagation of the IMU state using measurements from the IMU.

The propagation proceeds as follows:

orientation: q+ = q ** q(angular rate * deltaT)

update the position: p+ = v- * deltaT + 1/2 a_w * t^2

velocity: v+ = v + a_w * deltaT
- gyro bias: b_g+ = b_g + b_g_perturbation
- acceleration bias: b_a+ = b_a + b_a_perturbation
- gravity: g+ = g

Parameters:

measurement	ImuMeasurement from the IMU
deltaT	Time step in seconds

Definition at line 62 of file imu_ekf.cpp.

void imu_filter::ImuEkf::propagateCovariance ( ) [private]

Definition at line 101 of file imu_ekf.cpp.

void imu_filter::ImuEkf::propagateOrientation	(	Eigen::Quaterniond &	qnew,
		const ImuState &	state,
		const ImuMeasurement &	measurement,
		double	deltaT
	)		`[private]`

Definition at line 204 of file imu_ekf.cpp.

void imu_filter::ImuEkf::propagateState	(	const ImuState &	oldState,
		const ImuMeasurement &	meas,
		double	deltaT,
		ImuState &	newState
	)		`[private]`

Definition at line 253 of file imu_ekf.cpp.

void imu_filter::ImuEkf::propagateTranslation	(	Eigen::Vector3d &	tnew,
		const Eigen::Quaterniond &	rot,
		const ImuState &	state,
		const ImuMeasurement &	measurement,
		double	deltaT
	)		`[private]`

Definition at line 216 of file imu_ekf.cpp.

void imu_filter::ImuEkf::propagateVelocity	(	Eigen::Vector3d &	vnew,
		const Eigen::Quaterniond &	rot,
		const ImuState &	state,
		const ImuMeasurement &	measurement,
		double	deltaT
	)		`[private]`

Definition at line 234 of file imu_ekf.cpp.

void imu_filter::ImuEkf::setEstimateAccBias ( bool v ) [inline]

whether to include the accelerometer bias into the state estimation process

Parameters:

v	flag whether to do or not

Definition at line 184 of file imu_ekf.h.

void imu_filter::ImuEkf::setEstimateGravity ( bool v ) [inline]

whether to include the gravity into the state estimation process

Parameters:

v	flag whether to do or not

Definition at line 172 of file imu_ekf.h.

void imu_filter::ImuEkf::setEstimateGyroBias ( bool v ) [inline]

whether to include the gyro bias into the state estimation process

Parameters:

v	flag whether to do or not

Definition at line 178 of file imu_ekf.h.

void imu_filter::ImuEkf::setExtrinsicTransform	(	const Eigen::Quaterniond &	rot,
		const Eigen::Vector3d &	trans
	)

Set the extrinsic transformations This sets the transformation from the target frame to the IMU. Note: you only need to do this, if the IMU is not your target frame!

Parameters:

rot	Rotation from the target frame to the IMU frame
trans	Translation from the target frame to the IMU frame

Definition at line 185 of file imu_ekf.cpp.

void imu_filter::ImuEkf::setProcessNoise ( const ProcessNoiseMatrix & noiseMat ) [inline]

setter for the process noise

Process Noise Q is used in the propagation and related to the noise levels of the IMU measurements

Parameters:

noiseMat Process Noise Matrix

Definition at line 147 of file imu_ekf.h.

void imu_filter::ImuEkf::setStateCovariance ( const ImuState::CovarianceMatrix & covMat ) [inline]

setter for the state covariance

Parameters:

covmat the state covariance matrix

Definition at line 133 of file imu_ekf.h.

void imu_filter::ImuEkf::setStateCovariance ( const ImuState::StateVector & diag ) [inline]

setter for the state covariance

Parameters:

covmat the state covariance matrix as diagonal vector

Definition at line 139 of file imu_ekf.h.

void imu_filter::ImuEkf::setVelocityDamping ( double v ) [inline]

this sets a damping factor for the velocity propagation

Parameters:

v	damping factor

Definition at line 190 of file imu_ekf.h.

const ImuState& imu_filter::ImuEkf::state ( ) const [inline]

access the current state of the filter

Returns:: reference to the current state

Definition at line 106 of file imu_ekf.h.

ImuState& imu_filter::ImuEkf::state ( ) [inline]

Definition at line 107 of file imu_ekf.h.

void imu_filter::ImuEkf::stateJacobian	(	const ImuState &	state,
		const ImuMeasurement &	meas,
		double	deltaT,
		double	step,
		Eigen::Matrix< double, 19, 19 > &	F
	)		`[private]`