/**
* @Brief - Main interface, takes in an update with a visionBall and a motion message
* Should be called whenever new information, and simply dont pass it the same
* message twice!
* @params - visionball is a NEW vision message
- motion is a NEW motion message
*/
void MMKalmanFilter::update(messages::VBall visionBall,
messages::RobotLocation odometry)
{
predictFilters(odometry);
// Update with sensor if we have made an observation
// sometimes get weird ball observatoins so check for valid dist (indicator)
if(visionBall.on() && (visionBall.distance() > 5) && (visionBall.distance() < 800)) // We see the ball
{
//Before we mess with anything, decide if we saw the ball twice in a row
consecutiveObservation = (framesWithoutBall == 0) ? true : false;
// Update our visual observation history
lastVisRelX = visRelX;
lastVisRelY = visRelY;
//Calc relx and rely from vision
float sinB,cosB;
sincosf(visionBall.bearing(),&sinB,&cosB);
visRelX = visionBall.distance()*cosB;
visRelY = visionBall.distance()*sinB;
// Update our observation buffer for re-initializing the moving filter
curEntry = (curEntry+1)%params.bufferSize;
obsvBuffer[curEntry] = CartesianObservation(visRelX, visRelY);
if (!fullBuffer && curEntry==0)
{ // If buffer wasnt full but is now
fullBuffer = true;
}
// If we havent seen the ball for a long time, re-init our filters
if (framesWithoutBall >= params.framesTillReset)
{
// Reset the filters
initialize(visRelX, visRelY, params.initCovX, params.initCovY);
// Reset relevant variables
framesWithoutBall = 0;
}
// #HACK for competition - If we get into a bad observation cycle then change it
else if (filters.at((unsigned)1)->getSpeed() > 700.f && consecutiveObservation){
initialize(visRelX, visRelY, params.initCovX, params.initCovY);
}
// else if (fullBuffer) {
// // Calc velocity through these frames, if high reset moving filter
// CartesianObservation vel = calcVelocityOfBuffer();
// float speedThroughFrames = calcSpeed(vel.relX, vel.relY);
// // Calc diff between observation and estimata
// float estDiff = calcSpeed(visRelX - filters.at((unsigned)0)->getRelXPosEst(),
// visRelY - filters.at((unsigned)0)->getRelYPosEst());
// // Much higher than our current estimate
// // if ((speedThroughFrames > filters.at((unsigned)1)->getSpeed() + 60.f)
// // && (estDiff > params.badStationaryThresh))
// //if(estDiff > params.badStationaryThresh)
// // If moving velocity <10, give this a try
// if (std::abs(filters.at((unsigned)1)->getSpeed()) < 10.f
// && speedThroughFrames > 10.f)
// {
// //std::cout << "\nBall Kicked!" << std::endl;
// ufvector4 newMovingX = filters.at((unsigned)0)->getStateEst();
// newMovingX(2) = vel.relX;
// newMovingX(3) = vel.relY;
// ufmatrix4 newMovingCov = boost::numeric::ublas::identity_matrix <float>(4);
// newMovingCov(0,0) = 10.f;
// newMovingCov(1,1) = 10.f;
// newMovingCov(2,2) = 20.f;
// newMovingCov(3,3) = 20.f;
// filters.at((unsigned)1)->initialize(newMovingX, newMovingCov);
// }
// }
// Now correct our filters with the vision observation
updateWithVision(visionBall);
updatePredictions();
}
else {
consecutiveObservation = false;
// don't use/wipeout buffer
fullBuffer = false;
curEntry = 0;
}
// Determine filter
if(TRACK_MOVEMENT) {
float movingScore = filters.at((unsigned)1)->getScore();
float stationaryScore = filters.at((unsigned)0)->getScore();
if( (movingScore < stationaryScore))
{ // consider the ball to be moving
bestFilter = 1;
}
else {
bestFilter = 0;
}
}
else {
bestFilter = 0;
}
// Now update our estimates before housekeeping
prevStateEst = stateEst;
prevCovEst = covEst;
stateEst = filters.at((unsigned)bestFilter)->getStateEst();
covEst = filters.at((unsigned)bestFilter)->getCovEst();
// Housekeep
framesWithoutBall = (visionBall.on()) ? (0) : (framesWithoutBall+1);
stationary = filters.at((unsigned)bestFilter)->isStationary();
}
/**
* @Brief - Main interface, takes in an update with a visionBall and a motion message
* Should be called whenever new information, and simply dont pass it the same
* message twice!
* @params - visionball is a NEW vision message
- motion is a NEW motion message
*/
void MMKalmanFilter::update(messages::VisionBall visionBall,
messages::RobotLocation odometry)
{
// Predict the filters!
// for(unsigned i=0; i<filters.size(); i++)
// filters.at(i)->printEst();
predictFilters(odometry);
// std::cout << "Predicted Filters" << std::endl;
// for(unsigned i=0; i<filters.size(); i++)
// filters.at(i)->printEst();
// Update with sensor if we have made an observation
if(visionBall.on()) // We see the ball
{
// std::cout << "BALL SEEN ----------------------------------------" << std::endl;
//Before we mess with anything, decide if we saw the ball twice in a row
consecutiveObservation = (framesWithoutBall == 0) ? true : false;
// Update our visual observation history
lastVisRelX = visRelX;
lastVisRelY = visRelY;
//Calc relx and rely from vision
float sinB,cosB;
sincosf(visionBall.bearing(),&sinB,&cosB);
visRelX = visionBall.distance()*cosB;
visRelY = visionBall.distance()*sinB;
// If we havent seen the ball for a long time, re-init our filters
if (framesWithoutBall >= params.framesTillReset && consecutiveObservation)
{
// Reset the filters
initialize(visRelX, visRelY, params.initCovX, params.initCovY);
// Reset relevant variables
framesWithoutBall = 0;
}
// #HACK for competition - If we get into a bad observation cycle then change it
if (filters.at((unsigned)1)->getSpeed() > 700.f && consecutiveObservation)
initialize(visRelX, visRelY, params.initCovX, params.initCovY);
// Now correct our filters with the vision observation
updateWithVision(visionBall);
//Normalize all of the filter weights, currently ignore 'best' weight
normalizeFilterWeights();
updatePredictions();
}
else // Didn't see the ball, need to know for when we cycle
consecutiveObservation = false;
// #HACK - shouldnt know how many filters there are but... US OPEN!
//Determine if we are using the stationary
// std::cout << "Velocity Mag:\t" << filters.at((unsigned)1)->getSpeed() << std::endl;
if (filters.at((unsigned)1)->getSpeed() > params.movingThresh)
{ // consider the ball to be moving
bestFilter = 1;
}
else {
bestFilter = 0;
}
// Now update our estimates before housekeeping
prevStateEst = stateEst;
prevCovEst = covEst;
stateEst = filters.at((unsigned)bestFilter)->getStateEst();
covEst = filters.at((unsigned)bestFilter)->getCovEst();
/** commented out due to using only 2 filters **/
// Kill the two worst estimates and re-init them if we made an observation
// if(visionBall.on())
// cycleFilters();
// Housekeep
framesWithoutBall = (visionBall.on()) ? (0) : (framesWithoutBall+1);
stationary = filters.at((unsigned)bestFilter)->isStationary();
}
