#include <Ikf.hpp>

Public Member Functions
	~Ikf ()

void	Init (const Eigen::Matrix< _Scalar, Ikf::IKFSTATEVECTORSIZE, Ikf::IKFSTATEVECTORSIZE > &P_0, const Eigen::Matrix< _Scalar, 3, 3 > &Ra, const Eigen::Matrix< _Scalar, 3, 3 > &Rg, const Eigen::Matrix< _Scalar, 3, 3 > &Rm, const Eigen::Matrix< _Scalar, 3, 3 > &Ri, const Eigen::Matrix< _Scalar, 3, 3 > &Qbg, const Eigen::Matrix< _Scalar, 3, 3 > &Qba, const Eigen::Matrix< _Scalar, 3, 3 > &Qbi, double g, double alpha, unsigned int am1, unsigned int am2, double agamma, unsigned int im1, unsigned int im2, double igamma)
	This function Initialize the vectors and matrix of the IKF. More...

void	predict (Eigen::Matrix< _Scalar, 3, 1 > &u, double dt)
	Performs the prediction step of the filter. More...

void	update (const Eigen::Matrix< _Scalar, 3, 1 > &acc, bool acc_on)

void	update (const Eigen::Matrix< _Scalar, 3, 1 > &acc, bool acc_on, const Eigen::Matrix< _Scalar, 3, 1 > &measurement, bool measurement_on)

void	update (const Eigen::Matrix< _Scalar, 3, 1 > &measurement1, const Eigen::Matrix< _Scalar, 3, 1 > &measurement2)

void	update (const Eigen::Matrix< _Scalar, 3, 1 > &acc, bool acc_on, const Eigen::Matrix< _Scalar, 3, 1 > &incl, bool incl_on, const Eigen::Matrix< _Scalar, 3, 1 > &mag, bool magn_on)
	Performs the measurement and correction steps of the filter. More...

bool	setAttitude (const Eigen::Quaternion< double > &initq)
	This function Initialize Attitude. More...

void	setState (const Eigen::Matrix< double, IKFSTATEVECTORSIZE, 1 > &x_0)
	This function Initialize the State vector. More...

bool	setOmega (const Eigen::Matrix< _Scalar, 3, 1 > &u)
	This function set the initial Omega matrix. More...

void	setInitBias (const Eigen::Matrix< _Scalar, 3, 1 > &gbias, const Eigen::Matrix< _Scalar, 3, 1 > &abias, const Eigen::Matrix< _Scalar, 3, 1 > &ibias)
	On/Off initial bias. More...

void	setGravity (const double gravity)
	Initial gravity. More...

void	setCovariance (const Eigen::Matrix< double, Ikf::IKFSTATEVECTORSIZE, Ikf::IKFSTATEVECTORSIZE > &Pk)
	Set the filter covariance. More...

Eigen::Matrix< double, 3, 1 >	getGyroBias ()
	Gets the current gyroscopes bias. More...

Eigen::Matrix< double, 3, 1 >	getAccBias ()
	Gets the current accelerometers bias. More...

Eigen::Matrix< double, 3, 1 >	getInclBias ()
	Gets the current inclinometers bias. More...

Eigen::Matrix< double, IKFSTATEVECTORSIZE, 1 >	getState ()
	Gets the current state vector of the filter. More...

Eigen::Matrix< _Scalar, 3, 1 >	getGravityinBody ()
	Gets gravity in the IMU body frame. More...

Eigen::Matrix< _Scalar, 3, 1 >	getGravity () const
	Gets gravity in the local Geographic frame. More...

Eigen::Quaternion< double >	getAttitude ()
	Gets the current orientation in Quaternion. More...

Eigen::Matrix< double, IKFSTATEVECTORSIZE, IKFSTATEVECTORSIZE >	getCovariance ()
	Gets Noise covariance matrix. More...

void	Quaternion2DCM (Eigen::Quaternion< _Scalar > q, Eigen::Matrix< _Scalar, 3, 3 > C)
	Conversion Quaternion to DCM (Direct Cosine Matrix) (Alternative to Eigen) More...

Static Public Member Functions
template<typename _MatrixType >
static _MatrixType	guaranteeSPD (const _MatrixType &A)
	Method to guarantee Semi-Positive Definite (SPD) matrix. More...

Static Public Attributes
static const int	flagAcc = static_cast<int>(_Accelerometers)

static const int	flagIncl = static_cast<int>(_Inclinometers)

static const unsigned int	QUATERNION_SIZE = 4

static const unsigned int	IKFSTATEVECTORSIZE = 6 + (flagAcc3) + (flagIncl3)

Protected Member Functions
void	initAdaptiveAttitude (const unsigned int accM1, const unsigned int accM2, const double accGamma, const unsigned int incM1, const unsigned int incM2, const double incGamma)

Constructor & Destructor Documentation

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::~Ikf ( )

inline

Default Destructor

Member Function Documentation

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<double, 3, 1> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getAccBias ( )

inline

Gets the current accelerometers bias.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Quaternion<double> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getAttitude ( )

inline

Gets the current orientation in Quaternion.

Author: Javier Hidalgo Carrio.

Returns: Quaternion with the current orientation.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<double,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getCovariance ( )

inline

Gets Noise covariance matrix.

Author: Javier Hidalgo Carrio.

Returns: Matrix P of the covariance of the state vector

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<_Scalar, 3, 1> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getGravity ( ) const

inline

Gets gravity in the local Geographic frame.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<_Scalar, 3, 1> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getGravityinBody ( )

inline

Gets gravity in the IMU body frame.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<double, 3, 1> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getGyroBias ( )

inline

Gets the current gyroscopes bias.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<double, 3, 1> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getInclBias ( )

inline

Gets the current inclinometers bias.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

Eigen::Matrix<double,IKFSTATEVECTORSIZE,1> filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::getState ( )

inline

Gets the current state vector of the filter.

Author: Javier Hidalgo Carrio.

Returns: State Vector

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

template<typename _MatrixType >

static _MatrixType filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::guaranteeSPD ( const _MatrixType & A )

inlinestatic

Method to guarantee Semi-Positive Definite (SPD) matrix.

Single Value Decomposition

eigenvalues

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::Init	(	const Eigen::Matrix< _Scalar, Ikf< _Scalar, _Accelerometers, _Inclinometers >::IKFSTATEVECTORSIZE, Ikf< _Scalar, _Accelerometers, _Inclinometers >::IKFSTATEVECTORSIZE > &	P_0,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Ra,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Rg,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Rm,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Ri,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Qbg,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Qba,
		const Eigen::Matrix< _Scalar, 3, 3 > &	Qbi,
		double	g,
		double	alpha,
		unsigned int	am1,
		unsigned int	am2,
		double	agamma,
		unsigned int	im1,
		unsigned int	im2,
		double	igamma
	)

inline

This function Initialize the vectors and matrix of the IKF.

This method receives the measurement noise matrix of the sensors The theoretical gravity value and the Dip angle of the location.

Author: Javier Hidalgo Carrio.

Parameters

[in]	P_0	Initial state covariance matrix
[in]	Ra	measurement noise matrix of Accelerometers.
[in]	Rg	measurement noise matrix of Gyroscopes.
[in]	Rm	measurement noise matrix of Magnetometers.
[in]	Qbg	covariance noise matrix of the gyroscopes bias
[in]	Qba	covariance noise matrix of the accelerometers bias
[in]	g	local gravitational value.
[in]	alpha	Dip angle

Returns: void

Gravitation acceleration

Dip angle (alpha is in rad)

Kalman filter state, error covariance and process noise covariance

Initial error covariance

Static part of the observation matrices

System matrix A

Initial bias

Default omega matrix

Initial quaternion in Init

Fill matrix Rg, Ra, Rm and Ri

Initialize adaptive object

Print filter information

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::initAdaptiveAttitude	(	const unsigned int	accM1,
		const unsigned int	accM2,
		const double	accGamma,
		const unsigned int	incM1,
		const unsigned int	incM2,
		const double	incGamma
	)

inlineprotected

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::predict	(	Eigen::Matrix< _Scalar, 3, 1 > &	u,
		double	dt
	)

inline

Performs the prediction step of the filter.

It computes the discrete version of the matrix A to propagate forward the state vector x. It computes the Q and P matrix as well as the quaternion integration from the input vector u and the delta time.

Author: Javier Hidalgo Carrio.

Parameters

[in]	u	vector with the angular velocity
[in]	dt	delta time between samples

Returns: void

< Vector 2 product matrix

< Vec 2 product matrix

< Quaternion integration matrix

< Discrete System matrix

< Discrete Q matrix

Compute the vector2product matrix with the angular velocity

Eliminate the Bias

Compute the dA Matrix

Propagate the vector through the system

Third-order gyroscopes integration accuracy

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::Quaternion2DCM	(	Eigen::Quaternion< _Scalar > *	q,
		Eigen::Matrix< _Scalar, 3, 3 > *	C
	)

inline

Conversion Quaternion to DCM (Direct Cosine Matrix) (Alternative to Eigen)

Conversion to a transformation matrix from a quaternion The quaternion is represented in Eigen convention: w+xi+yj+zk, first element the scalar and others three are the vectorial part.

Author: Javier Hidalgo Carrio.

Parameters

[in]	*q	pointer to a quaternion vector.
[out]	*C	pointer to a matrix. The three by three matrix

Returns: void

Take the parameters of the quaternion

Create the DCM matrix from the actual quaternion

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

bool filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::setAttitude ( const Eigen::Quaternion< double > & initq )

inline

This function Initialize Attitude.

Initial orientation value beforeestart the IKF

Author: Javier Hidalgo Carrio.

Parameters

[in] *initq pointer to quaternion with the initial orientation

Returns: true if everything all right. false on other cases.

Initial orientation

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::setCovariance ( const Eigen::Matrix< double, Ikf< _Scalar, _Accelerometers, _Inclinometers >::IKFSTATEVECTORSIZE, Ikf< _Scalar, _Accelerometers, _Inclinometers >::IKFSTATEVECTORSIZE > & Pk )

inline

Set the filter covariance.

Parameters

[in] Pk covariance matrix of appropriate dimension

Returns: void.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::setGravity ( const double gravity )

inline

Initial gravity.

Set initial gravity. If after initialization a new theoretical or measured gravity is available use this method. Note: always before start running the filter.

Parameters

[in] gravity initial gravity value

Returns: void.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::setInitBias	(	const Eigen::Matrix< _Scalar, 3, 1 > &	gbias,
		const Eigen::Matrix< _Scalar, 3, 1 > &	abias,
		const Eigen::Matrix< _Scalar, 3, 1 > &	ibias
	)

inline

On/Off initial bias.

Initial On/Off sensor bias. Otherwise the default in Init is set them to zero.

Parameters

[in]	gbias	vector with initial gyroscopes bias
[in]	abias	vector with initial accelerometers bias

Returns: void.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

bool filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::setOmega ( const Eigen::Matrix< _Scalar, 3, 1 > & u )

inline

This function set the initial Omega matrix.

Initial Omega matrix with angular velocity for quaternion integration.

Author: Javier Hidalgo Carrio.

Parameters

[in] *u pointer to vector with the angular velocity

Returns: true if everything all right. false on other cases.

Initialization for quaternion integration

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::setState ( const Eigen::Matrix< double, IKFSTATEVECTORSIZE, 1 > & x_0 )

inline

This function Initialize the State vector.

The state vector is formed by 9 element. (0-2) -> the vector part of a error quaternion (3-5) -> gyroscope bias estimation (6-8) -> accelerometer bias estimation

Parameters

[in] *x_0 a initial/desired state vector

Returns: OK is everything all right. ERROR on other cases.

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::update	(	const Eigen::Matrix< _Scalar, 3, 1 > &	acc,
		bool	acc_on
	)

inline

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::update	(	const Eigen::Matrix< _Scalar, 3, 1 > &	acc,
		bool	acc_on,
		const Eigen::Matrix< _Scalar, 3, 1 > &	measurement,
		bool	measurement_on
	)

inline

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::update	(	const Eigen::Matrix< _Scalar, 3, 1 > &	measurement1,
		const Eigen::Matrix< _Scalar, 3, 1 > &	measurement2
	)

inline

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

void filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::update	(	const Eigen::Matrix< _Scalar, 3, 1 > &	acc,
		bool	acc_on,
		const Eigen::Matrix< _Scalar, 3, 1 > &	incl,
		bool	incl_on,
		const Eigen::Matrix< _Scalar, 3, 1 > &	mag,
		bool	magn_on
	)

inline

Performs the measurement and correction steps of the filter.

The IKf is based on two measurement step:\

Measurement step to correct Pitch and Roll from accelerometers.
Measurement step to correct Pitch and Roll from inclinometers.
Measurement step to correct Yaw angle from magnetometers.

The first measurement step is dynamics. The noise covariamce matrix of the update is dynamic depending on external accelerations felt on the accelerometers. That means the variance noise increase or decrease depending on the external acceleration. Thas is the main different between normal EKF.

The second measurement step only affects the Yaw (heading) angle.

Author: Javier Hidalgo Carrio.

Parameters

[in]	acc	pointer to vector with accelerations
[in]	magn	pointer to vector with magnetometers
[in]	magn_on	boolean value to connect or disconnect the magnetometers correction

Returns: void

< Vector 2 product matrix

< Measurement noise matrix from accelerometers matrix Ra

< Error covariance matrix for measurement 1

< Error covariance matrix for measurement 2

< Error covariance matrix for measurement 3

< Auxiliar matrix for computing Kalman gain in measurement

< Kalman Gain matrix for measurement 1

< Kalman Gain matrix for measurement 2

< Kalman Gain matrix for measurement 3

< Acceleration covariance matrix

< Inclinometers covariance matrix

< Attitude error quaternion

< Gravitation in the body frame

< Magnetic field in the body frame

< Measurement vector 1 Acc

< Measurement vector 2 Mag

< Measurement vector 3 Incl

< Auxiliar vector variable

Measurement step 1 Acc

First measurement step (Pitch and Roll correction from Acc)

Measurement

The adaptive algorithm

Compute the Kalman Gain Matrix

Update the state vector and the covariance matrix

Update the quaternion with the Indirect approach

This is necessary mainly because after(in the 2 measurement) C(q) is computed

Normalize quaternion

Reset the quaternion part of the state vector

Measurement step 2 Mag

It only updates Yaw angle

Second measurement step

Observation matrix

Measurement vector

Compute Kalman Gain

Update the state vector and the covariance matrix

Update the quaternion with the Indirect approach

Normalize quaternion

Reset the quaternion part of the state vector

Measurement step 3 Inclinometers

Measurement step (Pitch and Roll correction from Inclinometers)

Measurement

The adaptive algorithm

Compute the Kalman Gain Matrix

Update the state vector and the covariance matrix

Update the quaternion with the Indirect approach

This is necessary mainly because after(in the 2 measurement) C(q) is computed

Normalize quaternion

Reset the quaternion part of the state vector

Reset the rest of the state

Guarantee SPD Covariance matrix

Member Data Documentation

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

const int filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::flagAcc = static_cast<int>(_Accelerometers)

static

Convert bool to int is standard in C++

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

const int filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::flagIncl = static_cast<int>(_Inclinometers)

static

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

const unsigned int filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::IKFSTATEVECTORSIZE = 6 + (flagAcc*3) + (flagIncl*3)

static

template<typename _Scalar , bool _Accelerometers, bool _Inclinometers>

const unsigned int filter::Ikf< _Scalar, _Accelerometers, _Inclinometers >::QUATERNION_SIZE = 4

static

Constants definitions

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

/build/rock-master-18.09-slam-quater-ikf-0.20150930/src/Ikf.hpp

Public Member Functions

Static Public Member Functions

Static Public Attributes

Protected Member Functions

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation