jafar::rtslam::RobotInertial Class Reference

Inertial measurements unit - robot motion model. More...

---

Detailed Description

Inertial measurements unit - robot motion model.

The SLAM origin is the IMU origin.

Author:

jsola

This motion model is driven by IMU measurements and random perturbations, and defined by:

• The state vector: position, velocity, orientation quaternion, accelerometer bias, gyrometer bias, gravity:
  • x = [p q v ab wb g] , of size 19.

• The transition equation
  • x+ = move_func(x,u) -- which is implemented with internal variables, see move_func().

with u=control.x() the control vector

• u = [am, wm, abi, wbi]

stacking:

• am: the acceleration measurements, with noise
• wm: the gyrometer measurements, with noise
• abi: the accelerometer bias random walk noise (impulse)
• wbi: the gyrometer bias random walk noise (impulse)

The motion model equation x+ = f(x,u) is decomposed as: #if AVGSPEED

• p+ = p + (v + v+)/2*dt #else
• p+ = p + v*dt #endif
• q+ = q**((wm - wb)*dt + ti) <-- ** : quaternion product
• v+ = v + (R(q)*(am - ab) + g)*dt + vi <-- am and wm: IMU measurements
• ab+ = ab + abi <-- abi : random walk in acc bias with abi impulse perturbation
• wb+ = wb + wbi <-- wbi : random walk of gyro bias with wbi impulse perturbation
• g+ = g <-- g : gravity vector, constant but unknown

See also:

See the extense comments on move_func() in file robotInertial.cpp for algebraic details.

Definition at line 105 of file robotInertial.hpp.

#include <robotInertial.hpp>

Inheritance diagram for jafar::rtslam::RobotInertial:

List of all members.

Classes
struct	TempVariables
Public Member Functions
	RobotInertial (const map_ptr_t &_mapPtr)
virtual std::string	typeName () const
void	move_func (const vec &_x, const vec &_u, const vec &_n, double _dt, vec &_xnew, mat &_XNEW_x, mat &_XNEW_pert, unsigned tempSet=0) const
	Move one step ahead.
void	init_func (const vec &_x, const vec &_u, const vec &_U, vec &_xnew)
	Initialize the value of g with the average value of acceleration in the past.
virtual void	move_func ()
virtual size_t	mySize ()
virtual size_t	mySize_control ()
virtual size_t	mySize_perturbation ()
void	setInitialStd (double velLinStd, double aBiasStd, double wBiasStd, double gravStd)
void	setInitialParams (double gNorm)
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 Vab , class Vwb , class Vg >
void	splitState (const Vx &x, Vp &p, Vq &q, Vv &v, Vab &ab, Vwb &wb, Vg &g) const
	Split state vector.
template<class Vp , class Vq , class Vv , class Vab , class Vwb , class Vg , class Vx >
void	unsplitState (const Vp &p, const Vq &q, const Vv &v, const Vab &ab, const Vwb &wb, const Vg &g, Vx &x) const
	Compose state vector.
template<class Vu , class V >
void	splitControl (const Vu &u, V &am, V &wm) const
	Split control vector.
template<class Vn , class V >
void	splitPert (const Vn &n, V &an, V &wn, V &ar, V &wr) const
	Split perturbation vector.
vec	e_from_g (const vec3 &_g)
Private Attributes
double	g_norm
double	g_uncert
jblas::vec3	z_axis
ublas::range	iaPos
ublas::range	iaOriQuat
ublas::range	iaAbsVel
ublas::range	iaBiasA
ublas::range	iaBiasW
ublas::range	iaG
TempVariables	tempvars0
TempVariables	tempvars1
Static Private Attributes
static int	g_size

---

Member Function Documentation

void jafar::rtslam::RobotInertial::move_func	(	const vec &	_x,
		const vec &	_u,
		const vec &	_n,
		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. It updates the state and computes the convenient Jacobian matrices.

This motion model is driven by IMU measurements and random perturbations, and defined by:

• The state vector, x = [p q v ab wb g] , of size 19.

• The transition equation x+ = move(x,u), with u = [vi, ti, abi, wbi] the control impulse, is decomposed as: #if AVGSPEED
• p+ = p + (v + v+)/2*dt #else
• p+ = p + v*dt #endif
• q+ = q**((wm - wb)*dt + ti) <-- ** : quaternion product
• v+ = v + (R(q)*(am - ab) + g)*dt + vi <-- am and wm: IMU measurements
• ab+ = ab + abi <-- abi : random walk in acc bias with abi impulse perturbation
• wb+ = wb + wbi <-- wbi : random walk of gyro bias with wbi impulse perturbation
• g+ = g <-- g : gravity vector, constant but unknown

See also:

See the extense comments on move_func() in file robotInertial.cpp for algebraic details.

Implements jafar::rtslam::RobotAbstract.

template<class Vu , class V >

void jafar::rtslam::RobotInertial::splitControl	(	const Vu &	u,
		V &	am,
		V &	wm
	)		const [inline, protected]

Split control vector.

Extracts noisy measurements am and wm, and perturbationa ar and wr, from the control vector.

Parameters:

am	the acceleration measurements, with noise
wm	the gyrometer measurements, with noise
ar	the accelerometer bias random walk noise
wr	the gyrometer bias random walk noise

Definition at line 297 of file robotInertial.hpp.

template<class Vn , class V >

void jafar::rtslam::RobotInertial::splitPert	(	const Vn &	n,
		V &	an,
		V &	wn,
		V &	ar,
		V &	wr
	)		const [inline, protected]

Split perturbation vector.

Extracts noisy measurements am and wm, and perturbationa ar and wr, from the control vector.

Parameters:

am	the acceleration measurements, with noise
wm	the gyrometer measurements, with noise
ar	the accelerometer bias random walk noise
wr	the gyrometer bias random walk noise

Definition at line 312 of file robotInertial.hpp.

template<class Vx , class Vp , class Vq , class Vv , class Vab , class Vwb , class Vg >

void jafar::rtslam::RobotInertial::splitState	(	const Vx &	x,
		Vp &	p,
		Vq &	q,
		Vv &	v,
		Vab &	ab,
		Vwb &	wb,
		Vg &	g
	)		const [inline, protected]

Split state vector.

Extracts p, q, v, ab, wb and g from the state vector, x = [p, q, v, ab, wb, g].

Parameters:

p	the position
q	the quaternion
v	the linear velocity
ab	the accelerometer bias
wb	the gyrometer bias
g	the gravity vector

Definition at line 239 of file robotInertial.hpp.

template<class Vp , class Vq , class Vv , class Vab , class Vwb , class Vg , class Vx >

void jafar::rtslam::RobotInertial::unsplitState	(	const Vp &	p,
		const Vq &	q,
		const Vv &	v,
		const Vab &	ab,
		const Vwb &	wb,
		const Vg &	g,
		Vx &	x
	)		const [inline, protected]

Compose state vector.

Composes the state vector with p, q, v, ab, wb and g, x = [p, q, v, ab, wb, g].

Parameters:

p	the position
q	the quaternion
v	the linear velocity
ab	the accelerometer bias
wb	the gyrometer bias
g	the gravity vector

Definition at line 272 of file robotInertial.hpp.

virtual void jafar::rtslam::RobotInertial::writeLogData

(

kernel::DataLogger &

log

)

const [virtual]

Implements this method calling repeatidly log methods.

You should use writeData() or writeDataVector().

Reimplemented from jafar::rtslam::RobotAbstract.

virtual void jafar::rtslam::RobotInertial::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/robotInertial.hpp