void
carmen_fused_odometry_initialize(carmen_fused_odometry_parameters *fused_odometry_parameters)
{	
	if (xsens_xyz_h == NULL)
		xsens_xyz_h = create_xsens_xyz_handler(argc_g, argv_g, fused_odometry_parameters);
	else
		reset_xsens_xyz_handler(xsens_xyz_h);

	if (car_odom_h == NULL)
		car_odom_h = create_car_odometry_handler(xsens_xyz_h, fused_odometry_parameters);
	else
		reset_car_odometry_handler(car_odom_h);

	gps_initial_pos.x = 0.0;
	gps_initial_pos.y = 0.0;
	gps_initial_pos.z = 0.0;

}
void
carmen_fused_odometry_initialize(carmen_fused_odometry_parameters *fused_odometry_parameters)
{	
	if (xsens_xyz_h == NULL)
		xsens_xyz_h = create_xsens_xyz_handler(argc_g, argv_g, fused_odometry_parameters);
	else
		reset_xsens_xyz_handler(xsens_xyz_h);

	if (use_viso_odometry)
	{
		if (viso_h == NULL)
			viso_h = create_visual_odometry_handler(argc_g, argv_g);
		else
			reset_visual_odometry_handler(viso_h);
	}

	if (use_car_odometry)
	{
		if (car_odom_h == NULL)
			car_odom_h = create_car_odometry_handler(xsens_xyz_h, fused_odometry_parameters);
		else
			reset_car_odometry_handler(car_odom_h);
	}

	gps_initial_pos.x = 0.0;
	gps_initial_pos.y = 0.0;
	gps_initial_pos.z = 0.0;

	ut.v = 0.0;
	ut.phi = 0.0;
	ut.z = 0.0;
	ut.v_z = 0.0;
	ut.v_pitch = 0.0;
	ut.pitch = 0.0;
	ut.roll = 0.0;

	// last_prediction_time = carmen_get_time();
}