KalmanConstantSpeedModel Class Reference

Contains a KalmanBallLocator process model for balls with constant speed. More...

#include <ConstantSpeedModel.h>

Inheritance diagram for KalmanConstantSpeedModel:

[legend]Collaboration diagram for KalmanConstantSpeedModel:

[legend]List of all members.


Public Member Functions
	KalmanConstantSpeedModel ()
virtual	~KalmanConstantSpeedModel ()
virtual const char *	getModelName () const
	Returns the name of the process model.
virtual int	getStateDim () const
	Returns the dimension of the state.
virtual void	getP (double *pDest) const
	Retrieves the internal kalman filter covariance matrix.
virtual void	getQ (double *pDest) const
	Retrieves the process error covariance matrix.
virtual void	setQ (const double *pSource)
	Sets the process error covariance matrix.
virtual void	getR (double *pDest) const
	Retrieves the measurement error covariance matrix.
virtual void	setR (double *pSource)
	Sets the measurement error covariance matrix.
virtual void	reset ()
	Resets the process model.
virtual KalmanUpdateResult	update (double time, double x, double y, const RobotPose &pose, double panningVelocity)
	Performs an update step of the process model.
virtual KalmanPredictResult	predict (double time)
	Permorms a prediction of the ball state.
virtual void	adapt (const OdometryData &lastOdometry, const OdometryData &actualOdometry)
	Function to adapt the state of the model when position and rotation of the robot changes.
Private Types
enum	InitState { NotInited, PositionInited, Ready }
Private Member Functions
void	calculateVarianceAdaptionPolynom (double x[2], double fx[2], double result[2])
	Calculates the coefficients for the variance adpation Polynom.
Private Attributes
const double	panningVelocityThreshold
Vector_n< double, 4 >	x_act
	Actual state: x_act[0] is x-position in meters relative to robot x_act[1] is y-position in meters relative to robot x_act[2] is x-direction speed in meters per second x_act[3] is y-direction speed in meters per second.
Vector_n< double, 4 >	z
	measured state
Matrix_nxn< double, 4 >	A
	linear process model matrix
Matrix_nxn< double, 4 >	P
	internal covariance matrix
Matrix_nxn< double, 4 >	Q
	process model error covariance matrix
Matrix_nxn< double, 4 >	R
	measurement error covariance matrix
InitState	initState
	initialization state of the filter
double	lastLiklihood
	last liklihood of model (for predict function)
double	lastTime
	last time update permormed
Vector_n< double, 4 >	x_minus
Matrix_nxn< double, 4 >	P_minus
Matrix_nxn< double, 4 >	K
Vector_n< double, 4 >	x_predict
Matrix_nxn< double, 4 >	C
Matrix_nxn< double, 4 >	C_inv
Matrix_nxn< double, 4 >	globQ
	global process covariance matrix that is adapted with dt
Matrix_nxn< double, 4 >	globR
	global measurement covariance matrix
double	varianceAdaptionPosition [3]
	Coefficients of polynom of degree 2 for variance adaption Constant coefficient is the last one.
double	varianceAdaptionSpeed [3]
Static Private Attributes
double	defaultA [16]
	default linear process model matrix
double	defaultP [16]
	default internal covariance matrix
double	defaultQ [16]
	default process error covariance matrix
double	defaultR [16]
	default measurement error covariance matrix

Detailed Description

Contains a KalmanBallLocator process model for balls with constant speed.

Definition at line 22 of file ConstantSpeedModel.h.

Member Enumeration Documentation

enum KalmanConstantSpeedModel::InitState [private]

Enumeration values:

NotInited

PositionInited

Ready

Definition at line 73 of file ConstantSpeedModel.h.

Constructor & Destructor Documentation

KalmanConstantSpeedModel::KalmanConstantSpeedModel ( )

Definition at line 48 of file ConstantSpeedModel.cpp.
References A, calculateVarianceAdaptionPolynom(), defaultA, defaultQ, defaultR, globQ, globR, pi, reset(), varianceAdaptionPosition, and varianceAdaptionSpeed.

Here is the call graph for this function:

KalmanConstantSpeedModel::~KalmanConstantSpeedModel ( ) [virtual]

Definition at line 82 of file ConstantSpeedModel.cpp.

Member Function Documentation

virtual const char* KalmanConstantSpeedModel::getModelName ( ) const [inline, virtual]

Returns the name of the process model.

Returns:
name of process model

Implements KalmanProcessModelBase.
Definition at line 29 of file ConstantSpeedModel.h.

virtual int KalmanConstantSpeedModel::getStateDim ( ) const [inline, virtual]

Returns the dimension of the state.

Returns:
dimension of the state

Implements KalmanProcessModelBase.
Definition at line 31 of file ConstantSpeedModel.h.

virtual void KalmanConstantSpeedModel::getP ( double * pDest ) const [inline, virtual]

Retrieves the internal kalman filter covariance matrix.

Parameters:

pDest destination to store the covariance matrix to

Implements KalmanProcessModelBase.
Definition at line 32 of file ConstantSpeedModel.h.
References Matrix_nxn< double, 4 >::copyTo(), and P.

Here is the call graph for this function:

virtual void KalmanConstantSpeedModel::getQ ( double * pDest ) const [inline, virtual]

Retrieves the process error covariance matrix.

Parameters:

pDest destination to store the covariance matrix to

Implements KalmanProcessModelBase.
Definition at line 33 of file ConstantSpeedModel.h.
References Matrix_nxn< double, 4 >::copyTo(), and globQ.

Here is the call graph for this function:

virtual void KalmanConstantSpeedModel::setQ ( const double * pSource ) [inline, virtual]

Sets the process error covariance matrix.

Parameters:

pSource array containing the covariance matrix entries

Implements KalmanProcessModelBase.
Definition at line 34 of file ConstantSpeedModel.h.
References globQ.

virtual void KalmanConstantSpeedModel::getR ( double * pDest ) const [inline, virtual]

Retrieves the measurement error covariance matrix.

Parameters:

pDest destination to store the covariance matrix to

Implements KalmanProcessModelBase.
Definition at line 35 of file ConstantSpeedModel.h.
References Matrix_nxn< double, 4 >::copyTo(), and globR.

Here is the call graph for this function:

virtual void KalmanConstantSpeedModel::setR ( double * pSource ) [inline, virtual]

Sets the measurement error covariance matrix.

Parameters:

pSource array containing the covariance matrix entries

Implements KalmanProcessModelBase.
Definition at line 36 of file ConstantSpeedModel.h.
References globR.

void KalmanConstantSpeedModel::reset ( ) [virtual]

Resets the process model.

Implements KalmanProcessModelBase.
Definition at line 102 of file ConstantSpeedModel.cpp.
References defaultP, initState, lastLiklihood, NotInited, P, and x_act.
Referenced by KalmanConstantSpeedModel(), predict(), and update().

KalmanUpdateResult KalmanConstantSpeedModel::update ( double time,

double x,

double y,

const RobotPose & pose,

double panningVelocity

) [virtual]

Performs an update step of the process model.

Parameters:

time time in seconds when the percepts were determined

x measured x-position of the ball in meters relative to the robot

y measured y-position of the ball in meters relative to the robot

pose the current robot pose, used for visualization

panningVelocity the head panning angular velocity, used to adjust the R covariance matrix

Returns:
the update results of the process model

Implements KalmanProcessModelBase.
Definition at line 113 of file ConstantSpeedModel.cpp.
References A, C, C_inv, det(), Matrix_nxn< double, 4 >::eye(), MVTools::getMaxExpDouble(), globQ, globR, idText, initState, invert(), MVTools::isNearZero(), K, lastLiklihood, KalmanUpdateResult::liklihood, LINE, NotInited, OUTPUT, KalmanProcessModelBase::OutputException(), P, P_minus, panningVelocityThreshold, KalmanProcessModelBase::pi, PositionInited, Q, R, Ready, Geometry::relative2FieldCoord(), reset(), KalmanUpdateResult::state, Matrix2x2< V >::transpose(), transpose(), varianceAdaptionPosition, varianceAdaptionSpeed, KalmanBallState::vx, KalmanBallState::vy, Vector2< V >::x, KalmanBallState::x, x_act, x_minus, Vector2< V >::y, and KalmanBallState::y.

Here is the call graph for this function:

KalmanPredictResult KalmanConstantSpeedModel::predict ( double time ) [virtual]

Permorms a prediction of the ball state.

Parameters:

time actual time in seconds

Returns:
the predict results of the process model

Implements KalmanProcessModelBase.
Definition at line 328 of file ConstantSpeedModel.cpp.
References A, initState, lastLiklihood, KalmanPredictResult::liklihood, KalmanProcessModelBase::OutputException(), Ready, reset(), KalmanPredictResult::state, KalmanBallState::vx, KalmanBallState::vy, KalmanBallState::x, x_act, x_predict, and KalmanBallState::y.

Here is the call graph for this function:

void KalmanConstantSpeedModel::adapt ( const OdometryData & lastOdometry,

const OdometryData & actualOdometry

) [virtual]

Function to adapt the state of the model when position and rotation of the robot changes.

Parameters:

lastOdometry last odometry of robot (original values in millimeters are used)

actualOdometry actual odometry of robot (original values in millimeters are used)

Implements KalmanProcessModelBase.
Definition at line 366 of file ConstantSpeedModel.cpp.
References Pose2D::getAngle(), Vector2< V >::x, x_act, and Vector2< V >::y.

Here is the call graph for this function:

void KalmanConstantSpeedModel::calculateVarianceAdaptionPolynom ( double x[2],

double fx[2],

double result[2]

) [private]

Calculates the coefficients for the variance adpation Polynom.

Parameters:

x The x values to take

fx The f(x) values at the x positions

result The resultung coefficients

Definition at line 86 of file ConstantSpeedModel.cpp.
References A, and Matrix_nxn< T, N >::solve().
Referenced by KalmanConstantSpeedModel().

Here is the call graph for this function:

Member Data Documentation

const double KalmanConstantSpeedModel::panningVelocityThreshold [private]

Definition at line 57 of file ConstantSpeedModel.h.
Referenced by update().

Vector_n<double, 4> KalmanConstantSpeedModel::x_act [private]

Actual state: x_act[0] is x-position in meters relative to robot x_act[1] is y-position in meters relative to robot x_act[2] is x-direction speed in meters per second x_act[3] is y-direction speed in meters per second.

Definition at line 66 of file ConstantSpeedModel.h.
Referenced by adapt(), predict(), reset(), and update().

Vector_n<double, 4> KalmanConstantSpeedModel::z [private]

measured state

Definition at line 67 of file ConstantSpeedModel.h.

Matrix_nxn<double, 4> KalmanConstantSpeedModel::A [private]

linear process model matrix

Definition at line 68 of file ConstantSpeedModel.h.
Referenced by calculateVarianceAdaptionPolynom(), KalmanConstantSpeedModel(), predict(), and update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::P [private]

internal covariance matrix

Definition at line 69 of file ConstantSpeedModel.h.
Referenced by getP(), reset(), and update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::Q [private]

process model error covariance matrix

Definition at line 70 of file ConstantSpeedModel.h.
Referenced by update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::R [private]

measurement error covariance matrix

Definition at line 71 of file ConstantSpeedModel.h.
Referenced by update().

InitState KalmanConstantSpeedModel::initState [private]

initialization state of the filter

Definition at line 79 of file ConstantSpeedModel.h.
Referenced by predict(), reset(), and update().

double KalmanConstantSpeedModel::lastLiklihood [private]

last liklihood of model (for predict function)

Definition at line 80 of file ConstantSpeedModel.h.
Referenced by predict(), reset(), and update().

double KalmanConstantSpeedModel::lastTime [private]

last time update permormed

Definition at line 81 of file ConstantSpeedModel.h.

Vector_n<double, 4> KalmanConstantSpeedModel::x_minus [private]

Definition at line 84 of file ConstantSpeedModel.h.
Referenced by update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::P_minus [private]

Definition at line 85 of file ConstantSpeedModel.h.
Referenced by update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::K [private]

Definition at line 86 of file ConstantSpeedModel.h.
Referenced by update().

Vector_n<double, 4> KalmanConstantSpeedModel::x_predict [private]

Definition at line 87 of file ConstantSpeedModel.h.
Referenced by predict().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::C [private]

Definition at line 88 of file ConstantSpeedModel.h.
Referenced by update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::C_inv [private]

Definition at line 89 of file ConstantSpeedModel.h.
Referenced by update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::globQ [private]

global process covariance matrix that is adapted with dt

Definition at line 92 of file ConstantSpeedModel.h.
Referenced by getQ(), KalmanConstantSpeedModel(), setQ(), and update().

Matrix_nxn<double, 4> KalmanConstantSpeedModel::globR [private]

global measurement covariance matrix

Definition at line 94 of file ConstantSpeedModel.h.
Referenced by getR(), KalmanConstantSpeedModel(), setR(), and update().

double KalmanConstantSpeedModel::varianceAdaptionPosition[3] [private]

Coefficients of polynom of degree 2 for variance adaption Constant coefficient is the last one.

Definition at line 100 of file ConstantSpeedModel.h.
Referenced by KalmanConstantSpeedModel(), and update().

double KalmanConstantSpeedModel::varianceAdaptionSpeed[3] [private]

Definition at line 101 of file ConstantSpeedModel.h.
Referenced by KalmanConstantSpeedModel(), and update().

double KalmanConstantSpeedModel::defaultA [static, private]

Initial value:
{ 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 }
default linear process model matrix

Definition at line 13 of file ConstantSpeedModel.cpp.
Referenced by KalmanConstantSpeedModel().

double KalmanConstantSpeedModel::defaultP [static, private]

Initial value:
{ 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 }
default internal covariance matrix

Definition at line 21 of file ConstantSpeedModel.cpp.
Referenced by reset().

double KalmanConstantSpeedModel::defaultQ [static, private]

Initial value:
{ 0.004, 0.000, 0.004, 0.000, 0.000, 0.004, 0.000, 0.004, 0.004, 0.000, 0.004, 0.000, 0.000, 0.004, 0.000, 0.004 }
default process error covariance matrix

Definition at line 31 of file ConstantSpeedModel.cpp.
Referenced by KalmanConstantSpeedModel().

double KalmanConstantSpeedModel::defaultR [static, private]

Initial value:
{ 0.01*0.01, 0.00*0.00, 0.00*0.00, 0.00*0.00, 0.00*0.00, 0.01*0.01, 0.00*0.00, 0.00*0.00, 0.00*0.00, 0.00*0.00, 0.03*0.03, 0.00*0.00, 0.00*0.00, 0.00*0.00, 0.00*0.00, 0.03*0.03 }
default measurement error covariance matrix

Definition at line 40 of file ConstantSpeedModel.cpp.
Referenced by KalmanConstantSpeedModel().

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

Modules/BallLocator/GT2004ProcessModels/ConstantSpeedModel.h
Modules/BallLocator/GT2004ProcessModels/ConstantSpeedModel.cpp

Generated on Thu Sep 23 20:09:00 2004 for GT2004 by

1.3.6

KalmanConstantSpeedModel Class Reference

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation