|
quater_ukf
|
#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 |
| 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)
< 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
| Eigen::Quaternion< double > filter::ukf::getAttitude | ( | ) |
Gets the current orientation in Quaternion.
| Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, ukf::UKFSTATEVECTORSIZE > filter::ukf::getCovariance | ( | ) |
Gets Noise covariance matrix.
| Eigen::Matrix< double, ukf::NUMAXIS, 1 > filter::ukf::getEuler | ( | ) |
Gets the current orientation in Euler angles.
Gets the current orientation in Euler angles (rad)
| Eigen::Matrix< double, ukf::UKFSTATEVECTORSIZE, 1 > filter::ukf::getState | ( | ) |
Gets the current state vector of the filter.
| double filter::ukf::GravityModel | ( | double | latitude, |
| double | altitude | ||
| ) |
This computes the theoretical gravity value according to the WGS-84 ellipsoid earth model.
| [in] | latitude | double the latitude value in radian |
| [in] | altitude | double with the altitude value in meters |
< g magnitude at zero altitude
Nominal Gravity model
Gravity affects by the altitude (aprox the value r = Re
| 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
| [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 |
Indirect Kalman Filter methods
Gravitation acceleration
Set the parameters
Set the state vector
Set the matrices
Set the initial attitude
Print values
| 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)
| [in,out] | *quat | the quaternion to propagate |
| [in] | *angvelo | pointer to vector with the angular velocity |
| [in] | dt | delta time between samples |
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
| 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
| [in] | *u | pointer to vector with the angular velocity |
| [in] | dt | delta time between samples |
< 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
| bool filter::ukf::setAttitude | ( | Eigen::Quaternion< double > * | initq | ) |
This function Initilize Attitude.
Initial orientation value beforeestart the IKF
| [in] | *initq | pointer to quaternion with the initial orientation |
Initial orientation
| 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.
| [in,out] | *u | pointer to angular velocity |
| [in] | *qb_g | quaternion from body frame to geographic frame |
| [in] | latitude | location latitude angle in radians |
< 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
| 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
| [in] | *u | pointer to vector with the angular velocity |
| [in] | dt | delta time between samples |
< scalar part of a quaternion
< vectorial part of a quaternion
< Summation variable
< Summation matrix
< 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
|
static |
Number of Sigma Points Sensors constant parameters
|
static |
Number of variables of the vector state-space representation
|
static |
Number of parameters of a quaternion
|
static |
1.8.6