ia_codigo_media-particula

particle
compute_average_state(std::vector<particle> *particle_set_t)
{
	particle mean_particle;
	double total_weight = 0.0;
	double mean_x, mean_y, mean_theta_x, mean_theta_y, mean_velocity;

	/* compute mean particle pose */
	mean_x = 0.0;
	mean_y = 0.0;
	mean_theta_x = 0.0;
	mean_theta_y = 0.0;
	mean_velocity = 0.0;
	for(std::vector<particle>::iterator it = particle_set_t->begin();
				it != particle_set_t->end(); it++)
	{
		mean_x += it->pose.x * it->weight;
		mean_y += it->pose.y * it->weight;
		mean_theta_x += cos(it->pose.theta) * it->weight;
		mean_theta_y += sin(it->pose.theta) * it->weight;
		mean_velocity += it->velocity * it->weight;
		total_weight += it->weight;
	}

	mean_particle.pose.x = mean_x / total_weight;
	mean_particle.pose.y = mean_y / total_weight;
	mean_particle.pose.theta = carmen_normalize_theta(atan2(mean_theta_y, mean_theta_x));
	mean_particle.velocity = mean_velocity / total_weight;
//	mean_particle.velocity = mean_velocity;

	return(mean_particle);
}