jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER > Class Template Reference

Constant velocity model robot class. More...

---

Detailed Description

template<unsigned LIN_ORDER, unsigned ANG_ORDER>
class jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >

Constant velocity model robot class.

The SLAM origin is the camera origin.

Author:

croussil

This class implements a rigid frame in 3D moving with a constant position/velocity/acceleration/jerk motion model. This model is the following:

• p += v*dt <-- position
  
• q = q**(w*dt) <-- orientation quaternion
  
• v += vi <-- linear velocity
  
• w += wi <-- angular velocity

where vi and wi are linear and angular velocity random impulses, and ** is the quaternion product.

This model is embedded into the system variables as follows:

• the prediction function is x+ = f(x,u), implemented with method move().
• the state vector is x = state.x = [p q v w]
• the state covariance is in the map, P = state.P = (map.filter).P(state.ia, state.ia)
• the control vector is u = control.x = [vi wi]
• the control covariances matrix is U = control.P
• the Jacobians of f(x,u) provided by move() are XNEW_x and XNEW_control.
• the perturbation covariance is obtained with the method computeStatePerturbation(), as follows:
  • Q = XNEW_control * control.P * XNEW_control'
    

See also:

Explore the comments in file robotConstantVelocity.cpp for full algebraic details.

Definition at line 61 of file robotConstantMotionModel.hpp.

#include <robotConstantMotionModel.hpp>

Inheritance diagram for jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >:

List of all members.

Classes
struct	TempVariables
Public Member Functions
	RobotConstantMotionModel (const map_ptr_t &_mapPtr)
	Remote constructor from remote map.
virtual std::string	typeName () const
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)
virtual void	writeLogHeader (kernel::DataLogger &log) const
	Implements this method calling repeatidly log methods.
virtual void	writeLogData (kernel::DataLogger &log) const
	Implements this method calling repeatidly log methods.
Static Public Member Functions
static size_t	size ()
static size_t	size_control ()
static size_t	size_perturbation ()
Protected Member Functions
template<class Vx , class Vp , class Vq , class Vv , class Vw , class Vva , class Vwa , class Vvj , class Vwj >
void	splitState (const Vx x, Vp &p, Vq &q, Vv &v, Vw &w, Vva &va, Vwa &wa, Vvj &vj, Vwj &wj) const
	Split state vector.
template<class Vp , class Vq , class Vv , class Vw , class Vva , class Vwa , class Vvj , class Vwj , class Vx >
void	unsplitState (const Vp &p, const Vq &q, const Vv &v, const Vw &w, const Vva &va, const Vwa &wa, const Vvj &vj, const Vwj &wj, 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
ublas::range	iaPos
ublas::range	iaOriQuat
ublas::range	iaAbsVel
ublas::range	iaAngVel
ublas::range	iaAbsAcc
ublas::range	iaAngAcc
ublas::range	iaAbsJerk
ublas::range	iaAngJerk
TempVariables	tempvars0
TempVariables	tempvars1

---

Constructor & Destructor Documentation

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::RobotConstantMotionModel

(

const map_ptr_t &

_mapPtr

)

Remote constructor from remote map.

Parameters:

_map	the remote map

---

Member Function Documentation

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

void jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::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.

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

template<class Vu , class V >

void jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::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 207 of file robotConstantMotionModel.hpp.

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

template<class Vx , class Vp , class Vq , class Vv , class Vw , class Vva , class Vwa , class Vvj , class Vwj >

void jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::splitState	(	const Vx	x,
		Vp &	p,
		Vq &	q,
		Vv &	v,
		Vw &	w,
		Vva &	va,
		Vwa &	wa,
		Vvj &	vj,
		Vwj &	wj
	)		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

Definition at line 148 of file robotConstantMotionModel.hpp.

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

template<class Vp , class Vq , class Vv , class Vw , class Vva , class Vwa , class Vvj , class Vwj , class Vx >

void jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::unsplitState	(	const Vp &	p,
		const Vq &	q,
		const Vv &	v,
		const Vw &	w,
		const Vva &	va,
		const Vwa &	wa,
		const Vvj &	vj,
		const Vwj &	wj,
		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

Definition at line 179 of file robotConstantMotionModel.hpp.

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

virtual void jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::writeLogData

(

kernel::DataLogger &

log

)

const [virtual]

Implements this method calling repeatidly log methods.

You should use writeData() or writeDataVector().

Reimplemented from jafar::rtslam::RobotAbstract.

template<unsigned LIN_ORDER, unsigned ANG_ORDER>

virtual void jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >::writeLogHeader

(

kernel::DataLogger &

log

)

const [virtual]

Implements this method calling repeatidly log methods.

You should use writeComment(), writeLegend() or writeLegendTokens().

Reimplemented from jafar::rtslam::RobotAbstract.

---

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

• modules/rtslam/include/rtslam/robotConstantMotionModel.hpp