#include <ukf.hpp>

Public Types
enum	DECLINATION_CONSTS { EAST = 1, WEST = 2 }

Public Member Functions
Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, 1 >	getState ()
	Gets the current state vector of the filter. More...

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

Eigen::Matrix< double, ukf::NUMAXIS, 1 >	getEuler ()
	Gets the current orientation in Euler angles. More...

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

bool	setAttitude (Eigen::Quaternion< double > *initq)
	This function Initilize Attitude. More...

void	Init (Matrix< double, ukf::UKFSTATEVECTORSIZE, 1 > x_0, Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE > P_0, Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE > Q, Eigen::Matrix< double, ukf::NUMAXIS, ukf::NUMAXIS > R, Eigen::Quaternion< double > *at_q, double a, double f, double lambda, double g)
	This function initilize the filter. More...

double	GravityModel (double latitude, double altitude)
	This computes the theoretical gravity value according to the WGS-84 ellipsoid earth model. More...

void	SubstractEarthRotation (Eigen::Matrix< double, ukf::NUMAXIS, 1 > u, Eigen::Quaternion< double > qb_g, double latitude)
	Substract the Earth rotation from the gyroscopes readout. More...

void	Omega (Eigen::Quaternion< double > quat, Eigen::Matrix< double, ukf::NUMAXIS, 1 > angvelo, double dt)
	Discrete-time quaternion kinematic equation. More...

void	predict (Eigen::Matrix< double, ukf::NUMAXIS, 1 > *u, double dt)
	Performs the prediction step of the filter. More...

void	update (Eigen::Matrix< double, ukf::NUMAXIS, 1 > acc, Eigen::Matrix< double, ukf::NUMAXIS, 1 > mag)
	Performs the measurement and correction steps of the filter. More...

void	attitudeUpdate ()
	Performs attitude quaternion update. More...

Static Public Attributes
static const int	UKFSTATEVECTORSIZE = 6

static const int	QUATERSIZE = 4

static const int	SIGPOINTSIZE = (2*ukf::UKFSTATEVECTORSIZE) + 1

static const int	NUMAXIS = 3

Member Enumeration Documentation

◆ DECLINATION_CONSTS

enum filter::ukf::DECLINATION_CONSTS

Number of axis sensed by the sensors UKF constant parameters

Enumerator
EAST
WEST	EAST is 1 and means positive magnetic declination

Member Function Documentation

◆ attitudeUpdate()

void filter::ukf::attitudeUpdate ( )

Performs attitude quaternion update.

Performs the update step of the filter. Only update the attitude quaternion from a given rotation in rodrigues parameter form.

It only performs the update of the attitude quaternion from the state vector (x) After the ccomputation set x to zero for next propagation (only the Rodriguez parameters part of x)

Author: Javier Hidalgo Carrio.

Returns: void

< scalar part of a quaternion

< vectorial part of a quaternion

Update the attitude quaternion using the state vector (rodrigues parameter)

Set to zero for the next propagation

◆ getAttitude()

Eigen::Quaternion< double > filter::ukf::getAttitude ( )

Gets the current orientation in Quaternion.

Author: Javier Hidalgo Carrio.

Returns: Quaternion with the current orientation.

◆ getCovariance()

Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE > filter::ukf::getCovariance ( )

Gets Noise covariance matrix.

Author: Javier Hidalgo Carrio.

Returns: Matrix P of the covariance of the state vector

◆ getEuler()

Eigen::Matrix< double, ukf::NUMAXIS, 1 > filter::ukf::getEuler ( )

Gets the current orientation in Euler angles.

Gets the current orientation in Euler angles (rad)

Author: Javier Hidalgo Carrio.

Returns: Current orientation in Euler angles.

◆ getState()

Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, 1 > filter::ukf::getState ( )

Gets the current state vector of the filter.

Author: Javier Hidalgo Carrio.

Returns: State Vector

◆ GravityModel()

double filter::ukf::GravityModel	(	double	latitude,
		double	altitude
	)

This computes the theoretical gravity value according to the WGS-84 ellipsoid earth model.

Author: Javier Hidalgo Carrio.

Parameters

[in]	latitude	double the latitude value in radian
[in]	altitude	double with the altitude value in meters

Returns: double. the theoretical value of the local gravity

< g magnitude at zero altitude

Nominal Gravity model

Gravity affects by the altitude (aprox the value r = Re

◆ Init()

void filter::ukf::Init	(	Matrix< double, ukf::UKFSTATEVECTORSIZE, 1 > *	x_0,
		Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE > *	P_0,
		Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE > *	Q,
		Eigen::Matrix< double, ukf::NUMAXIS, ukf::NUMAXIS > *	R,
		Eigen::Quaternion< double > *	at_q,
		double	a,
		double	f,
		double	lambda,
		double	g
	)

This function initilize the filter.

This function Initilize the vectors and matrix of the UKF.

This method receives the measurement noise matrix of the sensors and filter parameters

Author: Javier Hidalgo Carrio.

Parameters

[in]	*x_0	initial state vector
[in]	*P_0	initial convariance matrix of the process.
[in]	*Q	noise covariance matrix of the model
[in]	*R	noise covariance matrix of the measurement
[in]	*at_q	initial attitude quaternion
[in]	*a	parameter for the UKF
[in]	*f	parameter for the UKF\
[in]	*lambda	parameter for the UKF, to define the distance of the sigma point with respect to the mean
[in]	g	local gravity value

Returns: void

Indirect Kalman Filter methods

Gravitation acceleration

Set the parameters

Set the state vector

Set the matrices

Set the initial attitude

Print values

◆ Omega()

void filter::ukf::Omega	(	Eigen::Quaternion< double > *	quat,
		Eigen::Matrix< double, ukf::NUMAXIS, 1 > *	angvelo,
		double	dt
	)

Discrete-time quaternion kinematic equation.

It computes the quaternion kinematics from angular velocity. It is the quaternions integration (discrete version)

Author: Javier Hidalgo Carrio.

Parameters

[in,out]	*quat	the quaternion to propagate
[in]	*angvelo	pointer to vector with the angular velocity
[in]	dt	delta time between samples

Returns: void

If angular velocity is not zero

Copy the quaternion

Psi vector calculation

Create the cross-product matrix from the angular velocity

Upper matrix

Create the omega transition matrix

Columns

Rows

Propagate forward in time, y = (A) x + y

Store the update quaternion in the argument quaternion

◆ predict()

void filter::ukf::predict	(	Eigen::Matrix< double, ukf::NUMAXIS, 1 > *	u,
		double	dt
	)

Performs the prediction step of the filter.

It computes the sigma point, the error quaternions, the sigma point quaternion form the previous error quaternions and the propagation step of the filter

Author: Javier Hidalgo Carrio.

Parameters

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

Returns: void

< scalar part of a quaternion

< vectorial part of a quaternion

< Auxiliar quaternion for operations

< Vector containing the rodrigues parameters , upper part of the state vector and the sigma points

< Vector of corrected angular velocity

< Summation variable

< Summation matrix

Compute the sigma points

Calculate Error Quaternion

Calculate

Store in the quaternion

Note the order of the arguments in a quaternion: the real w coefficient first, while internally the coefficients are stored in the following order: [x, y, z, w]

Compute sigma point quaternions from error quaternions

Propagate quaternions forward (sigma point quaternions) u is the vector with the angular velocities

The estimated angular velocities are given by substracting the bias

Attitude quaternion dynamic matrix Omega

Store the inverse value of the quaternion when i = 0

Propagate error quaternions

Compute the sigma points

Vectorial part of a quaternion

Predicted mean

Predicted covariance

◆ setAttitude()

bool filter::ukf::setAttitude ( Eigen::Quaternion< double > * initq )

This function Initilize Attitude.

Initial orientation value beforeestart the IKF

Author: Javier Hidalgo Carrio.

Parameters

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

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

Initial orientation

◆ SubstractEarthRotation()

void filter::ukf::SubstractEarthRotation	(	Eigen::Matrix< double, ukf::NUMAXIS, 1 > *	u,
		Eigen::Quaternion< double > *	qb_g,
		double	latitude
	)

Substract the Earth rotation from the gyroscopes readout.

This function computes the substraction of the rotation of the Earth (EARTHW) from the gyroscope values. This function uses quaternion of transformation from the body to the geographic frame and the latitude in radians.

Author: Javier Hidalgo Carrio.

Parameters

[in,out]	*u	pointer to angular velocity
[in]	*qb_g	quaternion from body frame to geographic frame
[in]	latitude	location latitude angle in radians

Returns: void

< vector of earth rotation components expressed in the geografic frame according to the latitude

Compute the v vector expressed in the body frame

Subtract the earth rotation to the vector of inputs (u = u-v

◆ update()

void filter::ukf::update	(	Eigen::Matrix< double, ukf::NUMAXIS, 1 > *	acc,
		Eigen::Matrix< double, ukf::NUMAXIS, 1 > *	mag
	)

Performs the measurement and correction steps of the filter.

Performs the update (measurement and correction) step of the filter.

The UKf measurement step

Author: Javier Hidalgo Carrio.

Parameters

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

Returns: void

< scalar part of a quaternion

< vectorial part of a quaternion

< Summation variable

< Summation matrix

Reference vector

Compute observation from the state prediction (from the propagated sigma point quaternion))

Predicted mean of the observation

Predicted covariance of the observation

Update using Accelerometers

Compute quaternion from euler angles

Measurement vector

Innovation covariance

Cross-correlation matrix

Compute the Kalman Gain

Innovation in the measurement

Correction of the mean value (state)

Covariance matrix of the process

Update the attitude quaternion using the state vector (rodrigues parameter)

Set to zero for the next propagation

Member Data Documentation

◆ NUMAXIS

const int filter::ukf::NUMAXIS = 3

static

Number of Sigma Points Sensors constant parameters

◆ QUATERSIZE

const int filter::ukf::QUATERSIZE = 4

static

Number of variables of the vector state-space representation

◆ SIGPOINTSIZE

const int filter::ukf::SIGPOINTSIZE = (2*ukf::UKFSTATEVECTORSIZE) + 1

static

Number of parameters of a quaternion

◆ UKFSTATEVECTORSIZE

const int filter::ukf::UKFSTATEVECTORSIZE = 6

static

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

/build/rock-master-18.09-slam-quater-ukf-0.20131122/src/ukf.hpp
/build/rock-master-18.09-slam-quater-ukf-0.20131122/src/ukf.cpp

Public Types

Public Member Functions

Static Public Attributes

Member Enumeration Documentation

◆ DECLINATION_CONSTS

Member Function Documentation

◆ attitudeUpdate()

◆ getAttitude()

◆ getCovariance()

◆ getEuler()

◆ getState()

◆ GravityModel()

◆ Init()

◆ Omega()

◆ predict()

◆ setAttitude()

◆ SubstractEarthRotation()

◆ update()

Member Data Documentation

◆ NUMAXIS

◆ QUATERSIZE

◆ SIGPOINTSIZE

◆ UKFSTATEVECTORSIZE