예제 #1
0
        /**
         * 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";
        }
예제 #2
0
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;
}
예제 #3
0
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;
}
예제 #4
0
/**
 * 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);
    }
}
예제 #5
0
/**
 * 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);
    }
}
예제 #6
0
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;
}
예제 #7
0
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());
}
예제 #8
0
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));
}