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;
}
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();
}