/** * @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(); }