C++ (Cpp) R_to_earth_zeroyaw Examples

Example #1

File: ekf_helper.cpp Project: Aerobota/ecl

// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
	// If we don't a tilt estimate then we cannot initialise the yaw
	if (!_control_status.flags.tilt_align) {
		return false;
	}

	// get the roll, pitch, yaw estimates and set the yaw to zero
	matrix::Quaternion<float> q(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
				    _state.quat_nominal(3));
	matrix::Euler<float> euler_init(q);
	euler_init(2) = 0.0f;

	// rotate the magnetometer measurements into earth axes
	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
	euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));

	// calculate initial quaternion states for the ekf
	// we don't change the output attitude to avoid jumps
	_state.quat_nominal = Quaternion(euler_init);

	// reset the angle error variances because the yaw angle could have changed by a significant amount
	// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
	// will grow them again.
	zeroRows(P, 0, 2);
	zeroCols(P, 0, 2);

	// calculate initial earth magnetic field states
	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init;

	// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
	zeroRows(P, 16, 21);
	zeroCols(P, 16, 21);

	for (uint8_t index = 16; index <= 21; index ++) {
		P[index][index] = sq(_params.mag_noise);
	}

	return true;
}

Example #2

File: ekf.cpp Project: priseborough/ecl

bool Ekf::initialiseFilter(void)
{
	_state.ang_error.setZero();
	_state.vel.setZero();
	_state.pos.setZero();
	_state.gyro_bias.setZero();
	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
	_state.accel_z_bias = 0.0f;
	_state.mag_I.setZero();
	_state.mag_B.setZero();
	_state.wind_vel.setZero();

	// get initial roll and pitch estimate from accel vector, assuming vehicle is static
	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;

	float pitch = 0.0f;
	float roll = 0.0f;

	if (accel_init.norm() > 0.001f) {
		accel_init.normalize();

		pitch = asinf(accel_init(0));
		roll = -asinf(accel_init(1) / cosf(pitch));
	}

	matrix::Euler<float> euler_init(roll, pitch, 0.0f);

	// Get the latest magnetic field measurement.
	// If we don't have a measurement then we cannot initialise the filter
	magSample mag_init = _mag_buffer.get_newest();

	if (mag_init.time_us == 0) {
		return false;
	}

	// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
	// TODO use declination if available
	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
	float declination = 0.0f;
	euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));

	// calculate initial quaternion states
	_state.quat_nominal = Quaternion(euler_init);
	_output_new.quat_nominal = _state.quat_nominal;

	// calculate initial earth magnetic field states
	matrix::Dcm<float> R_to_earth(euler_init);
	_state.mag_I = R_to_earth * mag_init.mag;

	resetVelocity();
	resetPosition();

	// initialize vertical position with newest baro measurement
	baroSample baro_init = _baro_buffer.get_newest();

	if (baro_init.time_us == 0) {
		return false;
	}

	_state.pos(2) = -baro_init.hgt;
	_output_new.pos(2) = -baro_init.hgt;

	initialiseCovariance();

	return true;
}