static carmen_fused_odometry_state_vector
randomize_state_vector(carmen_fused_odometry_state_vector initial_state, carmen_fused_odometry_parameters *fused_odometry_parameters)
{
carmen_fused_odometry_state_vector new_sv;
carmen_point_t std = get_std_error(xsens_xyz_h, fused_odometry_parameters);
new_sv.pose.position.x = initial_state.pose.position.x + carmen_gaussian_random(0.0, std.x);
new_sv.pose.position.y = initial_state.pose.position.y + carmen_gaussian_random(0.0, std.y);
new_sv.pose.position.z = 0.0; // initial_state.pose.position.z + carmen_gaussian_random(0.0, fused_odometry_parameters->xsens_gps_z_std_error);
new_sv.pose.orientation.roll = 0.0; // initial_state.pose.orientation.roll + carmen_gaussian_random(0.0, fused_odometry_parameters->xsens_roll_std_error);
new_sv.pose.orientation.pitch = 0.0; // initial_state.pose.orientation.pitch + carmen_gaussian_random(0.0, fused_odometry_parameters->xsens_pitch_std_error);
new_sv.pose.orientation.yaw = initial_state.pose.orientation.yaw + carmen_gaussian_random(0.0, std.theta);
new_sv.xsens_yaw_bias = 0.0; // initial_state.xsens_yaw_bias + carmen_gaussian_random(0.0, fused_odometry_parameters->xsens_yaw_bias_noise);
new_sv.velocity.x = 0.0; // initial_state.velocity.x + carmen_gaussian_random(0.0, 1.0);
new_sv.velocity.y = 0.0; // initial_state.velocity.y + carmen_gaussian_random(0.0, 1.0);
new_sv.velocity.z = 0.0; // initial_state.velocity.z + carmen_gaussian_random(0.0, 1.0);
new_sv.ang_velocity.roll = 0.0; // initial_state.ang_velocity.roll + carmen_gaussian_random(0.0, 0.01);
new_sv.ang_velocity.pitch = 0.0; // initial_state.ang_velocity.pitch + carmen_gaussian_random(0.0, 0.3);
new_sv.ang_velocity.yaw = 0.0; // initial_state.ang_velocity.yaw + carmen_gaussian_random(0.0, 0.3);
new_sv.phi = initial_state.phi; // + carmen_gaussian_random(0.0, fused_odometry_parameters->phi_noise_phi);
new_sv.timestamp = initial_state.timestamp;
return new_sv;
}
void
carmen_simulator_create_object(double x, double y, double theta,
carmen_simulator_object_t type,
double speed)
{
check_list_capacity();
if (speed == -1)
speed = fabs(carmen_gaussian_random(0.0, person_speed/2.0) +
person_speed/2.0);
object_list[num_objects].type = type;
object_list[num_objects].is_robot = 0;
object_list[num_objects].x1 = x+cos(theta+M_PI/2)*MAX_STEP/4;
object_list[num_objects].y1 = y+sin(theta+M_PI/2)*MAX_STEP/4;
object_list[num_objects].x2 = x-cos(theta+M_PI/2)*MAX_STEP/4;
object_list[num_objects].y2 = y-sin(theta+M_PI/2)*MAX_STEP/4;
object_list[num_objects].theta = theta;
object_list[num_objects].width = leg_width;
object_list[num_objects].tv = speed;
object_list[num_objects].rv = 0;
traj_object_list[num_objects].t_vel = speed;
traj_object_list[num_objects].r_vel = speed;
traj_object_list[num_objects].x = x;
traj_object_list[num_objects].y = y;
traj_object_list[num_objects].theta = theta;
num_objects++;
}
carmen_fused_odometry_particle
sample_motion_model_simple(carmen_fused_odometry_particle x_t_1, carmen_fused_odometry_control ut, double dt, carmen_fused_odometry_parameters *fused_odometry_parameters)
{
double L = fused_odometry_parameters->axis_distance;
carmen_fused_odometry_particle x_t = x_t_1;
x_t.state.velocity.x = ut.v +
carmen_gaussian_random(0.0,
fused_odometry_parameters->velocity_noise_velocity * ut.v * ut.v +
fused_odometry_parameters->velocity_noise_phi * ut.phi * ut.phi);
x_t.state.velocity.y = 0.0;
x_t.state.velocity.z = 0.0;
x_t.state.phi = ut.phi + carmen_gaussian_random(0.0,
fused_odometry_parameters->phi_noise_phi * ut.phi * ut.phi +
fused_odometry_parameters->phi_noise_velocity * ut.v * ut.v);
x_t.state.ang_velocity.roll = 0.0;
x_t.state.ang_velocity.yaw = 0.0;
// This will limit the wheel position to the maximum angle the car can turn
if (x_t.state.phi > fused_odometry_parameters->maximum_phi)
{
x_t.state.phi = fused_odometry_parameters->maximum_phi;
}
else if (x_t.state.phi < -fused_odometry_parameters->maximum_phi)
{
x_t.state.phi = -fused_odometry_parameters->maximum_phi;
}
x_t.state.pose.orientation.roll = ut.roll;
x_t.state.pose.orientation.pitch = ut.pitch;
x_t.state.xsens_yaw_bias = 0.0;
x_t.state.pose.position.x += dt * ut.v * cos(x_t_1.state.pose.orientation.yaw);
x_t.state.pose.position.y += dt * ut.v * sin(x_t_1.state.pose.orientation.yaw);
x_t.state.pose.position.z = 0.0;
x_t.state.pose.orientation.yaw = x_t_1.state.pose.orientation.yaw + (x_t.state.velocity.x * tan(x_t.state.phi) / L) * dt;
x_t.state.pose.orientation.yaw = carmen_normalize_theta(x_t.state.pose.orientation.yaw);
x_t.weight = x_t_1.weight;
return (x_t);
}
/* initialize particles from a gaussian distribution */
void
carmen_localize_ackerman_initialize_particles_gaussians(carmen_localize_ackerman_particle_filter_p filter,
int num_modes,
carmen_point_t *mean,
carmen_point_t *std)
{
int i, j, each, start, end;
double x, y, theta;
each = (int) floor(filter->param->num_particles / (double) num_modes);
for(i = 0; i < num_modes; i++)
{
start = i * each;
if(i == num_modes - 1)
end = filter->param->num_particles;
else
end = (i + 1) * each;
for(j = start; j < end; j++)
{
x = carmen_gaussian_random(mean[i].x, std[i].x);
y = carmen_gaussian_random(mean[i].y, std[i].y);
theta = carmen_normalize_theta(carmen_gaussian_random(mean[i].theta, std[i].theta));
filter->particles[j].x = x;
filter->particles[j].y = y;
filter->particles[j].theta = theta;
filter->particles[j].weight = 0.0;
}
}
filter->initialized = 1;
filter->first_odometry = 1;
// if (num_modes < 2)
if (0) // @@@ Alberto: mudancca de significado de global_mode para que indique apenas que nao convergiu
filter->global_mode = 0;
else
filter->global_mode = 1;
filter->distance_travelled = 0;
}
/* adds error to a laser scan */
static void
add_laser_error(carmen_laser_laser_message * laser,
carmen_simulator_ackerman_laser_config_t *laser_config)
{
int i;
for (i = 0; i < laser_config->num_lasers; i ++)
{
if (laser->range[i] > laser_config->max_range)
laser->range[i] = laser_config->max_range;
else if (carmen_uniform_random(0, 1.0) < laser_config->prob_of_random_max)
laser->range[i] = laser_config->max_range;
else if (carmen_uniform_random(0, 1.0) < laser_config->prob_of_random_reading)
laser->range[i] = carmen_uniform_random(0, laser_config->num_lasers);
else
laser->range[i] += carmen_gaussian_random(0.0, laser_config->variance);
}
}
double carmen_localize_ackerman_sample_noisy_turn(double delta_t, double delta_theta,
carmen_localize_ackerman_motion_model_t *model)
{
double turn_mean, turn_std_dev;
double sample;
turn_mean = delta_t * model->mean_t_d + delta_theta * model->mean_t_t;
turn_std_dev = fabs(delta_t) * model->std_dev_t_d + fabs(delta_theta) * model->std_dev_t_t;
if (turn_std_dev < 1e-6)
return turn_mean;
do
{
sample = carmen_gaussian_random(turn_mean, turn_std_dev);
} while (fabs(sample - turn_mean) > 2*turn_std_dev);
return sample;
}
double
compute_new_velocity_with_ann(carmen_simulator_ackerman_config_t *simulator_config)
{
static double throttle_command = 0.0;
static double brakes_command = 100.0 / 100.0;
static int gear_command = 0;
static fann_type velocity_ann_input[NUM_VELOCITY_ANN_INPUTS];
fann_type *velocity_ann_output;
static struct fann *velocity_ann = NULL;
if (velocity_ann == NULL)
{
velocity_ann = fann_create_from_file("velocity_ann.net");
if (velocity_ann == NULL)
{
printf("Error: Could not create velocity_ann\n");
exit(1);
}
init_velocity_ann_input(velocity_ann_input);
}
carmen_ford_escape_hybrid_velocity_PID_controler(&throttle_command, &brakes_command, &gear_command,
simulator_config->target_v, simulator_config->v, simulator_config->delta_t);
if (gear_command == 129) // marcha reh
{
build_velocity_ann_input(velocity_ann_input, throttle_command, brakes_command, -simulator_config->v);
velocity_ann_output = fann_run(velocity_ann, velocity_ann_input);
simulator_config->v = velocity_ann_output[0];
}
else
{
build_velocity_ann_input(velocity_ann_input, throttle_command, brakes_command, simulator_config->v);
velocity_ann_output = fann_run(velocity_ann, velocity_ann_input);
simulator_config->v = velocity_ann_output[0];
}
simulator_config->v = simulator_config->v + simulator_config->v * carmen_gaussian_random(0.0, 0.007); // add some noise
return (simulator_config->v);
}
double carmen_localize_ackerman_sample_noisy_crossrange(double delta_t,
double delta_theta,
carmen_localize_ackerman_motion_model_t
*model)
{
double crossrange_mean, crossrange_std_dev;
double sample;
crossrange_mean = delta_t * model->mean_c_d + delta_theta * model->mean_c_t;
crossrange_std_dev = fabs(delta_t) * model->std_dev_c_d + fabs(delta_theta) * model->std_dev_c_t;
if (crossrange_std_dev < 1e-6)
return crossrange_mean;
do
{
sample = carmen_gaussian_random(crossrange_mean, crossrange_std_dev);
} while (fabs(sample - crossrange_mean) > 2 * crossrange_std_dev);
return sample;
}
static carmen_fused_odometry_particle
sample_motion_model(carmen_fused_odometry_particle x_t_1, carmen_fused_odometry_control ut, double dt, carmen_fused_odometry_parameters *fused_odometry_parameters)
{
double L = fused_odometry_parameters->axis_distance;
carmen_fused_odometry_particle x_t = x_t_1;
x_t.state.velocity.x = ut.v +
carmen_gaussian_random(0.0,
fused_odometry_parameters->velocity_noise_velocity * ut.v * ut.v +
fused_odometry_parameters->velocity_noise_phi * ut.phi * ut.phi);
x_t.state.velocity.y = 0.0;
x_t.state.velocity.z = 0.0;
//x_t.state.velocity.z = ut.v_z + carmen_gaussian_random(0.0, alpha5 * ut.v_z * ut.v_z);// + alpha6 * ut.v_pitch * ut.v_pitch);
x_t.state.phi = ut.phi + carmen_gaussian_random(0.0,
fused_odometry_parameters->phi_noise_phi * ut.phi * ut.phi +
fused_odometry_parameters->phi_noise_velocity * ut.v * ut.v);
x_t.state.ang_velocity.roll = 0.0;
// x_t.state.ang_velocity.pitch = 0.0;
// x_t.state.ang_velocity.pitch = ut.v_pitch + carmen_gaussian_random(0.0,
// fused_odometry_parameters->pitch_v_noise_pitch_v * ut.v_pitch * ut.v_pitch +
// fused_odometry_parameters->pitch_v_noise_velocity * ut.v * ut.v);
x_t.state.ang_velocity.yaw = 0.0;
// This will limit the wheel position to the maximum angle the car can turn
if (x_t.state.phi > fused_odometry_parameters->maximum_phi)
{
x_t.state.phi = fused_odometry_parameters->maximum_phi;
}
else if (x_t.state.phi < -fused_odometry_parameters->maximum_phi)
{
x_t.state.phi = -fused_odometry_parameters->maximum_phi;
}
//sensor_vector_imu imu_zt = create_sensor_vector_imu(xsens_matrix_message);
//x_t.state.pose.orientation.roll = imu_zt.orientation.roll;
x_t.state.pose.orientation.roll = ut.roll;
x_t.state.pose.orientation.pitch = ut.pitch;
//x_t.state.pose.orientation.pitch = x_t_1.state.pose.orientation.pitch + x_t.state.ang_velocity.pitch * dt;
//x_t.state.pose.orientation.pitch = 0.0;
x_t.state.pose.orientation.yaw = x_t_1.state.pose.orientation.yaw + (x_t.state.velocity.x * tan(x_t.state.phi) / L) * dt;
x_t.state.pose.orientation.yaw = carmen_normalize_theta(x_t.state.pose.orientation.yaw);
if (0) // (fabs(ut.v) > fused_odometry_parameters->minimum_speed_for_correction) && ut.gps_available)
{
x_t.state.xsens_yaw_bias = x_t_1.state.xsens_yaw_bias * 0.9999 + carmen_gaussian_random(0.0, fused_odometry_parameters->xsens_yaw_bias_noise);
if (x_t.state.xsens_yaw_bias > fused_odometry_parameters->xsens_maximum_yaw_bias)
{
x_t.state.xsens_yaw_bias = fused_odometry_parameters->xsens_maximum_yaw_bias;
}
else if (x_t.state.xsens_yaw_bias < -fused_odometry_parameters->xsens_maximum_yaw_bias)
{
x_t.state.xsens_yaw_bias = -fused_odometry_parameters->xsens_maximum_yaw_bias;
}
}
else
x_t.state.xsens_yaw_bias = 0.0;
tf::Vector3 displacement_car_reference(x_t.state.velocity.x * dt, x_t.state.velocity.y * dt, x_t.state.velocity.z * dt);
tf::Transform car_to_global;
car_to_global.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
car_to_global.setRotation(tf::Quaternion(x_t.state.pose.orientation.yaw, x_t.state.pose.orientation.pitch, x_t.state.pose.orientation.roll));
tf::Vector3 displacement_global_reference = car_to_global * displacement_car_reference;
tf::Vector3 x_t_global_position(x_t_1.state.pose.position.x, x_t_1.state.pose.position.y, x_t_1.state.pose.position.z);
x_t_global_position = x_t_global_position + displacement_global_reference;
double xy_uncertainty_due_to_grid_resolution = (0.2 / 4.0) * (0.2 / 4.0);
double yaw_uncertainty_due_to_grid_resolution = asin((0.2 / 4.0)) * asin((0.2 / 4.0));
x_t.state.pose.position.x = x_t_global_position.getX() + carmen_gaussian_random(0.0, xy_uncertainty_due_to_grid_resolution);
x_t.state.pose.position.y = x_t_global_position.getY() + carmen_gaussian_random(0.0, xy_uncertainty_due_to_grid_resolution);
x_t.state.pose.position.z = 0.0; //x_t_global_position.getZ();
x_t.state.pose.orientation.yaw = carmen_normalize_theta(x_t.state.pose.orientation.yaw + carmen_gaussian_random(0.0, yaw_uncertainty_due_to_grid_resolution));
x_t.weight = x_t_1.weight;
return (x_t);
}
particle_datmo_t
sample_motion_model(double delta_time, particle_datmo_t particle_t_1, double mean, particle_datmo_t mean_particle)
{
particle_datmo_t particle_t;
carmen_point_t pose_t, pose_t_1;
double v;
pose_t_1.x = particle_t_1.pose.x;
pose_t_1.y = particle_t_1.pose.y;
pose_t_1.theta = particle_t_1.pose.theta;
v = particle_t_1.velocity;
v = v + carmen_gaussian_random(0.0, alpha_1);
// v = v + carmen_gaussian_random(0.0, mean*0.1);
if (v > v_max) v = v_max;
if (v < v_min) v = v_min;
pose_t_1.theta = pose_t_1.theta + carmen_gaussian_random(0.0, alpha_2);
/* Os resultados melhores estão com inicialização aleatória da orientação utilizando o mesmo desvio padrão fixo.
double c = 100.0;
if (mean > c) {
mean = c;
}
mean = mean * M_PI / c;
double theta_noise = carmen_gaussian_random(0.0, mean);
if(theta_noise > M_PI) {
theta_noise = M_PI;
} else if (theta_noise < -M_PI){
theta_noise = -M_PI;
}
pose_t_1.theta = pose_t_1.theta + theta_noise;
*/
pose_t_1.theta = carmen_normalize_theta(pose_t_1.theta);
pose_t.x = pose_t_1.x + delta_time*v*cos(pose_t_1.theta);
pose_t.y = pose_t_1.y + delta_time*v*sin(pose_t_1.theta);
particle_t.pose.theta = pose_t_1.theta;
particle_t.pose.x = pose_t.x;
particle_t.pose.y = pose_t.y;
particle_t.velocity = v;
particle_t.weight = particle_t_1.weight;
// Keep same class with 90% probability
#ifndef BASELINE
// fixme modificado para o baseline
if ((rand() % 100) <= 90)
//if (carmen_uniform_random(0.0, 100.0) <= 90.0)
particle_t.class_id = particle_t_1.class_id;
else
particle_t.class_id = get_random_model_id(num_of_models);
#else
particle_t.class_id = 0;
#endif
particle_t.model_features = get_obj_model_features(particle_t.class_id);
// particle_t.model_features_3d = get_obj_model_features_3d(particle_t.class_id, object_models_3d);
if(mean > 0 && mean_particle.dist > 0)
{
}
return particle_t;
}
