/** * Updates the particle set according to the motion. * * @TODO Currently assumes odometry is how FakeOdometryModule creates it, * ie (delta X, delta Y, etc... * Verify when motion module is pulled * @return the updated ParticleSet. */ void MotionSystem::update(ParticleSet& particles, const messages::RobotLocation& deltaMotionInfo) { ParticleIt iter; for(iter = particles.begin(); iter != particles.end(); iter++) { Particle* particle = &(*iter); /** Should be used if odometry gives global **/ /** Should also be TESTED extensively **/ // float sinh, cosh; // sincosf(deltaMotionInfo.h() - particle->getLocation().h(), // &sinh, &cosh); // float changeX = cosh * deltaMotionInfo.x() + sinh * deltaMotionInfo.y(); // float changeY = cosh * deltaMotionInfo.y() - sinh * deltaMotionInfo.x(); float changeX = deltaMotionInfo.x(); float changeY = deltaMotionInfo.y(); float changeH = deltaMotionInfo.h(); particle->shift(changeX, changeY, changeH); randomlyShiftParticle(particle); } // std::cout << "\n\n Updated Particles w/ Motion \n"; }
QPoint FieldViewerPainter::getRelLoc(messages::RobotLocation loc, float dist, float bear) { float sin, cos; float ninetyDeg = 1.5707963; sincosf((loc.h() + bear), &sin, &cos); float relX = dist*cos + loc.x(); float relY = dist*sin + loc.y(); QPoint relLoc(relX,relY); return relLoc; }
messages::RobotLocation LandmarkSystem::relRobotToAbsolute(const messages::RobotLocation& observation, const messages::RobotLocation& loc) { // Translation rotation to absolute coordinate system double transX, transY; man::vision::translateRotate(observation.x(), observation.y(), loc.x(), loc.y(), loc.h(), transX, transY); // Return transformed robot location messages::RobotLocation transformed; transformed.set_x(transX); transformed.set_y(transY); return transformed; }
/** * Updates the particle set according to the motion. * * @return the updated ParticleSet. */ void MotionSystem::update(ParticleSet& particles, const messages::RobotLocation& odometry, float error) { // Store the last odometry and set the current one lastOdometry.set_x(curOdometry.x()); lastOdometry.set_y(curOdometry.y()); lastOdometry.set_h(curOdometry.h()); curOdometry.set_x(odometry.x()); curOdometry.set_y(odometry.y()); curOdometry.set_h(odometry.h()); // change in the robot frame float dX_R = curOdometry.x() - lastOdometry.x(); float dY_R = curOdometry.y() - lastOdometry.y(); float dH_R = curOdometry.h() - lastOdometry.h(); if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) || (std::fabs(dH_R) > 0.35f) ) { // Probably reset odometry somewhere, so skip frame // std::cout << "std::fabs(dX_R) = " << std::fabs(dX_R) << " std::fabs(dY_R) = " << std::fabs(dY_R) << std::endl; // std::cout << "curOdometry.x() = " << curOdometry.x() << " curOdometry.y() = " << curOdometry.y() << std::endl; std::cout << "Odometry reset, motion frame skipped in loc" << std::endl; return; } float dX, dY, dH; ParticleIt iter; for(iter = particles.begin(); iter != particles.end(); iter++) { Particle* particle = &(*iter); // Rotate from the robot frame to the global to add the translation float sinh, cosh; sincosf(curOdometry.h() - particle->getLocation().h(), &sinh, &cosh); dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X; dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y; dH = dH_R * FRICTION_FACTOR_H; particle->shift(dX, dY, dH); // noiseShiftWithOdo(particle, dX, dY, dH, error); randomlyShiftParticle(particle, false); } }
/** * Updates the particle set according to the motion. * * @return the updated ParticleSet. */ void MotionSystem::update(ParticleSet& particles, const messages::RobotLocation& odometry, bool nearMid) { // Store the last odometry and set the current one lastOdometry.set_x(curOdometry.x()); lastOdometry.set_y(curOdometry.y()); lastOdometry.set_h(curOdometry.h()); curOdometry.set_x(odometry.x()); curOdometry.set_y(odometry.y()); curOdometry.set_h(odometry.h()); // change in the robot frame float dX_R = curOdometry.x() - lastOdometry.x(); float dY_R = curOdometry.y() - lastOdometry.y(); float dH_R = curOdometry.h() - lastOdometry.h(); if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) ) { //Probably reset odometry somewhere so skip a frame return; } float dX, dY, dH; ParticleIt iter; for(iter = particles.begin(); iter != particles.end(); iter++) { Particle* particle = &(*iter); // Rotate from the robot frame to the global to add the translation float sinh, cosh; sincosf(curOdometry.h() - particle->getLocation().h(), &sinh, &cosh); dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X; dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y; dH = dH_R * FRICTION_FACTOR_H; // just add the rotation particle->shift(dX, dY, dH); noiseShiftWithOdo(particle, dX, dY, dH); //randomlyShiftParticle(particle, nearMid); } }
double LandmarkSystem::scoreObservation(const messages::RobotLocation& observation, const Landmark& correspondingLandmark, const messages::RobotLocation& loc, double wz0) { // Observation, cartesian to polar double rObsv, tObsv; vision::cartesianToPolar(observation.x(), observation.y(), rObsv, tObsv); // Landmark in map, absolute to robot relative double xMapRel, yMapRel; vision::inverseTranslateRotate(std::get<1>(correspondingLandmark), std::get<2>(correspondingLandmark), loc.x(), loc.y(), loc.h(), xMapRel, yMapRel); // Landmark in map, cartesian to polar double rMapRel, tMapRel; vision::cartesianToPolar(xMapRel, yMapRel, rMapRel, tMapRel); // Calculate tilt to both observation and corresponding line in map // NOTE better to score error in r in angular coordinates since this // weights close observations more highly double observationTilt = atan(rObsv / wz0); double correspondingTilt = atan(rMapRel / wz0); // Find differences in tilt and t double tiltDiff = vision::diffRadians(observationTilt, correspondingTilt); double tDiff = vision::diffRadians(tMapRel, tObsv); // Evaluate gaussian to get probability of observation from location loc // TODO params boost::math::normal_distribution<> tiltGaussian(0, 5*TO_RAD); boost::math::normal_distribution<> tGaussian(0, 20*TO_RAD); double tiltProb = pdf(tiltGaussian, tiltDiff); double tProb = pdf(tGaussian, tDiff); if (debug) { std::cout << "In scoreObservation:" << std::endl; std::cout << observation.x() << "," << observation.y() << std::endl; std::cout << rObsv << "," << tObsv << std::endl; std::cout << std::get<1>(correspondingLandmark) << "," << std::get<2>(correspondingLandmark) << std::endl; std::cout << xMapRel << "," << yMapRel << std::endl; std::cout << rMapRel << "," << tMapRel << std::endl; std::cout << tiltDiff << "," << tDiff << std::endl; std::cout << tiltProb << "/" << tProb << std::endl; std::cout << (tiltProb * tProb) << std::endl; } // Make the conditional independence assumption return tiltProb * tProb; }
void FieldViewerPainter::paintRobotLocation(QPaintEvent* event, messages::RobotLocation loc, bool red, int size) { QPainter painter(this); //Move origin to bottem left and scale to flip the y axis painter.translate(0,FIELD_GREEN_HEIGHT*scaleFactor); painter.scale(scaleFactor, -scaleFactor); if (red) painter.setBrush(Qt::red); QPoint locCenter(loc.x(), loc.y()); painter.drawEllipse(locCenter, size, size); painter.drawLine(loc.x(), loc.y(), size * std::cos(loc.h()) + loc.x(), size * std::sin(loc.h()) + loc.y()); }
void KalmanFilter::predict(messages::RobotLocation odometry, float deltaT) { float diffX = odometry.x() - lastOdometry.x(); lastOdometry.set_x(odometry.x()); float diffY = odometry.y() - lastOdometry.y(); lastOdometry.set_y(odometry.y()); float diffH = odometry.h() - lastOdometry.h(); lastOdometry.set_h(odometry.h()); float sinh, cosh; sincosf(odometry.h(), &sinh, &cosh); // change into the robot frame float dX = cosh*diffX + sinh*diffY; float dY = cosh*diffY - sinh*diffX; float dH = diffH;// * 2.4f; if( (std::fabs(dX) > 3.f) || (std::fabs(dY) > 3.f) ) { //Probably reset odometry somewhere so skip a frame dX = 0.f; dY = 0.f; dH = 0.f; } //Calculate A = rotation matrix * trajectory matrix //First calc rotation matrix float sinDeltaH, cosDeltaH; sincosf(dH,&sinDeltaH, &cosDeltaH); // Keep track of the deviation float rotationDeviation = dH * params.rotationDeviation; float sinRotDev, cosRotDev; sincosf(rotationDeviation, &sinRotDev, &cosRotDev); // Generate the rotation and rotationDeviationRotation matrices // We rotate counterclockwise for positive dH // So everything rotates clockwise around us ufmatrix4 rotation = boost::numeric::ublas::identity_matrix <float>(4); ufmatrix4 rotationDeviationRotation = boost::numeric::ublas::identity_matrix <float>(4); // nxm matrix is n rows, m columns rotation(0,0) = cosDeltaH; rotation(1,0) = -sinDeltaH; rotation(0,1) = sinDeltaH; rotation(1,1) = cosDeltaH; rotationDeviationRotation(0,0) = cosRotDev; rotationDeviationRotation(1,0) = -sinRotDev; rotationDeviationRotation(0,1) = sinRotDev; rotationDeviationRotation(1,1) = cosRotDev; if (stationary){ rotation(2,2) = 0.f; rotation(3,3) = 0.f; rotationDeviationRotation(2,2) = 0.f; rotationDeviationRotation(3,3) = 0.f; } else { // estimate assumes ball is moving rotation(2,2) = cosDeltaH; rotation(3,2) = sinDeltaH; rotation(2,3) = -sinDeltaH; rotation(3,3) = cosDeltaH; rotationDeviationRotation(2,3) = cosRotDev; rotationDeviationRotation(3,2) = sinRotDev; rotationDeviationRotation(2,3) = -sinRotDev; rotationDeviationRotation(3,3) = cosRotDev; } // Calculate the trajectory ufmatrix4 trajectory = boost::numeric::ublas::identity_matrix <float>(4); if (!stationary) { //if estimate is moving, predict to where using velocity trajectory(0,2) = deltaT; trajectory(1,3) = deltaT; } // Calculate the translation from odometry // If we go left (positive y, everything else shifts down // NOTE: Using notation from Probabilistic Robotics, B = Identity // so no need to compute it or calculate anything with it ufvector4 translation = NBMath::vector4D(-dX, -dY, 0.f, 0.f); // And deviation float xTransDev = dX * params.transXDeviation; xTransDev *= xTransDev; float yTransDev = dY * params.transYDeviation; yTransDev *= yTransDev; ufvector4 translationDeviation = NBMath::vector4D(xTransDev, yTransDev, 0.f, 0.f); // Incorporate Friction // Have params.ballFriction in cm/sec^2 // ballFriction * deltaT = impact from friction that frame in cm/sec (velocity) // in each direction, so need to add that impact if velocity is positive, // need to subtract that impact if velocity is negative // Incorporate friction if the ball is moving if (!stationary) // moving { float xVelFactor = (std::abs(x(2)) + params.ballFriction*deltaT)/std::abs(x(2)); float yVelFactor = (std::abs(x(3)) + params.ballFriction*deltaT)/std::abs(x(3)); if( xVelFactor < 0.001f) x(2) = .0001f; if( yVelFactor < 0.001f) x(3) = .0001f; // Determine if ball is still moving float velMagnitude = getSpeed(); if(velMagnitude > 2.f) // basically still moving { // vel = vel * (absVel + decel)/absVel x(2) *= xVelFactor; x(3) *= yVelFactor; } } // Calculate the expected state ufmatrix4 A = prod(rotation,trajectory); ufmatrix4 ATranspose = trans(A); ufvector4 p = prod(A,x); x = p + translation; // Calculate the covariance Cov = A*Cov*ATranspose ufmatrix4 covTimesATranspose = prod(cov, ATranspose); cov = prod(A, covTimesATranspose); // Add noise: Process noise, rotation dev, translation dev ufvector4 noise; noise = boost::numeric::ublas::zero_vector<float>(4); // Add process noise noise(0) += params.processDeviationPosX; noise(1) += params.processDeviationPosY; if(!stationary){ noise(2) += params.processDeviationVelX; noise(3) += params.processDeviationVelY; } // Add translation deviation noise noise += translationDeviation; // Add rotation deviation noise // We've already made the matrix using the deviation matrix // so lets pump it through // #CRUDE_APPROXIMATION @b_human ufvector4 noiseFromRot = prod(rotationDeviationRotation, x) - x; for (int i=0; i<4; i++) noise(i) += std::abs(noiseFromRot(i)); // Add all this noise to the covariance for(int i=0; i<4; i++){ cov(i,i) += noise(i); } // Housekeep filteredDist = std::sqrt(x(0)*x(0) + x(1)*x(1)); filteredBear = NBMath::safe_atan2(x(1),x(0)); }