void UKF::PredictMeasurement(int n_z, const MatrixXd &Zsig, VectorXd &z_pred, MatrixXd &S, MatrixXd &R) {

  // predict measurement mean
  z_pred.fill(0.0);
  for (int i=0; i < 2 * n_aug_ + 1; i++) {
    z_pred += weights_(i) * Zsig.col(i);
  }

  // measurement covariance matrix S
  S.fill(0.0);
  for (int i = 0; i < 2 * n_aug_ + 1; i++) {
    VectorXd z_diff = Zsig.col(i) - z_pred;
    while (z_diff(1) >  M_PI) z_diff(1) -= 2. * M_PI;
    while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
    S += weights_(i) * z_diff * z_diff.transpose();
  }

  S += R;
}
Example #2
0
void  UpdaterMean::costsToWeights(const VectorXd& costs, string weighting_method, double eliteness, VectorXd& weights) const
{
  weights.resize(costs.size());
  if (weighting_method.compare("PI-BB")==0)
  {
    // PI^2 style weighting: continuous, cost exponention
    double h = eliteness; // In PI^2, eliteness parameter is known as "h"
    double range = costs.maxCoeff()-costs.minCoeff();
    if (range==0)
      weights.fill(1);
    else
      weights = (-h*(costs.array()-costs.minCoeff())/range).exp();
  } 
  else if (weighting_method.compare("CMA-ES")==0 || weighting_method.compare("CEM")==0 )
  {
    // CMA-ES and CEM are rank-based, so we must first sort the costs, and the assign a weight to 
    // each rank.
    VectorXd costs_sorted = costs; 
    std::sort(costs_sorted.data(), costs_sorted.data()+costs_sorted.size());
    // In Python this is more elegant because we have argsort.
    // indices = np.argsort(costs)
    // It is possible to do this with fancy lambda functions or std::pair in C++ too, but  I don't
    // mind writing two for loops instead ;-)
    
    weights.fill(0.0);
    int mu = eliteness; // In CMA-ES, eliteness parameter is known as "mu"
    assert(mu<costs.size());
    for (int ii=0; ii<mu; ii++)
    {
      double cur_cost = costs_sorted[ii];
      for (int jj=0; jj<costs.size(); jj++)
      {
        if (costs[jj] == cur_cost)
        {
          if (weighting_method.compare("CEM")==0)
            weights[jj] = 1.0/mu; // CEM
          else
            weights[jj] = log(mu+0.5) - log(ii+1); // CMA-ES
          break;
        }
      }
      
    }
    // For debugging
    //MatrixXd print_mat(3,costs.size());
    //print_mat.row(0) = costs_sorted;
    //print_mat.row(1) = costs;
    //print_mat.row(2) = weights;
    //cout << print_mat << endl;
  }
  else
  {
    cout << __FILE__ << ":" << __LINE__ << ":WARNING: Unknown weighting method '" << weighting_method << "'. Calling with PI-BB weighting." << endl; 
    costsToWeights(costs, "PI-BB", eliteness, weights);
    return;
  }
  
  // Relative standard deviation of total costs
  double mean = weights.mean();
  double std = sqrt((weights.array()-mean).pow(2).mean());
  double rel_std = std/mean;
  if (rel_std<1e-10)
  {
      // Special case: all costs are the same
      // Set same weights for all.
      weights.fill(1);
  }

  // Normalize weights
  weights = weights/weights.sum();

}