void Ekf::controlFusionModes()
{
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Get the magnetic declination
calcMagDeclination();
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
}
// control use of various external sources for position and velocity aiding
controlExternalVisionAiding();
controlOpticalFlowAiding();
controlGpsAiding();
controlHeightAiding();
controlMagAiding();
}
void Ekf::controlFusionModes()
{
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Get the magnetic declination
calcMagDeclination();
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
ECL_INFO("EKF alignment complete");
}
}
// check for arrival of new sensor data at the fusion time horizon
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// control use of observations for aiding
controlMagFusion();
controlExternalVisionFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlBaroFusion();
controlRangeFinderFusion();
controlAirDataFusion();
// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
controlVelPosFusion();
}
// Check the tilt and yaw alignmnent status
// Used during initial bootstrap alignment of the filter
void NavEKF3_core::checkAttitudeAlignmentStatus()
{
// Check for tilt convergence - used during initial alignment
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if (!tiltAlignComplete) {
Vector3f angleErrVarVec = calcRotVecVariances();
if ((angleErrVarVec.x + angleErrVarVec.y) < sq(0.05235f)) {
tiltAlignComplete = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index);
}
}
// submit yaw and magnetic field reset request
if (!yawAlignComplete && tiltAlignComplete && use_compass()) {
magYawResetRequest = true;
}
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// 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 the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances
Vector3f angle_err_var_vec = calcRotVecVariances();
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate the initial quaternion
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// use a 321 sequence
// rotate the magnetometer measurement into earth frame
matrix::Euler<float> euler321(_state.quat_nominal);
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321);
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler321);
} else {
// use a 312 sequence
// Calculate the 312 sequence euler angles that rotate from earth to body frame
// See http://www.atacolorado.com/eulersequences.doc
Vector3f euler312;
euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
euler312(0) = 0.0f;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
float c2 = cosf(euler312(2));
float s2 = sinf(euler312(2));
float s1 = sinf(euler312(1));
float c1 = cosf(euler312(1));
float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0));
matrix::Dcm<float> R_to_earth;
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// re-calculate the rotation matrix using the updated yaw angle
s0 = sinf(euler312(0));
c0 = cosf(euler312(0));
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(R_to_earth);
}
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// update the yaw angle variance using the variance of the measurement
if (_params.fusion_mode & MASK_USE_EVYAW) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angle_err_var_vec);
// calculate initial earth magnetic field states
_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);
}
// 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);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;
return true;
}
