Exemple #1
0
/**
 * get the measurement covariance at state
 */
void MapMeasurementFunction::getMeasurementCov(const RBIS & state, Eigen::Matrix3d & R_effective)
{
  double xy[2] = { state.position()(0), state.position()(1) };

  Eigen::Vector3d eulers = state.getEulerAngles();
  double phipsi[2] = { eulers(0), eulers(2) };

  R_effective = phi_psi_xy_cov_map->readValue(phipsi)->readValue(xy);

  double theta_state = eulers(1);
  double z_state = state.position()(2);

  if (fabs(z_state - z_height) > 2.0) {
    fprintf(stderr, "warning, querying measurement at different height than measurements were simulated, %f, %f\n",
        z_state, z_height);
  }

  if (fabs(theta_state - theta) > bot_to_radians(20)) {
    fprintf(stderr,
        "warning, querying measurement at different theta (pitch) than measurements were simulated, %f, %f\n",
        bot_to_degrees(theta_state), bot_to_degrees(theta));
  }
}