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