bool learn_covariance(Eigen::MatrixXd& covar, const Eigen::VectorXd& q) {
if (adaptation_window()) _estimator.add_sample(q);
if (end_adaptation_window()) {
compute_next_window();
_estimator.sample_covariance(covar);
double n = static_cast<double>(_estimator.num_samples());
covar = (n / (n + 5.0)) * covar
+ 1e-3 * (5.0 / (n + 5.0)) * Eigen::MatrixXd::Identity(covar.rows(), covar.cols());
_estimator.restart();
++_adapt_window_counter;
return true;
}
++_adapt_window_counter;
return false;
}
bool learn_variance(Eigen::VectorXd& var, const Eigen::VectorXd& q) {
if (adaptation_window())
estimator_.add_sample(q);
if (end_adaptation_window()) {
compute_next_window();
estimator_.sample_variance(var);
double n = static_cast<double>(estimator_.num_samples());
var = (n / (n + 5.0)) * var
+ 1e-3 * (5.0 / (n + 5.0)) * Eigen::VectorXd::Ones(var.size());
estimator_.restart();
++adapt_window_counter_;
return true;
}
++adapt_window_counter_;
return false;
}