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(); }
// 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; }