00001
00002 /***************************************************************************
00003  *  relvelo.cpp - Implementation of velocity model based on relative ball
00004  *                 positions and relative robot velocity
00005  *
00006  *  Generated: Tue Oct 04 15:54:27 2005
00007  *  Copyright  2005  Tim Niemueller [www.niemueller.de]
00008  *
00009  ****************************************************************************/
00010
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version. A runtime exception applies to
00015  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00016  *
00017  *  This program is distributed in the hope that it will be useful,
00018  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  *  GNU Library General Public License for more details.
00021  *
00022  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00023  */
00024
00025 #include <models/velocity/relvelo.h>
00026
00027 #include <utils/system/console_colors.h>
00028 #include <utils/time/time.h>
00029
00030 #include <cmath>
00031 #include <cstdlib>
00032
00033 using namespace std;
00034 
00035 /** @class VelocityFromRelative <models/velocity/relvelo.h>
00036  * Calculate velocity from relative positions.
00037  */
00038 
00039 /** Constructor.
00040  * @param model relative position model
00041  * @param max_history_length maximum history length
00042  * @param calc_interval calculation interval
00043  */
00044 VelocityFromRelative::VelocityFromRelative(RelativePositionModel* model,
00045                                            unsigned int max_history_length,
00046                                            unsigned int calc_interval)
00047 {
00048   this->relative_pos_model = model;
00049   this->max_history_length = max_history_length;
00050   this->calc_interval      = calc_interval;
00051
00052   //kalman_enabled = true;
00053
00054   robot_rel_vel_x = robot_rel_vel_y = 0.f;
00055
00056   velocity_x = velocity_y = 0.f;
00057
00058   /*
00059   // initial variance for ball pos
00060   var_proc_x = 300;
00061   var_proc_y =  50;
00062   var_meas_x = 300;
00063   var_meas_y =  50;
00064 
00065   // initial variance for ball pos
00066   kalman_filter = new kalmanFilter2Dim();
00067   CMatrix<float> initialStateVarianceBall(2,2);
00068   initialStateVarianceBall[0][0] = var_meas_x;
00069   initialStateVarianceBall[1][0] =    0.0;
00070   initialStateVarianceBall[0][1] =    0.0;
00071   initialStateVarianceBall[1][1] = var_meas_y;
00072   kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 
00073 
00074   // process noise for ball pos kf, initial estimates, refined in calc()
00075   kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
00076   kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
00077   */
00078
00079   avg_vx_sum = 0.f;
00080   avg_vx_num = 0;
00081
00082   avg_vy_sum = 0.f;
00083   avg_vy_num = 0;
00084
00085   ball_history.clear();
00086
00087 }
00088
00089 
00090 /** Destructor. */
00091 VelocityFromRelative::~VelocityFromRelative()
00092 {
00093 }
00094
00095
00096 void
00097 VelocityFromRelative::setPanTilt(float pan, float tilt)
00098 {
00099 }
00100
00101
00102 void
00103 VelocityFromRelative::setRobotPosition(float x, float y, float ori, timeval t)
00104 {
00105 }
00106
00107
00108 void
00109 VelocityFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t)
00110 {
00111   robot_rel_vel_x = rel_vel_x;
00112   robot_rel_vel_y = rel_vel_y;
00113   robot_rel_vel_t.tv_sec = t.tv_sec;
00114   robot_rel_vel_t.tv_usec = t.tv_usec;
00115 }
00116
00117 void
00118 VelocityFromRelative::setTime(timeval t)
00119 {
00120   now.tv_sec  = t.tv_sec;
00121   now.tv_usec = t.tv_usec;
00122 }
00123
00124
00125 void
00126 VelocityFromRelative::setTimeNow()
00127 {
00128   gettimeofday(&now, NULL);
00129 }
00130
00131
00132 void
00133 VelocityFromRelative::getTime(long int *sec, long int *usec)
00134 {
00135   *sec  = now.tv_sec;
00136   *usec = now.tv_usec;
00137 }
00138
00139
00140 void
00141 VelocityFromRelative::getVelocity(float *vel_x, float *vel_y)
00142 {
00143   if (vel_x != NULL) {
00144     *vel_x = velocity_x;
00145   }
00146   if (vel_y != NULL) {
00147     *vel_y = velocity_y;
00148   }
00149 }
00150
00151
00152 float
00153 VelocityFromRelative::getVelocityX()
00154 {
00155   return velocity_x;
00156 }
00157
00158
00159 float
00160 VelocityFromRelative::getVelocityY()
00161 {
00162   return velocity_y;
00163 }
00164
00165
00166 void
00167 VelocityFromRelative::calc()
00168 {
00169   /*
00170   char user_input = toupper( getkey() );
00171 
00172   if ( ! relative_pos_model->isPosValid() ) {
00173     return;
00174   }
00175 
00176   if (user_input == 'P') {
00177     cout << "Enter new kalman process variance values (X Y):" << flush;
00178     cin >> var_proc_x >> var_proc_y;
00179   } else if (user_input == 'M') {
00180     cout << "Enter new kalman measurement variance values (X Y):" << flush;
00181     cin >> var_meas_x >> var_meas_y;
00182   } else if (user_input == 'R') {
00183     cout << "Reset" << endl;
00184     reset();
00185   } else if (user_input == 'C') {
00186     cout << "Current kalman measurement variance (X Y) = ("
00187          << var_meas_x << " " << var_meas_y << ")" << endl
00188          << "Current kalman process variance (X Y)     = ("
00189          << var_proc_x << " " << var_proc_y << ")" << endl;
00190   } else if (user_input == 'K') {
00191     kalman_enabled = ! kalman_enabled;
00192     if ( kalman_enabled ) {
00193       cout << "Kalman filtering enabled" << endl;
00194       kalman_filter->reset();
00195     } else {
00196       cout << "Kalman filtering disabled" << endl;
00197     }
00198   }
00199   */
00200
00201   // Gather needed data
00202   cur_ball_x = relative_pos_model->get_x();
00203   cur_ball_y = relative_pos_model->get_y();
00204   cur_ball_dist = relative_pos_model->get_distance();
00205
00206   if ( isnan(cur_ball_x) || isinf(cur_ball_x) ||
00207        isnan(cur_ball_y) || isinf(cur_ball_y) ||
00208        isnan(cur_ball_dist) || isinf(cur_ball_dist) ) {
00209     // cout << cred << "relative position model returned nan/inf value(s)!" << cnormal << endl;
00210     return;
00211   }
00212
00213   // if we project the last ball position by the velocity we calculated
00214   // at that time we can compare this to the current position and estimate
00215   // an error from this information
00216   if (last_available) {
00217     proj_time_diff_sec = fawkes::time_diff_sec(now, last_time);
00218     proj_x = last_x + velocity_x * proj_time_diff_sec;
00219     proj_y = last_y + velocity_y * proj_time_diff_sec;
00220     last_proj_error_x = cur_ball_x - proj_x;
00221     last_proj_error_y = cur_ball_y - proj_y;
00222     last_available = false;
00223   } else {
00224     last_proj_error_x = cur_ball_x;
00225     last_proj_error_y = cur_ball_x;
00226   }
00227
00228
00229   // newest entry first
00230   vel_postime_t *vpt = (vel_postime_t *)malloc(sizeof(vel_postime_t));;
00231   vpt->x             = cur_ball_x;
00232   vpt->y             = cur_ball_y;
00233   vpt->t.tv_sec      = now.tv_sec;
00234   vpt->t.tv_usec     = now.tv_usec;
00235   ball_history.push_front( vpt );
00236
00237
00238   if (ball_history.size() >= 2) {
00239
00240     // we need at least two entries
00241     // take the last robot velocity, then find the corresponding ball_pos entry
00242     // in the history and an entry about 100ms away to extrapolate the
00243     // ball velocity, then correct this by the robot's velocity we got
00244
00245     if ( fawkes::time_diff_sec(robot_rel_vel_t, vel_last_time) != 0 ) {
00246       // We have a new robot position data, calculate new velocity
00247
00248       vel_last_time.tv_sec  = robot_rel_vel_t.tv_sec;
00249       vel_last_time.tv_usec = robot_rel_vel_t.tv_usec;
00250
00251       f_diff_sec = HUGE;
00252       float time_diff;
00253       vel_postime_t *young = NULL;
00254       vel_postime_t *old   = NULL;
00255       unsigned int  step = 0;
00256       for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) {
00257         // Find the ball pos history entry closest in time (but still younger) to
00258         // the new position data
00259         time_diff = fawkes::time_diff_sec((*bh_it)->t, robot_rel_vel_t);
00260         if ( (time_diff > 0) && (time_diff < f_diff_sec) ) {
00261           f_diff_sec = time_diff;
00262           young = (*bh_it);
00263         } else {
00264           // Now find second time
00265           if (step != calc_interval) {
00266             ++step;
00267           } else {
00268             // Found a second time
00269             old = *bh_it;
00270             ++bh_it;
00271             break;
00272           }
00273         }
00274       }
00275
00276       if ((young != NULL) && (old != NULL)) {
00277         // we found two valid times
00278
00279         diff_x = young->x - old->x;
00280         diff_y = young->y - old->y;
00281
00282         f_diff_sec = fawkes::time_diff_sec(young->t, old->t);
00283
00284         velocity_x = diff_x / f_diff_sec;
00285         velocity_y = diff_y / f_diff_sec;
00286
00287         //cout << "f_diff_sec=" << f_diff_sec << "  vx=" << velocity_x << "  vy=" << velocity_y << endl;
00288
00289         velocity_x += robot_rel_vel_x;
00290         velocity_y += robot_rel_vel_y;
00291
00292         velocity_x -= last_proj_error_x * proj_time_diff_sec;
00293         velocity_y -= last_proj_error_y * proj_time_diff_sec;
00294
00295         //cout << "vx+rx=" << velocity_x << "  vy+ry=" << velocity_y << endl;
00296
00297         /*
00298         cout << endl
00299              << "VELOCITY CALCULATION" << endl
00300              << "  History size  : " << ball_history.size() << endl
00301              << "  Ball position" << endl
00302              << "    young       : (" << young->x << ", " << young->y << ")" << endl
00303              << "    old         : (" << old->x << ", " << old->y << ")" << endl
00304              << "    difference  : " << diff_x << ", " << diff_y << ")" << endl
00305              << "  Time" << endl
00306              << "    current     :  " << young->t.tv_sec << " sec, " << young->t.tv_usec << " usec" << endl
00307              << "    old         :  " << old->t.tv_sec << " sec, " << old->t.tv_usec << " usec" << endl
00308              << "    difference  :  " << f_diff_sec << " sec" << endl
00309              << "  Projection" << endl
00310              << "  proj error    : (" << last_proj_error_x << "," << last_proj_error_y << ")" << endl
00311              << "  Velocity" << endl
00312              << "    robot       : (" << robot_rel_vel_x << ", " << robot_rel_vel_y << ")" << endl
00313              << "    Ball" << endl
00314              << "      raw       : (" << velocity_x - robot_rel_vel_x << ", " << velocity_y - robot_rel_vel_y << ")" << endl
00315              << "      corrected : (" << velocity_x << ", " << velocity_y << ")" << endl;
00316         */
00317
00318         /*
00319         if ( kalman_enabled ) {
00320           applyKalmanFilter();
00321         }
00322         */
00323
00324         last_x = cur_ball_x;
00325         last_y = cur_ball_y;
00326         last_time.tv_sec = now.tv_sec;
00327         last_time.tv_usec = now.tv_usec;
00328         last_available = true;
00329
00330         /*
00331         cout << "      filtered  : (" << clightpurple << velocity_x << cnormal
00332              << ", " << clightpurple << velocity_y << cnormal << ")" << endl
00333              << endl;
00334         */
00335
00336         // erase old history entries
00337         if (bh_it != ball_history.end()) {
00338           ball_history.erase(bh_it, ball_history.end());
00339         }
00340       } else {
00341         // cout << "did not find matching young and old record" << endl;
00342         velocity_x = 0.f;
00343         velocity_y = 0.f;
00344       }
00345     } else {
00346       // we did not get a new robot position, keep old velocities for 2 seconds
00347       if (fawkes::time_diff_sec(now, vel_last_time) > 2) {
00348         // cout << "did not get new robot position for more than 2 sec, resetting" << endl;
00349         velocity_x = 0.f;
00350         velocity_y = 0.f;
00351       }
00352     }
00353   } else {
00354     // cout << "history too short" << endl;
00355     velocity_x = 0.f;
00356     velocity_y = 0.f;
00357   }
00358
00359   if (ball_history.size() > max_history_length) {
00360     bh_it = ball_history.begin();
00361     for (unsigned int i = 0; i < max_history_length; ++i) {
00362       ++bh_it;
00363     }
00364     ball_history.erase(bh_it, ball_history.end());
00365   }
00366
00367 }
00368
00369
00370 void
00371 VelocityFromRelative::reset()
00372 {
00373   /*
00374   if (kalman_enabled) {
00375     kalman_filter->reset();
00376   }
00377   */
00378   avg_vx_sum = 0.f;
00379   avg_vx_num = 0;
00380   avg_vy_sum = 0.f;
00381   avg_vy_num = 0;
00382   velocity_x = 0.f;
00383   velocity_y = 0.f;
00384   ball_history.clear();
00385 }
00386
00387
00388 const char *
00389 VelocityFromRelative::getName() const
00390 {
00391   return "VelocityModel::VelocityFromRelative";
00392 }
00393
00394
00395 coordsys_type_t
00396 VelocityFromRelative::getCoordinateSystem()
00397 {
00398   return COORDSYS_ROBOT_CART;
00399 }
00400
00401 /*
00402 void
00403 VelocityFromRelative::applyKalmanFilter()
00404 {
00405   /
00406   avg_vx_sum += velocity_x;
00407   avg_vy_sum += velocity_y;
00408 
00409   ++avg_vx_num;
00410   ++avg_vy_num;
00411 
00412   avg_vx = avg_vx_sum / avg_vx_num;
00413   avg_vy = avg_vy_sum / avg_vy_num;
00414 
00415   age_factor = (fawkes::time_diff_sec(now, robot_rel_vel_t) + f_diff_sec);
00416 
00417   rx = (velocity_x - avg_vx) * age_factor;
00418   ry = (velocity_y - avg_vy) * age_factor;
00419 
00420   kalman_filter->setProcessCovariance( rx * rx, ry * ry );
00421 
00422   rx = (velocity_x - avg_vx) * cur_ball_dist;
00423   ry = (velocity_y - avg_vy) * cur_ball_dist;
00424 
00425   kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
00426   /
00427 
00428   kalman_filter->setProcessCovariance( var_proc_x * cur_ball_dist,
00429                                        var_proc_y * cur_ball_dist);
00430   kalman_filter->setMeasurementCovariance( var_meas_x * cur_ball_dist,
00431                                            var_meas_y * cur_ball_dist );
00432 
00433   kalman_filter->setMeasurementX( velocity_x );
00434   kalman_filter->setMeasurementY( velocity_y );
00435   kalman_filter->doCalculation();
00436 
00437   velocity_x = kalman_filter->getStateX();
00438   velocity_y = kalman_filter->getStateY();
00439 
00440   velocity_x = round( velocity_x * 10 ) / 10;
00441   velocity_y = round( velocity_y * 10 ) / 10;
00442 
00443   if (isnan(velocity_x) || isinf(velocity_x) ||
00444       isnan(velocity_y) || isinf(velocity_y) ) {
00445     reset();
00446   }
00447 
00448 }
00449 */