pose_ekf::KFD_PosVelAcc Class Reference

#include <KFD_PosVelAcc.hpp>

Public Member Functions
	KFD_PosVelAcc ()
	~KFD_PosVelAcc ()
void	copyState (const KFD_PosVelAcc &kfd)
bool	positionObservation (Eigen::Vector3d position, Eigen::Matrix3d covariance, float reject_position_threshol)
bool	velocityObservation (Eigen::Vector3d velocity, Eigen::Matrix3d covariance, float reject_velocity_threshol)
bool	positionZObservation (double z, double error, double rejection_threshold)
void	setPosition (Eigen::Vector3d position, Eigen::Matrix3d covariance)
void	correct_state ()
void	predict (Eigen::Vector3d acc, double dt, Eigen::Matrix< double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE > process_noise)
void	setRotation (Eigen::Quaterniond R)
Eigen::Quaterniond	getRotation ()
Eigen::Vector3d	getPosition ()
Eigen::Vector3d	getVelocity ()
Eigen::Vector3d	getAccBias ()
Eigen::Matrix3d	getPositionCovariance ()
Eigen::Matrix3d	getVelocityCovariance ()
Eigen::Matrix3d	getAccCovariance ()
void	init (const Eigen::Matrix< double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE > &P, const Eigen::Matrix< double, StatePosVelAcc::SIZE, 1 > &x)

Public Attributes
StatePosVelAcc	x
Eigen::Vector3d	velocity_world
Eigen::Vector3d	position_world
KalmanFilter::KF< StatePosVelAcc::SIZE > *	filter
Eigen::Matrix< double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE >	Q
Eigen::Quaterniond	R_input_to_world

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

Constructor & Destructor Documentation

KFD_PosVelAcc()

KFD_PosVelAcc::KFD_PosVelAcc

(

)

CHECK

~KFD_PosVelAcc()

KFD_PosVelAcc::~KFD_PosVelAcc

(

)

Member Function Documentation

copyState()

void KFD_PosVelAcc::copyState

(

const KFD_PosVelAcc &

kfd

)

Copy the Kalman Filter State from KFD

correct_state()

void KFD_PosVelAcc::correct_state

(

)

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

getAccBias()

Eigen::Vector3d KFD_PosVelAcc::getAccBias

(

)

getAccCovariance()

Eigen::Matrix3d KFD_PosVelAcc::getAccCovariance

(

)

get the Acceleration covariance

getPosition()

Eigen::Vector3d KFD_PosVelAcc::getPosition

(

)

gets the estimated position

getPositionCovariance()

Eigen::Matrix3d KFD_PosVelAcc::getPositionCovariance

(

)

get the Position covariance

getRotation()

Eigen::Quaterniond pose_ekf::KFD_PosVelAcc::getRotation ( )

inline

getVelocity()

Eigen::Vector3d KFD_PosVelAcc::getVelocity

(

)

gets the estimated velocity

getVelocityCovariance()

Eigen::Matrix3d KFD_PosVelAcc::getVelocityCovariance

(

)

get the Velocity covariance

init()

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

set the inital values for state x and covariance P

configurarion hook

positionObservation()

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

A 3D position observation

positionZObservation()

bool KFD_PosVelAcc::positionZObservation	(	double	z,
		double	error,
		double	rejection_threshold
	)

A z position observation

predict()

void KFD_PosVelAcc::predict	(	Eigen::Vector3d	acc,
		double	dt,
		Eigen::Matrix< double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE >	process_noise
	)

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

Inertial Navigation

Kalman Filter

setPosition()

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

set the position

setRotation()

void KFD_PosVelAcc::setRotation

(

Eigen::Quaterniond

R

)

velocityObservation()

bool KFD_PosVelAcc::velocityObservation	(	Eigen::Vector3d	velocity,
		Eigen::Matrix3d	covariance,
		float	reject_velocity_threshol
	)

A 3D velocity observation

Member Data Documentation

_POS_DEGREE_OF_FREEDOM

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

static

_POS_MEASUREMENT_SIZE

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

static

ensure alignment for eigen vector types

_VEL_DEGREE_OF_FREEDOM

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

static

_VEL_MEASUREMENT_SIZE

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

static

filter

KalmanFilter::KF<StatePosVelAcc::SIZE>* pose_ekf::KFD_PosVelAcc::filter

Instance of the Extended Kalman filter

position_world

Eigen::Vector3d pose_ekf::KFD_PosVelAcc::position_world

Q

Eigen::Matrix<double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE> pose_ekf::KFD_PosVelAcc::Q

process noise

R_input_to_world

Eigen::Quaterniond pose_ekf::KFD_PosVelAcc::R_input_to_world

velocity_world

Eigen::Vector3d pose_ekf::KFD_PosVelAcc::velocity_world

estimated position, velocity

x

StatePosVelAcc pose_ekf::KFD_PosVelAcc::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_PosVelAcc.hpp
• /build/rock-master-18.09-slam-pose-ekf-0.20121031/src/KFD_PosVelAcc.cpp