jafar::rtslam::RobotCenteredConstantVelocity Class Reference

RobotCentric Constant velocity model robot class. More...

---

Detailed Description

RobotCentric Constant velocity model robot class.

Author:

jmcodol

This class implements a rigid frame in 3D moving with a constant velocity motion model.

From [Bounding Uncertaincy in EKF-SLAM. The Robocentric Local Approach], Ruben Martinez-Cantin, Jose A. Castellanos

Definition at line 35 of file robotCenteredConstantVelocity.hpp.

#include <robotCenteredConstantVelocity.hpp>

Inheritance diagram for jafar::rtslam::RobotCenteredConstantVelocity:

List of all members.

Classes
struct	TempVariables
Public Member Functions
	RobotCenteredConstantVelocity (const map_ptr_t &_mapPtr)
	Remote constructor from remote map.
virtual std::string	typeName () const
void	getFrameForReframing (vec7 &frame)
void	move_func (const vec &_x, const vec &_u, const vec &_n, const double _dt, vec &_xnew, mat &_XNEW_x, mat &_XNEW_pert, unsigned tempSet=0) const
	Move one step ahead.
void	computePertJacobian ()
virtual size_t	mySize ()
virtual size_t	mySize_control ()
virtual size_t	mySize_perturbation ()
void	setVelocityStd (double velLinStd, double velAngStd)
	we have : robot state = r = [ R2 ] = [ x_Rk^Rk-1 ] = [ pose ] [ R1 ] [ x_B^Rk-1 ] [ -base ]
Static Public Member Functions
static size_t	size ()
	The size of the robot in map.
static size_t	size_control ()
static size_t	size_perturbation ()
Protected Member Functions
template<class Vx , class Vp , class Vq , class Vv , class Vw >
void	splitState (const Vx x, Vp &p, Vq &q, Vv &v, Vw &w, Vp &pBase, Vq &qBase) const
	Split state vector.
template<class Vp , class Vq , class Vv , class Vw , class Vx >
void	unsplitState (const Vp &p, const Vq &q, const Vv &v, const Vw &w, const Vp &pBase, const Vq &qBase, Vx &x) const
	Compose state vector.
template<class Vu , class V >
void	splitControl (const Vu &u, V &vi, V &wi) const
	Split control vector.
Private Attributes
TempVariables	tempvars0
TempVariables	tempvars1

---

Constructor & Destructor Documentation

jafar::rtslam::RobotCenteredConstantVelocity::RobotCenteredConstantVelocity

(

const map_ptr_t &

_mapPtr

)

Remote constructor from remote map.

Parameters:

_map	the remote map

---

Member Function Documentation

void jafar::rtslam::RobotCenteredConstantVelocity::move_func	(	const vec &	_x,
		const vec &	_u,
		const vec &	_n,
		const double	_dt,
		vec &	_xnew,
		mat &	_XNEW_x,
		mat &	_XNEW_pert,
		unsigned	tempSet = 0
	)		const [virtual]

Move one step ahead.

This function predicts the robot state one step of length dt ahead in time, according to the control input control.x and the time interval control.dt.

Parameters:

_x	the current state vecto
_p	the perturbation vector
_dt	the sampling time
_xnew	the new state vector
_XNEW_x	the Jacobian of xnew wrt x
_XNEW_pert	the Jacobian of xnew wrt p

Implements jafar::rtslam::RobotAbstract.

void jafar::rtslam::RobotCenteredConstantVelocity::setVelocityStd	(	double	velLinStd,
		double	velAngStd
	)		[inline]

we have : robot state = r = [ R2 ] = [ x_Rk^Rk-1 ] = [ pose ] [ R1 ] [ x_B^Rk-1 ] [ -base ]

Definition at line 111 of file robotCenteredConstantVelocity.hpp.

References jafar::rtslam::RobotAbstract::pose, and jafar::rtslam::MapObject::state.

static size_t jafar::rtslam::RobotCenteredConstantVelocity::size

(

void

)

[inline, static]

The size of the robot in map.

Warning, there is no respect to the article, in the sense that : in the article we have : x = [ x_B^Rk-1 ] = [ R1 ] We are, in our case, using : R = [ R2 ] = [ x_Rk^Rk-1 ] [ x_F^Rk-1 ] [ L ] [ R1 ] [ x_B^Rk-1 ] [ x_Rk^Rk-1 ] [ R2 ] L = [ L ] = [ x_F^Rk-1 ] the robot contains :

• R2 = x_Rk_Rkm1 : the pose/speed/... of the robot at time k, with respect to the frame of the robot at time k-1
• R1 = x_B_Rkm1 : the pose of the Base Frame B, with respect to the frame of the robot at time k-1.

X = [ x_Rk_Rkm1 ; x_B_Rkm1 ] ;

(see eq. 7) in article

Reimplemented from jafar::rtslam::MapObject.

Definition at line 89 of file robotCenteredConstantVelocity.hpp.

template<class Vu , class V >

void jafar::rtslam::RobotCenteredConstantVelocity::splitControl	(	const Vu &	u,
		V &	vi,
		V &	wi
	)		const [inline, protected]

Split control vector.

Extracts impulses vi and wi from the control vector.

Parameters:

vi	the linear impulse.
wi	the angular impulse.

Definition at line 211 of file robotCenteredConstantVelocity.hpp.

template<class Vx , class Vp , class Vq , class Vv , class Vw >

void jafar::rtslam::RobotCenteredConstantVelocity::splitState	(	const Vx	x,
		Vp &	p,
		Vq &	q,
		Vv &	v,
		Vw &	w,
		Vp &	pBase,
		Vq &	qBase
	)		const [inline, protected]

Split state vector.

Extracts p, q, v and w from the state vector, x = [p, q, v, w].

Parameters:

x	the state vector
p	the position
q	the quaternion
v	the linear velocity
w	the angular velocity Split state vector.

Extracts p, q, v w pBase and qBase from the state vector, x = [p, q, v, w, pBase, qBase].

Parameters:

x	the state vector
p	the position
q	the quaternion
v	the linear velocity
w	the angular velocity
pBase	the position of the base frame in the current frame
qBase	the quaternion of the base frame in the current frame

Definition at line 152 of file robotCenteredConstantVelocity.hpp.

template<class Vp , class Vq , class Vv , class Vw , class Vx >

void jafar::rtslam::RobotCenteredConstantVelocity::unsplitState	(	const Vp &	p,
		const Vq &	q,
		const Vv &	v,
		const Vw &	w,
		const Vp &	pBase,
		const Vq &	qBase,
		Vx &	x
	)		const [inline, protected]

Compose state vector.

Composes the state vector with p, q, v and w, x = [p, q, v, w].

Parameters:

p	the position
q	the quaternion
v	the linear velocity
w	the angular velocity
x	the state vector Compose state vector.

Composes the state vector with p, q, v w, pBase and qBase, x = [p, q, v, w, pBase, qBase].

Parameters:

p	the position
q	the quaternion
v	the linear velocity
w	the angular velocity
x	the state vector
pBase	the position of the base frame in the current frame
qBase	the quaternion of the base frame in the current frame

Definition at line 193 of file robotCenteredConstantVelocity.hpp.

---

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

• modules/rtslam/include/rtslam/robotCenteredConstantVelocity.hpp