stateObservation::kine Namespace Reference

Namespaces
namespace	internal

Classes
struct	indexes
struct	indexes< quaternion >
struct	indexes< rotationVector >
struct	Kinematics
	Class facilitating the manipulation of the kinematics of a frame within another and the associated operations. More...
struct	LocalKinematics
	Class facilitating the manipulation of the local kinematics of a frame within another and the associated operations. More...
class	Orientation

Enumerations
enum	rotationType { matrix = 0 , rotationVector = 1 , quaternion = 2 , angleaxis = 3 }

Functions
void	integrateKinematics (Vector3 &position, const Vector3 &velocity, double dt)
void	integrateKinematics (Vector3 &position, Vector3 &velocity, const Vector3 &acceleration, double dt)
void	integrateKinematics (Matrix3 &orientation, const Vector3 &rotationVelocity, double dt)
void	integrateKinematics (Matrix3 &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	integrateKinematics (Quaternion &orientation, const Vector3 &rotationVelocity, double dt)
void	integrateKinematics (Quaternion &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	integrateKinematics (Vector3 &position, Vector3 &velocity, const Vector3 &acceleration, Matrix3 &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	integrateKinematics (Vector3 &position, Vector3 &velocity, const Vector3 &acceleration, Quaternion &orientation, Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, double dt)
void	integrateKinematics (Vector3 &position, const Vector3 &velocity, Matrix3 &orientation, const Vector3 &rotationVelocity, double dt)
	integrates the postition/orientation given the velocities
void	integrateKinematics (Vector3 &position, const Vector3 &velocity, Quaternion &orientation, const Vector3 &rotationVelocity, double dt)
	integrates the postition/orientation given the velocities
Vector	regulateRotationVector (const Vector3 &v)
AngleAxis	rotationVectorToAngleAxis (const Vector3 &v)
	Transforms the rotation vector into angle axis.
Matrix3	rotationVectorToRotationMatrix (const Vector3 &v)
	Transforms the rotation vector into rotation matrix.
Quaternion	rotationVectorToQuaternion (const Vector3 &v)
	Transforms the rotation vector into quaternion.
Vector3	rotationMatrixToRotationVector (const Matrix3 &R)
	Transforms the rotation matrix into rotation vector.
Vector3	quaternionToRotationVector (const Quaternion &q)
	Tranbsform a quaternion into rotation vector.
Vector3	quaternionToRotationVector (const Vector4 &v)
	Transforms a quaternion into rotation vector.
double	scalarComponent (const Quaternion &q)
	scalar component of a quaternion
Vector3	vectorComponent (const Quaternion &q)
	vector part of the quaternion
Vector3	rotationMatrixToRollPitchYaw (const Matrix3 &R, Vector3 &v)
Vector3	rotationMatrixToRollPitchYaw (const Matrix3 &R)
Matrix3	rollPitchYawToRotationMatrix (double roll, double pitch, double yaw)
Matrix3	rollPitchYawToRotationMatrix (const Vector3 &rpy)
Quaternion	rollPitchYawToQuaternion (double roll, double pitch, double yaw)
Quaternion	rollPitchYawToQuaternion (const Vector3 &rpy)
Matrix3	orthogonalizeRotationMatrix (const Matrix3 &M)
	Projects the Matrix to so(3)
Matrix3	skewSymmetric (const Vector3 &v, Matrix3 &R)
	transform a 3d vector into a skew symmetric 3x3 matrix
Matrix3	skewSymmetric (const Vector3 &v)
	transform a 3d vector into a skew symmetric 3x3 matrix
Matrix3	skewSymmetric2 (const Vector3 &v, Matrix3 &R)
	transform a 3d vector into a squared skew symmetric 3x3 matrix
Matrix3	skewSymmetric2 (const Vector3 &v)
	transform a 3d vector into a squared skew symmetric 3x3 matrix
Vector6	homogeneousMatrixToVector6 (const Matrix4 &M)
	transforms a homogeneous matrix into 6d vector (position theta mu)
Matrix4	vector6ToHomogeneousMatrix (const Vector6 &v)
	transforms a 6d vector (position theta mu) into a homogeneous matrix
Matrix3	twoVectorsToRotationMatrix (const Vector3 &v1, const Vector3 Rv1)
	Builds the smallest angle matrix allowing to get from a NORMALIZED vector v1 to its imahe Rv1 This is based on Rodrigues formula.
bool	isPureYaw (const Matrix3 &R)
	checks if this matrix is a pure yaw matrix or not
Vector3	getInvariantHorizontalVector (const Matrix3 &R)
	Gets a vector that remains horizontal with this rotation. This vector is NOT normalized.
Vector3	getInvariantOrthogonalVector (const Matrix3 &Rhat, const Vector3 &Rtez)
	Gets a vector \(v\) that is orthogonal to \(e_z\) and such that \(\hat{R}^T e_z\) is orthogonal to the tilt \(R^T e_z\). This vector is NOT normalized.
Matrix3	mergeTiltWithYaw (const Vector3 &Rtez, const Matrix3 &R2, const Vector3 &v=Vector3::UnitX()) noexcept(false)
	Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector)
Matrix3	mergeRoll1Pitch1WithYaw2 (const Matrix3 &R1, const Matrix3 &R2, const Vector3 &v=Vector3::UnitX())
	Merge the roll and pitch with the yaw from a rotation matrix (minimizes the deviation of the v vector)
Matrix3	mergeTiltWithYawAxisAgnostic (const Vector3 &Rtez, const Matrix3 &R2)
	Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector)
Matrix3	mergeRoll1Pitch1WithYaw2AxisAgnostic (const Matrix3 &R1, const Matrix3 &R2)
	Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector.
double	rotationMatrixToAngle (const Matrix3 &rotation, const Vector3 &axis, const Vector3 &v)
	take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the axis with this rotation
double	rotationMatrixToYaw (const Matrix3 &rotation, const Vector2 &v)
	take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vertical axis with this rotation
double	rotationMatrixToYaw (const Matrix3 &rotation)
	take 3x3 matrix represeting a rotation and gives the yaw angle from roll pitch yaw representation
double	rotationMatrixToYawAxisAgnostic (const Matrix3 &rotation)
	take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis
Quaternion	zeroRotationQuaternion ()
	Get the Identity Quaternion.
Quaternion	randomRotationQuaternion ()
	Get a uniformly random Quaternion.
double	randomAngle ()
	get a randomAngle between -pi and pu
bool	isRotationMatrix (const Matrix3 &, double precision=2 *cst::epsilon1)
	Checks if it is a rotation matrix (right-hand orthonormal) or not.
void	fixedPointRotationToTranslation (const Matrix3 &R, const Vector3 &rotationVelocity, const Vector3 &rotationAcceleration, const Vector3 &fixedPoint, Vector3 &outputTranslation, Vector3 &outputLinearVelocity, Vector3 &outputLinearAcceleration)
	transforms a rotation into translation given a constraint of a fixed point
Vector3	derivateRotationFD (const Quaternion &q1, const Quaternion &q2, double dt)
	derivates a quaternion using finite difference to get a angular velocity vector
Vector3	derivateRotationFD (const Vector3 &o1, const Vector3 &o2, double dt)
	derivates a rotation vector using finite difference to get a angular velocity vector
Vector6	derivateHomogeneousMatrixFD (const Matrix4 &m1, const Matrix4 &m2, double dt)
Vector6	derivatePoseThetaUFD (const Vector6 &v1, const Vector6 &v2, double dt)
void	derivateRotationMultiplicative (const Vector3 &deltaR, Matrix3 &dRdR, Matrix3 &dRddeltaR)
Matrix3	derivateRtvMultiplicative (const Matrix3 &R, const Vector3 &v)
IndexedVectorArray	reconstructStateTrajectory (const IndexedVectorArray &positionOrientation, double dt)
Vector	invertState (const Vector &state)
Matrix4	invertHomoMatrix (const Matrix4 &m)

Variables
constexpr double	quatNormTol = 1e-6
	relative tolereance to the square of quaternion norm.

Enumeration Type Documentation

rotationType

enum stateObservation::kine::rotationType

Enumerator
matrix
rotationVector
quaternion
angleaxis

Function Documentation

derivateHomogeneousMatrixFD()

Vector6 stateObservation::kine::derivateHomogeneousMatrixFD ( const Matrix4 &  m1, const Matrix4 &  m2, double  dt  )

inline

derivatePoseThetaUFD()

Vector6 stateObservation::kine::derivatePoseThetaUFD ( const Vector6 &  v1, const Vector6 &  v2, double  dt  )

inline

derivateRotationFD() [1/2]

Vector3 stateObservation::kine::derivateRotationFD ( const Quaternion &  q1, const Quaternion &  q2, double  dt  )

inline

derivates a quaternion using finite difference to get a angular velocity vector

derivateRotationFD() [2/2]

Vector3 stateObservation::kine::derivateRotationFD ( const Vector3 &  o1, const Vector3 &  o2, double  dt  )

inline

derivates a rotation vector using finite difference to get a angular velocity vector

derivateRotationMultiplicative()

void stateObservation::kine::derivateRotationMultiplicative ( const Vector3 &  deltaR, Matrix3 &  dRdR, Matrix3 &  dRddeltaR  )

inline

Computes the "multiplicative Jacobian" for Kalman filtering for example orientation is the current orientation dR is the rotation delta between the current orientation and the orientation at the next step. dRdR is the "multiplicative" Jacobian with regard to variations of orientation dRddeltaR is the "multiplicative" Jacobian with regard to variations of deltaR

derivateRtvMultiplicative()

Matrix3 stateObservation::kine::derivateRtvMultiplicative ( const Matrix3 &  R, const Vector3 &  v  )

inline

Computes the "multiplicative Jacobian" for a function R^T.v giving a vector v expressed in a local frame with regard to Rotations of this local frame

fixedPointRotationToTranslation()

void stateObservation::kine::fixedPointRotationToTranslation ( const Matrix3 &  R, const Vector3 &  rotationVelocity, const Vector3 &  rotationAcceleration, const Vector3 &  fixedPoint, Vector3 &  outputTranslation, Vector3 &  outputLinearVelocity, Vector3 &  outputLinearAcceleration  )

inline

transforms a rotation into translation given a constraint of a fixed point

getInvariantHorizontalVector()

Vector3 stateObservation::kine::getInvariantHorizontalVector ( const Matrix3 &  R )

inline

Gets a vector that remains horizontal with this rotation. This vector is NOT normalized.

There is a general version in getInvariantOrthogonalVector(). This can be used to extract yaw angle from a rotation matrix without needing to specify an order in the tils (e.g. roll then pich).

Parameters

R	the input rotation

Returns

Vector3 the output horizontal vector

getInvariantOrthogonalVector()

Vector3 stateObservation::kine::getInvariantOrthogonalVector ( const Matrix3 &  Rhat, const Vector3 &  Rtez  )

inline

Gets a vector \(v\) that is orthogonal to \(e_z\) and such that \(\hat{R}^T e_z\) is orthogonal to the tilt \(R^T e_z\). This vector is NOT normalized.

This is a generalization of getInvariantHorizontalVector() which corresponds to no tilt \(\hat{R}^T e_z=e_z\). This function is useful to merge the yaw from the rotation matrix with the tilt.

Parameters

Rhat	the input rotation matrix \(\hat{R}^T\)
Rtez	the input tilt \(\hat{R}^T e_z\)

Returns

Vector3 the output horizontal vector

homogeneousMatrixToVector6()

Vector6 stateObservation::kine::homogeneousMatrixToVector6 ( const Matrix4 &  M )

inline

transforms a homogeneous matrix into 6d vector (position theta mu)

integrateKinematics() [1/10]

void stateObservation::kine::integrateKinematics ( Matrix3 &  orientation, const Vector3 &  rotationVelocity, double  dt  )

inline

integrateKinematics() [2/10]

void stateObservation::kine::integrateKinematics ( Matrix3 &  orientation, Vector3 &  rotationVelocity, const Vector3 &  rotationAcceleration, double  dt  )

inline

integrateKinematics() [3/10]

void stateObservation::kine::integrateKinematics ( Quaternion &  orientation, const Vector3 &  rotationVelocity, double  dt  )

inline

integrateKinematics() [4/10]

void stateObservation::kine::integrateKinematics ( Quaternion &  orientation, Vector3 &  rotationVelocity, const Vector3 &  rotationAcceleration, double  dt  )

inline

integrateKinematics() [5/10]

void stateObservation::kine::integrateKinematics ( Vector3 &  position, const Vector3 &  velocity, double  dt  )

inline

integrateKinematics() [6/10]

void stateObservation::kine::integrateKinematics ( Vector3 &  position, const Vector3 &  velocity, Matrix3 &  orientation, const Vector3 &  rotationVelocity, double  dt  )

inline

integrates the postition/orientation given the velocities

integrateKinematics() [7/10]

void stateObservation::kine::integrateKinematics ( Vector3 &  position, const Vector3 &  velocity, Quaternion &  orientation, const Vector3 &  rotationVelocity, double  dt  )

inline

integrates the postition/orientation given the velocities

integrateKinematics() [8/10]

void stateObservation::kine::integrateKinematics ( Vector3 &  position, Vector3 &  velocity, const Vector3 &  acceleration, double  dt  )

inline

integrateKinematics() [9/10]

void stateObservation::kine::integrateKinematics ( Vector3 &  position, Vector3 &  velocity, const Vector3 &  acceleration, Matrix3 &  orientation, Vector3 &  rotationVelocity, const Vector3 &  rotationAcceleration, double  dt  )

inline

integrates the position/orientation and their time derivatives, given the accelerations, and initial velocities and positions. The rotations are expressed by rotation matrix

integrateKinematics() [10/10]

void stateObservation::kine::integrateKinematics ( Vector3 &  position, Vector3 &  velocity, const Vector3 &  acceleration, Quaternion &  orientation, Vector3 &  rotationVelocity, const Vector3 &  rotationAcceleration, double  dt  )

inline

integrates the position/orientation and their time derivatives, given the accelerations, and initial velocities and positions. The orientations are expressed by quaternions

invertHomoMatrix()

Matrix4 stateObservation::kine::invertHomoMatrix ( const Matrix4 &  m )

inline

invertState()

Vector stateObservation::kine::invertState ( const Vector &  state )

inline

isPureYaw()

bool stateObservation::kine::isPureYaw ( const Matrix3 &  R )

inline

checks if this matrix is a pure yaw matrix or not

Parameters

R	the rotation matrix

Returns

true is pure yaw

false is not pure yaw

isRotationMatrix()

bool stateObservation::kine::isRotationMatrix ( const Matrix3 &  , double  precision = 2 *cst::epsilon1  )

inline

Checks if it is a rotation matrix (right-hand orthonormal) or not.

Parameters

precision

the absolute precision of the test

Returns

true when it is a rotation matrix

false when not

mergeRoll1Pitch1WithYaw2()

Matrix3 stateObservation::kine::mergeRoll1Pitch1WithYaw2 ( const Matrix3 &  R1, const Matrix3 &  R2, const Vector3 &  v = Vector3::UnitX()  )

inline

Merge the roll and pitch with the yaw from a rotation matrix (minimizes the deviation of the v vector)

Parameters

R1	is the first rotation to get the roll and pitch
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference (for a traditional yaw v is initialized to \(e_x\))

Returns

Matrix3 the merged rotation matrix

mergeRoll1Pitch1WithYaw2AxisAgnostic()

Matrix3 stateObservation::kine::mergeRoll1Pitch1WithYaw2AxisAgnostic ( const Matrix3 &  R1, const Matrix3 &  R2  )

inline

Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector.

Parameters

R1	is the first rotation to get the roll and pitch
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference (for a traditional yaw v is initialized to \(e_x\))

Returns

Matrix3 the merged rotation matrix

mergeTiltWithYaw()

Matrix3 stateObservation::kine::mergeTiltWithYaw ( const Vector3 &  Rtez, const Matrix3 &  R2, const Vector3 &  v = Vector3::UnitX()  )

inline

Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector)

throws exception when the orientation is singlular (likely gimbal lock) to avoid these issues, we recommend to use mergeTiltWithYawAxisAgnostic()

Parameters

Rtez	the tilt \(R_1^T e_z\) (the local image of \(e_z\) unit vector)
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference it must be horizontal and normalized (for a traditional yaw v is by deftault \(e_x\))

Returns

Matrix3 the merged rotation matrix

mergeTiltWithYawAxisAgnostic()

Matrix3 stateObservation::kine::mergeTiltWithYawAxisAgnostic ( const Vector3 &  Rtez, const Matrix3 &  R2  )

inline

Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation of the v vector)

Parameters

Rtez	the tilt \(R_1^T e_z\) (the local image of \(e_z\) unit vector)
R2	is the second rotation matrix from which the "yaw" needs to be extracted
v	is the vector to use as reference (for a traditional yaw v is initialized to \(e_x\))

Returns

Matrix3 the merged rotation matrix

orthogonalizeRotationMatrix()

Matrix3 stateObservation::kine::orthogonalizeRotationMatrix ( const Matrix3 &  M )

inline

Projects the Matrix to so(3)

quaternionToRotationVector() [1/2]

Vector3 stateObservation::kine::quaternionToRotationVector ( const Quaternion &  q )

inline

Tranbsform a quaternion into rotation vector.

quaternionToRotationVector() [2/2]

Vector3 stateObservation::kine::quaternionToRotationVector ( const Vector4 &  v )

inline

Transforms a quaternion into rotation vector.

randomAngle()

double stateObservation::kine::randomAngle ( )

inline

get a randomAngle between -pi and pu

Returns

double the random angle

randomRotationQuaternion()

Quaternion stateObservation::kine::randomRotationQuaternion ( )

inline

Get a uniformly random Quaternion.

Returns

Quaternion

reconstructStateTrajectory()

IndexedVectorArray stateObservation::kine::reconstructStateTrajectory ( const IndexedVectorArray &  positionOrientation, double  dt  )

inline

uses the derivation to reconstruct the velocities and accelerations given trajectories in positions and orientations only

regulateRotationVector()

Vector stateObservation::kine::regulateRotationVector ( const Vector3 &  v )

inline

Puts the orientation vector norm between 0 and Pi if it gets close to 2pi

rollPitchYawToQuaternion() [1/2]

Quaternion stateObservation::kine::rollPitchYawToQuaternion ( const Vector3 &  rpy )

inline

rollPitchYawToQuaternion() [2/2]

Quaternion stateObservation::kine::rollPitchYawToQuaternion ( double  roll, double  pitch, double  yaw  )

inline

Transform the roll pitch yaw into rotation matrix ( R = Ry*Rp*Rr)

rollPitchYawToRotationMatrix() [1/2]

Matrix3 stateObservation::kine::rollPitchYawToRotationMatrix ( const Vector3 &  rpy )

inline

rollPitchYawToRotationMatrix() [2/2]

Matrix3 stateObservation::kine::rollPitchYawToRotationMatrix ( double  roll, double  pitch, double  yaw  )

inline

Transform the roll pitch yaw into rotation matrix ( R = Ry*Rp*Rr)

rotationMatrixToAngle()

double stateObservation::kine::rotationMatrixToAngle ( const Matrix3 &  rotation, const Vector3 &  axis, const Vector3 &  v  )

inline

take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the axis with this rotation

Parameters

rotation	The 3x3 rotation matrix
axis	the axis of rotation (must be normalized)
v	the vector that is rotated with the rotation (must be orthogonal to axis and normalized)

Returns

double the angle

rotationMatrixToRollPitchYaw() [1/2]

Vector3 stateObservation::kine::rotationMatrixToRollPitchYaw ( const Matrix3 &  R )

inline

rotationMatrixToRollPitchYaw() [2/2]

Vector3 stateObservation::kine::rotationMatrixToRollPitchYaw ( const Matrix3 &  R, Vector3 &  v  )

inline

Transforms the rotation matrix into roll pitch yaw (decompose R into Ry*Rp*Rr)

rotationMatrixToRotationVector()

Vector3 stateObservation::kine::rotationMatrixToRotationVector ( const Matrix3 &  R )

inline

Transforms the rotation matrix into rotation vector.

rotationMatrixToYaw() [1/2]

double stateObservation::kine::rotationMatrixToYaw ( const Matrix3 &  rotation )

inline

take 3x3 matrix represeting a rotation and gives the yaw angle from roll pitch yaw representation

Parameters

rotation

The 3x3 rotation matrix

Returns

double the angle

rotationMatrixToYaw() [2/2]

double stateObservation::kine::rotationMatrixToYaw ( const Matrix3 &  rotation, const Vector2 &  v  )

inline

take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vertical axis with this rotation

this is a generalization of yaw extraction (yaw is equivalent to v = Matrix3::UnitX(), but it is more efficiant to calll the dedicated rotationMatrixToYaw() without vector parameter).

Parameters

rotation	The 3x3 rotation matrix
v	the rotated vector (expressed in the horizontal plane, must be normalized)

Returns

double the angle

rotationMatrixToYawAxisAgnostic()

double stateObservation::kine::rotationMatrixToYawAxisAgnostic ( const Matrix3 &  rotation )

inline

take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis

This is similar to yaw angle but here we identify a horizontal vector that stays horizontal after rotation. this can be called axis agnostic yaw extraction. and get the angle between them

Parameters

rotation

The 3x3 rotation matrix

Returns

double the angle

rotationVectorToAngleAxis()

AngleAxis stateObservation::kine::rotationVectorToAngleAxis ( const Vector3 &  v )

inline

Transforms the rotation vector into angle axis.

rotationVectorToQuaternion()

Quaternion stateObservation::kine::rotationVectorToQuaternion ( const Vector3 &  v )

inline

Transforms the rotation vector into quaternion.

rotationVectorToRotationMatrix()

Matrix3 stateObservation::kine::rotationVectorToRotationMatrix ( const Vector3 &  v )

inline

Transforms the rotation vector into rotation matrix.

scalarComponent()

double stateObservation::kine::scalarComponent ( const Quaternion &  q )

inline

scalar component of a quaternion

skewSymmetric() [1/2]

Matrix3 stateObservation::kine::skewSymmetric ( const Vector3 &  v )

inline

transform a 3d vector into a skew symmetric 3x3 matrix

skewSymmetric() [2/2]

Matrix3 stateObservation::kine::skewSymmetric ( const Vector3 &  v, Matrix3 &  R  )

inline

transform a 3d vector into a skew symmetric 3x3 matrix

skewSymmetric2() [1/2]

Matrix3 stateObservation::kine::skewSymmetric2 ( const Vector3 &  v )

inline

transform a 3d vector into a squared skew symmetric 3x3 matrix

skewSymmetric2() [2/2]

Matrix3 stateObservation::kine::skewSymmetric2 ( const Vector3 &  v, Matrix3 &  R  )

inline

transform a 3d vector into a squared skew symmetric 3x3 matrix

twoVectorsToRotationMatrix()

Matrix3 stateObservation::kine::twoVectorsToRotationMatrix ( const Vector3 &  v1, const Vector3  Rv1  )

inline

Builds the smallest angle matrix allowing to get from a NORMALIZED vector v1 to its imahe Rv1 This is based on Rodrigues formula.

Parameters

v1	the NORMALIZED vector
Rv1	the NORMALIZED image of this vector by the rotation matrix R

Returns

Matrix3 the rotation matrix R

vector6ToHomogeneousMatrix()

Matrix4 stateObservation::kine::vector6ToHomogeneousMatrix ( const Vector6 &  v )

inline

transforms a 6d vector (position theta mu) into a homogeneous matrix

vectorComponent()

Vector3 stateObservation::kine::vectorComponent ( const Quaternion &  q )

inline

vector part of the quaternion

zeroRotationQuaternion()

Quaternion stateObservation::kine::zeroRotationQuaternion ( )

inline

Get the Identity Quaternion.

Returns

Quaternion

Variable Documentation

quatNormTol

constexpr double stateObservation::kine::quatNormTol = 1e-6

constexpr

relative tolereance to the square of quaternion norm.