pose_ekf::KFD_PosVelOriAcc Class Reference

#include <KFD_PosVelOriAcc.hpp>

Public Member Functions
	KFD_PosVelOriAcc ()
	~KFD_PosVelOriAcc ()
void	copyState (const KFD_PosVelOriAcc &kfd)
bool	positionObservation (Eigen::Vector3d position, Eigen::Matrix3d covariance, float reject_position_threshol)
bool	velocityObservation (Eigen::Vector3d velocity_body, Eigen::Matrix3d covariance, float reject_velocity_threshol)
bool	orientationObservation (Eigen::Quaterniond orientation, Eigen::Matrix3d covariance, float reject_orientation_threshol)
void	setPosition (Eigen::Vector3d position, Eigen::Matrix3d covariance)
void	setOrientation (Eigen::Quaterniond orientation, Eigen::Matrix3d covariance)
void	correct_state ()
void	predict (Eigen::Vector3d acc_intertial, Eigen::Vector3d angular_velocity, double dt, Eigen::Matrix< double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE > process_noise)
Eigen::Vector3d	getPosition ()
Eigen::Vector3d	getVelocity ()
Eigen::Quaterniond	getOrientationInertial2World ()
Eigen::Matrix3d	getPositionCovariance ()
Eigen::Matrix3d	getVelocityCovariance ()
Eigen::Matrix3d	getOrientationCovariance ()
Eigen::Quaterniond	angularCorrection ()
void	init (const Eigen::Matrix< double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE > &P, const Eigen::Matrix< double, StatePosVelOriAcc::SIZE, 1 > &x)

Public Attributes
StatePosVelOriAcc	x
Eigen::Vector3d	velocity_inertial
Eigen::Vector3d	position_world
Eigen::Quaterniond	R_inertial_2_world
KalmanFilter::KF< StatePosVelOriAcc::SIZE > *	filter
Eigen::Matrix< double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE >	Q

Static Public Attributes
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const unsigned int	_POS_MEASUREMENT_SIZE = 3
static const unsigned int	_VEL_MEASUREMENT_SIZE = 3
static const unsigned int	_POS_DEGREE_OF_FREEDOM = 3
static const unsigned int	_VEL_DEGREE_OF_FREEDOM = 3
static const unsigned int	_ORI_MEASUREMENT_SIZE = 3
static const unsigned int	_ORI_DEGREE_OF_FREEDOM = 3

Constructor & Destructor Documentation

KFD_PosVelOriAcc()

KFD_PosVelOriAcc::KFD_PosVelOriAcc

(

)

CHECK

~KFD_PosVelOriAcc()

KFD_PosVelOriAcc::~KFD_PosVelOriAcc

(

)

Member Function Documentation

angularCorrection()

Eigen::Quaterniond KFD_PosVelOriAcc::angularCorrection

(

)

get orientation correction calculated by the kalman filter

copyState()

void KFD_PosVelOriAcc::copyState

(

const KFD_PosVelOriAcc &

kfd

)

Copy the Kalman Filter State from KFD

correct_state()

void KFD_PosVelOriAcc::correct_state

(

)

The current state estimation will be corrected by the Kalman Filter error calculation than the states in the filter will be reseted.

getOrientationCovariance()

Eigen::Matrix3d KFD_PosVelOriAcc::getOrientationCovariance

(

)

get the Orientation covariance

getOrientationInertial2World()

Eigen::Quaterniond KFD_PosVelOriAcc::getOrientationInertial2World

(

)

gets the estimated pose

getPosition()

Eigen::Vector3d KFD_PosVelOriAcc::getPosition

(

)

gets the estimated position

getPositionCovariance()

Eigen::Matrix3d KFD_PosVelOriAcc::getPositionCovariance

(

)

get the Position covariance

getVelocity()

Eigen::Vector3d KFD_PosVelOriAcc::getVelocity

(

)

gets the estimated velocity

getVelocityCovariance()

Eigen::Matrix3d KFD_PosVelOriAcc::getVelocityCovariance

(

)

get the Velocity covariance

init()

void KFD_PosVelOriAcc::init	(	const Eigen::Matrix< double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE > &	P,
		const Eigen::Matrix< double, StatePosVelOriAcc::SIZE, 1 > &	x
	)

set the inital values for state x and covariance P

configurarion hook

orientationObservation()

bool KFD_PosVelOriAcc::orientationObservation	(	Eigen::Quaterniond	orientation,
		Eigen::Matrix3d	covariance,
		float	reject_orientation_threshol
	)

A 3D orientation observation

positionObservation()

bool KFD_PosVelOriAcc::positionObservation	(	Eigen::Vector3d	position,
		Eigen::Matrix3d	covariance,
		float	reject_position_threshol
	)

A 3D position observation

predict()

void KFD_PosVelOriAcc::predict	(	Eigen::Vector3d	acc_intertial,
		Eigen::Vector3d	angular_velocity,
		double	dt,
		Eigen::Matrix< double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE >	process_noise
	)

input acc and angular velocity data to the filter updating the kalman filter and pose estimation

Angular change calculation

Inertial Navigation

Kalman Filter

setOrientation()

void KFD_PosVelOriAcc::setOrientation	(	Eigen::Quaterniond	orientation,
		Eigen::Matrix3d	covariance
	)

set orientation

setPosition()

void KFD_PosVelOriAcc::setPosition	(	Eigen::Vector3d	position,
		Eigen::Matrix3d	covariance
	)

set the position

velocityObservation()

bool KFD_PosVelOriAcc::velocityObservation	(	Eigen::Vector3d	velocity_body,
		Eigen::Matrix3d	covariance,
		float	reject_velocity_threshol
	)

A 3D velocity observation

Member Data Documentation

_ORI_DEGREE_OF_FREEDOM

const unsigned int pose_ekf::KFD_PosVelOriAcc::_ORI_DEGREE_OF_FREEDOM = 3

static

_ORI_MEASUREMENT_SIZE

const unsigned int pose_ekf::KFD_PosVelOriAcc::_ORI_MEASUREMENT_SIZE = 3

static

_POS_DEGREE_OF_FREEDOM

const unsigned int pose_ekf::KFD_PosVelOriAcc::_POS_DEGREE_OF_FREEDOM = 3

static

_POS_MEASUREMENT_SIZE

EIGEN_MAKE_ALIGNED_OPERATOR_NEW const unsigned int pose_ekf::KFD_PosVelOriAcc::_POS_MEASUREMENT_SIZE = 3

static

ensure alignment for eigen vector types

_VEL_DEGREE_OF_FREEDOM

const unsigned int pose_ekf::KFD_PosVelOriAcc::_VEL_DEGREE_OF_FREEDOM = 3

static

_VEL_MEASUREMENT_SIZE

const unsigned int pose_ekf::KFD_PosVelOriAcc::_VEL_MEASUREMENT_SIZE = 3

static

filter

KalmanFilter::KF<StatePosVelOriAcc::SIZE>* pose_ekf::KFD_PosVelOriAcc::filter

Instance of the Extended Kalman filter

position_world

Eigen::Vector3d pose_ekf::KFD_PosVelOriAcc::position_world

Q

Eigen::Matrix<double, StatePosVelOriAcc::SIZE, StatePosVelOriAcc::SIZE> pose_ekf::KFD_PosVelOriAcc::Q

process noise

R_inertial_2_world

Eigen::Quaterniond pose_ekf::KFD_PosVelOriAcc::R_inertial_2_world

velocity_inertial

Eigen::Vector3d pose_ekf::KFD_PosVelOriAcc::velocity_inertial

covariance matrix estimated position, velocityand orientation

x

StatePosVelOriAcc pose_ekf::KFD_PosVelOriAcc::x

state estimate

---

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

• /build/rock-master-18.09-slam-pose-ekf-0.20121031/src/KFD_PosVelOriAcc.hpp
• /build/rock-master-18.09-slam-pose-ekf-0.20121031/src/KFD_PosVelOriAcc.cpp