KalmanConstantSpeedModel Class Reference

#include <ConstantSpeedModel.h>

Inheritance diagram for KalmanConstantSpeedModel:

KalmanProcessModelBase List of all members.

Public Member Functions

 KalmanConstantSpeedModel ()
virtual ~KalmanConstantSpeedModel ()
virtual const char * getModelName () const
virtual int getStateDim () const
virtual void getP (double *pDest) const
virtual void getQ (double *pDest) const
virtual void setQ (const double *pSource)
virtual void getR (double *pDest) const
virtual void setR (double *pSource)
virtual void reset ()
virtual KalmanUpdateResult update (double time, double x, double y, const RobotPose &pose, double panningVelocity)
virtual KalmanPredictResult predict (double time)
virtual void adapt (const OdometryData &lastOdometry, const OdometryData &actualOdometry)

Private Types

 NotInited
 PositionInited
 Ready
enum  InitState { NotInited, PositionInited, Ready }

Private Member Functions

void calculateVarianceAdaptionPolynom (double x[2], double fx[2], double result[2])

Private Attributes

const double panningVelocityThreshold
Vector_n< double, 4 > x_act
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]
double varianceAdaptionSpeed [3]

Static Private Attributes

static double defaultA [16]
 default linear process model matrix
static double defaultP [16]
 default internal covariance matrix
static double defaultQ [16]
 default process error covariance matrix
static 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]

Enumerator:
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, reset(), varianceAdaptionPosition, and varianceAdaptionSpeed.

KalmanConstantSpeedModel::~KalmanConstantSpeedModel (  )  [virtual]

Definition at line 82 of file ConstantSpeedModel.cpp.


Member Function Documentation

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, Vector2< V >::y, and z.

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, b, and Matrix_nxn< T, N >::solve().

Referenced by KalmanConstantSpeedModel().

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 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< T, N >::copyTo(), and P.

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< T, N >::copyTo(), and 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< T, N >::copyTo(), and globR.

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.

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, lastTime, KalmanPredictResult::liklihood, KalmanProcessModelBase::OutputException(), Ready, reset(), KalmanPredictResult::state, KalmanBallState::vx, KalmanBallState::vy, KalmanBallState::x, x_act, x_predict, and KalmanBallState::y.

void KalmanConstantSpeedModel::reset (  )  [virtual]

Resets the process model

Implements KalmanProcessModelBase.

Definition at line 102 of file ConstantSpeedModel.cpp.

References defaultP, initState, lastLiklihood, lastTime, NotInited, P, and x_act.

Referenced by KalmanConstantSpeedModel(), predict(), and update().

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::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.

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(), MVException::DetCloseToZero, MVException::DetNegative, MVTools::getMaxExpDouble(), globQ, globR, idText, initState, invert(), MVTools::isNearZero(), K, lastLiklihood, lastTime, KalmanUpdateResult::liklihood, LINE, NotInited, OUTPUT, KalmanProcessModelBase::OutputException(), P, P_minus, panningVelocityThreshold, KalmanProcessModelBase::pi, Drawings::pink, PositionInited, Q, R, Ready, Geometry::relative2FieldCoord(), reset(), KalmanUpdateResult::state, text, Matrix2x2< V >::transpose(), transpose(), varianceAdaptionPosition, varianceAdaptionSpeed, KalmanBallState::vx, KalmanBallState::vy, Vector2< V >::x, KalmanBallState::x, x_act, x_minus, Vector2< V >::y, KalmanBallState::y, and z.


Member Data Documentation

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::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().

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 103 of file ConstantSpeedModel.h.

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 104 of file ConstantSpeedModel.h.

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 105 of file ConstantSpeedModel.h.

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 106 of file ConstantSpeedModel.h.

Referenced by KalmanConstantSpeedModel().

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().

InitState KalmanConstantSpeedModel::initState [private]

initialization state of the filter

Definition at line 79 of file ConstantSpeedModel.h.

Referenced by predict(), reset(), and update().

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

Definition at line 86 of file ConstantSpeedModel.h.

Referenced by 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.

Referenced by predict(), reset(), 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::P_minus [private]

Definition at line 85 of file ConstantSpeedModel.h.

Referenced by update().

const double KalmanConstantSpeedModel::panningVelocityThreshold [private]

Definition at line 57 of file ConstantSpeedModel.h.

Referenced by 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().

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().

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::x_minus [private]

Definition at line 84 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().

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

measured state

Definition at line 67 of file ConstantSpeedModel.h.

Referenced by adapt(), and update().


The documentation for this class was generated from the following files:
Generated on Thu Dec 7 01:40:42 2006 for DT2005.panorama by  doxygen 1.4.7