Disable velocity estimator, fix observation weight calculation, add s…

…preading of particles around new observation when target is seen after previously there was no visibility

include/pfuclt_omni_dataset/pfuclt_particles.h

@@ -356,6 +356,17 @@ class ParticleFilter
     particles_[O_WEIGHT].assign(particles_[O_WEIGHT].size(), val);
   }
 
+  /**
+   * @brief spreadTargetParticlesSphere - spread a percentage of the target
+   * particle in a sphere around center
+   * @param particlesRatio - float between 0 and 1, corresponding to the
+   * percentage of particles that will be spread
+   * @param center - center of the sphere [x,y,z]
+   * @param radius - in meters
+   */
+  void spreadTargetParticlesSphere(float particlesRatio, pdata_t center[3],
+                                   float radius);
+
   /**
    * @brief predictTarget - predict target state step
    * @param robotNumber - the robot performing, for debugging purposes
@@ -587,6 +598,26 @@ class ParticleFilter
                                     ros::Time stamp)
   {
     bufTargetObservations_[robotNumber] = obs;
+
+    // If previously target not seen and now is found
+    if (obs.found && !state_.target.seen)
+    {
+      // Update to target seen
+      state_.target.seen = true;
+
+      // Observation to global frame
+      const ParticleFilter::State::RobotState& rs = state_.robots[robotNumber];
+      pdata_t ballGlobal[3];
+      ballGlobal[O_TX] = rs.pose[O_X] + obs.x * cos(rs.pose[O_THETA]) -
+                         obs.y * sin(rs.pose[O_THETA]);
+      ballGlobal[O_TY] = rs.pose[O_Y] + obs.x * sin(rs.pose[O_THETA]) +
+                         obs.y * cos(rs.pose[O_THETA]);
+      ballGlobal[O_TZ] = obs.z;
+
+      // Spread 50% of particles around ballGlobal in a sphere with 1.0 meter
+      // radius
+      spreadTargetParticlesSphere(0.5, ballGlobal, 1.0);
+    }
   }
 
   /**

src/pfuclt_particles.cpp

@@ -128,6 +128,21 @@ void ParticleFilter::dynamicReconfigureCallback(
 #endif
 }
 
+void ParticleFilter::spreadTargetParticlesSphere(float particlesRatio,
+                                                 pdata_t center[3],
+                                                 float radius)
+{
+  uint particlesToSpread = nParticles_ * particlesRatio;
+
+  boost::random::uniform_real_distribution<> dist(-radius, radius);
+
+  for (uint p = 0; p < particlesToSpread; ++p)
+  {
+    for (uint s = 0; s < STATES_PER_TARGET; ++s)
+      particles_[O_TARGET + s][p] = center[s] + dist(seed_);
+  }
+}
+
 void ParticleFilter::predictTarget()
 {
   *iteration_oss << "predictTarget() -> ";
@@ -148,8 +163,7 @@ void ParticleFilter::predictTarget()
     // Use X and Y velocity estimates
     for (uint s = 0; s < STATES_PER_TARGET - 1; ++s)
     {
-      pdata_t diff = state_.target.vel[s] * targetIterationTime_.diff +
-                     0.5 * accel[s] * pow(targetIterationTime_.diff, 2);
+      pdata_t diff = 0.5 * accel[s] * pow(targetIterationTime_.diff, 2);
 
       particles_[O_TARGET + s][p] += diff;
 
@@ -380,9 +394,9 @@ void ParticleFilter::fuseTarget()
         Z_Zcap[1] = Z[1] - Zcap[1];
         Z_Zcap[2] = Z[2] - Zcap[2];
 
-        expArg = -0.5 * (Z_Zcap[0] * Z_Zcap[0] / (.3 * obs->covXX) +
-                         Z_Zcap[1] * Z_Zcap[1] / (.3 * obs->covYY) +
-                         Z_Zcap[2] * Z_Zcap[2] * 10.0);
+        expArg = -0.5 * (Z_Zcap[0] * Z_Zcap[0] / obs->covXX +
+                         Z_Zcap[1] * Z_Zcap[1] / obs->covYY +
+                         Z_Zcap[2] * Z_Zcap[2] / .04);
         detValue =
             1.0; // pow((2 * M_PI * obs->covXX * obs->covYY * 10.0), -0.5);
 
@@ -561,9 +575,9 @@ void ParticleFilter::estimate()
     *iteration_oss << "DONE without estimating!";
 
     // Reset velocity estimator and target velocity
-    // state_.targetVelocityEstimator.reset();
-    // state_.target.vel[O_TX] = state_.target.vel[O_TY] =
-    //    state_.target.vel[O_TZ] = 0.0;
+    state_.targetVelocityEstimator.reset();
+    state_.target.vel[O_TX] = state_.target.vel[O_TY] =
+        state_.target.vel[O_TZ] = 0.0;
 
     // Increase standard deviation for target prediction
     if (dynamicVariables_.targetRandStddev != TARGET_RAND_STDDEV_LOST)
@@ -651,7 +665,7 @@ void ParticleFilter::estimate()
   // Ball velocity is estimated using linear regression
   if (state_.targetVelocityEstimator.isReadyToEstimate())
   {
-    state_.target.vel[O_TX] = state_.targetVelocityEstimator.estimate(O_X);
+    state_.target.vel[O_TX] = state_.targetVelocityEstimator.estimate(O_TX);
     state_.target.vel[O_TY] = state_.targetVelocityEstimator.estimate(O_TY);
     state_.target.vel[O_TZ] = state_.targetVelocityEstimator.estimate(O_TZ);