#include <ConstantSpeedModel.h>
Inheritance diagram for KalmanConstantSpeedModel:
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 |
Definition at line 22 of file ConstantSpeedModel.h.
enum KalmanConstantSpeedModel::InitState [private] |
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.
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.
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.
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
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
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
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
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
Implements KalmanProcessModelBase.
Definition at line 31 of file ConstantSpeedModel.h.
KalmanPredictResult KalmanConstantSpeedModel::predict | ( | double | time | ) | [virtual] |
Permorms a prediction of the ball state
time | actual time in seconds |
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
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
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
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 |
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.
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] |
Matrix_nxn<double, 4> KalmanConstantSpeedModel::C_inv [private] |
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 }
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 }
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 }
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 }
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] |
Matrix_nxn<double, 4> KalmanConstantSpeedModel::K [private] |
double KalmanConstantSpeedModel::lastLiklihood [private] |
double KalmanConstantSpeedModel::lastTime [private] |
Matrix_nxn<double, 4> KalmanConstantSpeedModel::P [private] |
Matrix_nxn<double, 4> KalmanConstantSpeedModel::P_minus [private] |
const double KalmanConstantSpeedModel::panningVelocityThreshold [private] |
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.
Vector_n<double, 4> KalmanConstantSpeedModel::x_minus [private] |
Vector_n<double, 4> KalmanConstantSpeedModel::x_predict [private] |
Vector_n<double, 4> KalmanConstantSpeedModel::z [private] |