예제 #1
0
파일: control.cpp 프로젝트: vvranjek/ecl
void Ekf::controlExternalVisionFusion()
{
	// Check for new exernal vision data
	if (_ev_data_ready) {

		// external vision position aiding selection logic
		if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
			// check for a exernal vision measurement that has fallen behind the fusion time horizon
			if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
				// turn on use of external vision measurements for position and height
				_control_status.flags.ev_pos = true;
				ECL_INFO("EKF switching to external vision position fusion");
				// turn off other forms of height aiding
				_control_status.flags.baro_hgt = false;
				_control_status.flags.gps_hgt = false;
				_control_status.flags.rng_hgt = false;
				// reset the position, height and velocity
				resetPosition();
				resetVelocity();
				resetHeight();
			}
		}

		// external vision yaw aiding selection logic
		if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
			// check for a exernal vision measurement that has fallen behind the fusion time horizon
			if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
				// reset the yaw angle to the value from the observaton quaternion
				// get the roll, pitch, yaw estimates from the quaternion states
				matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
							    _state.quat_nominal(3));
				matrix::Euler<float> euler_init(q_init);

				// get initial yaw from the observation quaternion
				extVisionSample ev_newest = _ext_vision_buffer.get_newest();
				matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
				matrix::Euler<float> euler_obs(q_obs);
				euler_init(2) = euler_obs(2);

				// save a copy of the quaternion state for later use in calculating the amount of reset change
				Quaternion quat_before_reset = _state.quat_nominal;

				// calculate initial quaternion states for the ekf
				_state.quat_nominal = Quaternion(euler_init);

				// calculate the amount that the quaternion has changed by
				_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();

				// add the reset amount to the output observer buffered data
				outputSample output_states;
				unsigned output_length = _output_buffer.get_length();
				for (unsigned i=0; i < output_length; i++) {
					output_states = _output_buffer.get_from_index(i);
					output_states.quat_nominal *= _state_reset_status.quat_change;
					_output_buffer.push_to_index(i,output_states);
				}

				// capture the reset event
				_state_reset_status.quat_counter++;

				// flag the yaw as aligned
				_control_status.flags.yaw_align = true;

				// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
				_control_status.flags.ev_yaw = true;
				_control_status.flags.mag_hdg = false;
				_control_status.flags.mag_3D = false;
				_control_status.flags.mag_dec = false;

				ECL_INFO("EKF switching to external vision yaw fusion");
			}
		}

		// determine if we should use the height observation
		if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
			_control_status.flags.baro_hgt = false;
			_control_status.flags.gps_hgt = false;
			_control_status.flags.rng_hgt = false;
			_control_status.flags.ev_hgt = true;
			_fuse_height = true;

		}

		// determine if we should use the horizontal position observations
		if (_control_status.flags.ev_pos) {
			_fuse_pos = true;

			// correct position and height for offset relative to IMU
			Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
			_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
			_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
			_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
		}

		// determine if we should use the yaw observation
		if (_control_status.flags.ev_yaw) {
			fuseHeading();
		}
	}
}
예제 #2
0
파일: control.cpp 프로젝트: tigerwood/ecl
void Ekf::controlExternalVisionAiding()
{
	// external vision position aiding selection logic
	if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
		// check for a exernal vision measurement that has fallen behind the fusion time horizon
		if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
			// turn on use of external vision measurements for position and height
			_control_status.flags.ev_pos = true;
			printf("EKF switching to external vision position fusion\n");
			// turn off other forms of height aiding
			_control_status.flags.baro_hgt = false;
			_control_status.flags.gps_hgt = false;
			_control_status.flags.rng_hgt = false;
			// reset the position, height and velocity
			resetPosition();
			resetVelocity();
			resetHeight();
		}
	}

	// external vision yaw aiding selection logic
	if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
		// check for a exernal vision measurement that has fallen behind the fusion time horizon
		if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
			// reset the yaw angle to the value from the observaton quaternion
			// get the roll, pitch, yaw estimates from the quaternion states
			matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
						    _state.quat_nominal(3));
			matrix::Euler<float> euler_init(q_init);

			// get initial yaw from the observation quaternion
			extVisionSample ev_newest = _ext_vision_buffer.get_newest();
			matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
			matrix::Euler<float> euler_obs(q_obs);
			euler_init(2) = euler_obs(2);

			// save a copy of the quaternion state for later use in calculating the amount of reset change
			Quaternion quat_before_reset = _state.quat_nominal;

			// calculate initial quaternion states for the ekf
			_state.quat_nominal = Quaternion(euler_init);

			// calculate the amount that the quaternion has changed by
			_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();

			// add the reset amount to the output observer buffered data
			outputSample output_states;
			unsigned output_length = _output_buffer.get_length();
			for (unsigned i=0; i < output_length; i++) {
				output_states = _output_buffer.get_from_index(i);
				output_states.quat_nominal *= _state_reset_status.quat_change;
				_output_buffer.push_to_index(i,output_states);
			}

			// capture the reset event
			_state_reset_status.quat_counter++;

			// flag the yaw as aligned
			_control_status.flags.yaw_align = true;

			// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
			_control_status.flags.ev_yaw = true;
			_control_status.flags.mag_hdg = false;
			_control_status.flags.mag_3D = false;
			_control_status.flags.mag_dec = false;

			printf("EKF switching to external vision yaw fusion\n");
		}
	}

}