// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations void NavEKF2_core::setWindMagStateLearningMode() { // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE)); // determine if the vehicle is manoevring if (accNavMagHoriz > 0.5f) { manoeuvring = true; } else { manoeuvring = false; } // Determine if learning of magnetic field states has been requested by the user bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3); // Deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2); // Inhibit the magnetic field calibration if not requested or denied inhibitMagStates = (!magCalRequested || magCalDenied); // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations // if we are not using those states if (inhibitMagStates && inhibitWindStates) { stateIndexLim = 15; } else if (inhibitWindStates) { stateIndexLim = 21; } else { stateIndexLim = 23; } }
// Update the filter status void NavEKF2_core::updateFilterStatus(void) { // init return value filterStatus.value = 0; bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid; bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && delAngBiasLearned; bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned; bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin; // set individual flags filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching }
// check for new magnetometer data and update store measurements if available void NavEKF2_core::readMagData() { // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable // magnetometer, then declare the magnetometers as failed for this flight uint8_t maxCount = _ahrs->get_compass()->get_count(); if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) { allMagSensorsFailed = true; return; } // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) { // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) { // search through the list of magnetometers for (uint8_t i=1; i<maxCount; i++) { uint8_t tempIndex = magSelectIndex + i; // loop back to the start index if we have exceeded the bounds if (tempIndex >= maxCount) { tempIndex -= maxCount; } // if the magnetometer is allowed to be used for yaw and has a different index, we start using it if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) { magSelectIndex = tempIndex; hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex); // reset the timeout flag and timer magTimeout = false; lastHealthyMagTime_ms = imuSampleTime_ms; // zero the learned magnetometer bias states stateStruct.body_magfield.zero(); // clear the measurement buffer storedMag.reset(); } } } // store time of last measurement update lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex); // estimate of time magnetometer measurement was taken, allowing for delays magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms; // Correct for the average intersampling delay due to the filter updaterate magDataNew.time_ms -= localFilterTimeStep_ms/2; // read compass data and scale to improve numerical conditioning magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f; // check for consistent data between magnetometers consistentMagData = _ahrs->get_compass()->consistent(); // save magnetometer measurement to buffer to be fused later storedMag.push(magDataNew); } }
// check for new magnetometer data and update store measurements if available void NavEKF2_core::readMagData() { // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) { // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets uint8_t maxCount = _ahrs->get_compass()->get_count(); if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) { // search through the list of magnetometers for (uint8_t i=1; i<maxCount; i++) { uint8_t tempIndex = magSelectIndex + i; // loop back to the start index if we have exceeded the bounds if (tempIndex >= maxCount) { tempIndex -= maxCount; } // if the magnetometer is allowed to be used for yaw and has a different index, we start using it if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) { magSelectIndex = tempIndex; hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex); // reset the timeout flag and timer magTimeout = false; lastHealthyMagTime_ms = imuSampleTime_ms; // zero the learned magnetometer bias states stateStruct.body_magfield.zero(); // clear the measurement buffer memset(&storedMag[0], 0, sizeof(storedMag)); } } } // store time of last measurement update lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex); // estimate of time magnetometer measurement was taken, allowing for delays magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms; // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms); // read compass data and scale to improve numerical conditioning magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f; // check for consistent data between magnetometers consistentMagData = _ahrs->get_compass()->consistent(); // save magnetometer measurement to buffer to be fused later StoreMag(); } }
// 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 if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.035f) && !tiltAlignComplete) { tiltAlignComplete = true; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index); } // submit yaw and magnetic field reset request depending on whether we have compass data if (use_compass()&& !yawAlignComplete && tiltAlignComplete) { magYawResetRequest = true; } }
// 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; } }
// correct a bearing in centi-degrees for wind void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd) { if (!use_compass() || !_flags.wind_estimation) { // we are not using the compass - no wind correction, // as GPS gives course over ground already return; } // if we are using a compass for navigation, then adjust the // heading to account for wind Vector3f wind = wind_estimate(); Vector2f wind2d = Vector2f(wind.x, wind.y); float speed; if (airspeed_estimate(&speed)) { Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed; Vector2f nav_adjusted = nav_vector - wind2d; nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100; } }
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations void NavEKF2_core::setWindMagStateLearningMode() { // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE)); // determine if the vehicle is manoevring if (accNavMagHoriz > 0.5f) { manoeuvring = true; } else { manoeuvring = false; } // Determine if learning of magnetic field states has been requested by the user bool magCalRequested = ((frontend->_magCal == 0) && inFlight) || // when flying ((frontend->_magCal == 1) && manoeuvring) || // when manoeuvring ((frontend->_magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed (frontend->_magCal == 4); // all the time // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user) bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4); // Inhibit the magnetic field calibration if not requested or denied inhibitMagStates = (!magCalRequested || magCalDenied); // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed // because we want it re-done for each takeoff if (onGround) { firstMagYawInit = false; } // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations // if we are not using those states if (inhibitMagStates && inhibitWindStates) { stateIndexLim = 15; } else if (inhibitWindStates) { stateIndexLim = 21; } else { stateIndexLim = 23; } }
// Check the tilt and yaw alignmnent status // Used during initial bootstrap alignment of the filter void NavEKF2_core::checkAttitudeAlignmentStatus() { // Check for tilt convergence - used during initial alignment float alpha = 1.0f*imuDataDelayed.delAngDT; float temp=tiltErrVec.length(); tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; if (tiltErrFilt < 0.005f && !tiltAlignComplete) { tiltAlignComplete = true; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index); } // submit yaw and magnetic field reset requests depending on whether we have compass data if (tiltAlignComplete && !yawAlignComplete) { if (use_compass()) { magYawResetRequest = true; gpsYawResetRequest = false; } else { magYawResetRequest = false; gpsYawResetRequest = true; } } }
// check for new magnetometer data and update store measurements if available void NavEKF2_core::readMagData() { // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) { // store time of last measurement update lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(); // estimate of time magnetometer measurement was taken, allowing for delays magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms; // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms); // read compass data and scale to improve numerical conditioning magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f; // check for consistent data between magnetometers consistentMagData = _ahrs->get_compass()->consistent(); // check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations if (_ahrs->get_compass()->healthy(0)) { Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0); bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z)); // Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration if (changeDetected && firstMagYawInit) { stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f; stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f; stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f; } lastMagOffsets = nowMagOffsets; } // save magnetometer measurement to buffer to be fused later StoreMag(); } }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // rotate accelerometer values into the earth frame for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (_ins.get_accel_health(i)) { _accel_ef[i] = _dcm_matrix * _ins.get_accel(i); // integrate the accel vector in the earth frame between GPS readings _ra_sum[i] += _accel_ef[i] * deltat; } } // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps() || _gps.status() < AP_GPS::GPS_OK_FIX_3D || _gps.num_sats() < _gps_minsats) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. // // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2f) { // not enough time has accumulated return; } float airspeed; if (_airspeed && _airspeed->use()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = hal.scheduler->millis(); _have_gps_lock = false; } else { if (_gps.last_fix_time_ms() == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = _gps.velocity(); last_correction_time = _gps.last_fix_time_ms(); if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } if (have_gps()) { // use GPS for positioning with any fix, even a 2D fix _last_lat = _gps.location().lat; _last_lng = _gps.location().lng; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we can update using // dead-reckoning from then on _have_position = true; } else { // update dead-reckoning position estimate _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; GA_e = Vector3f(0, 0, -1.0f); bool using_gps_corrections = false; float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS); if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) { float v_scale = gps_gain.get() * ra_scale; Vector3f vdelta = (velocity - _last_velocity) * v_scale; GA_e += vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information _last_failure_ms = hal.scheduler->millis(); return; } using_gps_corrections = true; } // calculate the error term in earth frame. // we do this for each available accelerometer then pick the // accelerometer that leads to the smallest error term. This takes // advantage of the different sample rates on different // accelerometers to dramatically reduce the impact of aliasing // due to harmonics of vibrations that match closely the sampling // rate of our accelerometers. On the Pixhawk we have the LSM303D // running at 800Hz and the MPU6000 running at 1kHz, by combining // the two the effects of aliasing are greatly reduced. Vector3f error[INS_MAX_INSTANCES]; Vector3f GA_b[INS_MAX_INSTANCES]; int8_t besti = -1; float best_error = 0; for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (!_ins.get_accel_health(i)) { // only use healthy sensors continue; } _ra_sum[i] *= ra_scale; // get the delayed ra_sum to match the GPS lag if (using_gps_corrections) { GA_b[i] = ra_delayed(i, _ra_sum[i]); } else { GA_b[i] = _ra_sum[i]; } if (GA_b[i].is_zero()) { // wait for some non-zero acceleration information continue; } GA_b[i].normalize(); if (GA_b[i].is_inf()) { // wait for some non-zero acceleration information continue; } error[i] = GA_b[i] % GA_e; float error_length = error[i].length(); if (besti == -1 || error_length < best_error) { besti = i; best_error = error_length; } } if (besti == -1) { // no healthy accelerometers! _last_failure_ms = hal.scheduler->millis(); return; } _active_accel_instance = besti; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = pythagorous2(GA_e.x, GA_e.y); // equation 11 float theta = atan2f(GA_b[besti].y, GA_b[besti].x); // equation 12 Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z); // step 6 error = GA_b[besti] % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (use_compass()) { if (have_gps() && gps_gain == 1.0f) { error[besti].z *= sinf(fabsf(roll)); } else { error[besti].z = 0; } } // if ins is unhealthy then stop attitude drift correction and // hope the gyros are OK for a while. Just slowly reduce _omega_P // to prevent previous bad accels from throwing us off if (!_ins.healthy()) { error[besti].zero(); } else { // convert the error term to body frame error[besti] = _dcm_matrix.mul_transpose(error[besti]); } if (error[besti].is_nan() || error[besti].is_inf()) { // don't allow bad values check_matrix(); _last_failure_ms = hal.scheduler->millis(); return; } _error_rp_sum += best_error; _error_rp_count++; // base the P gain on the spin rate float spin_rate = _omega.length(); // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error[besti] * _P_gain(spin_rate) * _kp; if (_flags.fast_ground_gains) { _omega_P *= 8; } if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && _gps.ground_speed() < GPS_SPEED_MIN && _ins.get_accel().x >= 7 && pitch_sensor > -3000 && pitch_sensor < 3000) { // assume we are in a launch acceleration, and reduce the // rp gain by 50% to reduce the impact of GPS lag on // takeoff attitude when using a catapult _omega_P *= 0.5f; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error[besti] * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step memset(&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; }
// yaw drift correction using the compass or GPS // this function prodoces the _omega_yaw_P vector, and also // contributes to the _omega_I.z long term yaw drift estimate void AP_AHRS_DCM::drift_correction_yaw(void) { bool new_value = false; float yaw_error; float yaw_deltat; if (use_compass()) { /* we are using compass for yaw */ if (_compass->last_update != _compass_last_update) { yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f; _compass_last_update = _compass->last_update; // we force an additional compass read() // here. This has the effect of throwing away // the first compass value, which can be bad if (!_flags.have_initial_yaw && _compass->read()) { float heading = _compass->calculate_heading(_dcm_matrix); _dcm_matrix.from_euler(roll, pitch, heading); _omega_yaw_P.zero(); _flags.have_initial_yaw = true; } new_value = true; yaw_error = yaw_error_compass(); // also update the _gps_last_update, so if we later // disable the compass due to significant yaw error we // don't suddenly change yaw with a reset _gps_last_update = _gps.last_fix_time_ms(); } } else if (_flags.fly_forward && have_gps()) { /* we are using GPS for yaw */ if (_gps.last_fix_time_ms() != _gps_last_update && _gps.ground_speed() >= GPS_SPEED_MIN) { yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f; _gps_last_update = _gps.last_fix_time_ms(); new_value = true; float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f); float yaw_error_rad = wrap_PI(gps_course_rad - yaw); yaw_error = sinf(yaw_error_rad); /* reset yaw to match GPS heading under any of the following 3 conditions: 1) if we have reached GPS_SPEED_MIN and have never had yaw information before 2) if the last time we got yaw information from the GPS is more than 20 seconds ago, which means we may have suffered from considerable gyro drift 3) if we are over 3*GPS_SPEED_MIN (which means 9m/s) and our yaw error is over 60 degrees, which means very poor yaw. This can happen on bungee launch when the operator pulls back the plane rapidly enough then on release the GPS heading changes very rapidly */ if (!_flags.have_initial_yaw || yaw_deltat > 20 || (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) { // reset DCM matrix based on current yaw _dcm_matrix.from_euler(roll, pitch, gps_course_rad); _omega_yaw_P.zero(); _flags.have_initial_yaw = true; yaw_error = 0; } } } if (!new_value) { // we don't have any new yaw information // slowly decay _omega_yaw_P to cope with loss // of our yaw source _omega_yaw_P *= 0.97f; return; } // convert the error vector to body frame float error_z = _dcm_matrix.c.z * yaw_error; // the spin rate changes the P gain, and disables the // integration at higher rates float spin_rate = _omega.length(); // update the proportional control to drag the // yaw back to the right value. We use a gain // that depends on the spin rate. See the fastRotations.pdf // paper from Bill Premerlani // We also adjust the gain depending on the rate of change of horizontal velocity which // is proportional to how observable the heading is from the acceerations and GPS velocity // The accelration derived heading will be more reliable in turns than compass or GPS _omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain(); if (_flags.fast_ground_gains) { _omega_yaw_P.z *= 8; } // don't update the drift term if we lost the yaw reference // for more than 2 seconds if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) { // also add to the I term _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat; } _error_yaw_sum += fabsf(yaw_error); _error_yaw_count++; }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Matrix3f temp_dcm = _dcm_matrix; Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // apply trim temp_dcm.rotateXY(_trim); // rotate accelerometer values into the earth frame _accel_ef = temp_dcm * _accel_vector; // integrate the accel vector in the earth frame between GPS readings _ra_sum += _accel_ef * deltat; // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps() || _gps->status() < GPS::GPS_OK_FIX_3D || _gps->num_sats < _gps_minsats) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. // // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2f) { // not enough time has accumulated return; } float airspeed; if (_airspeed && _airspeed->use()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = hal.scheduler->millis(); _have_gps_lock = false; } else { if (_gps->last_fix_time == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down()); last_correction_time = _gps->last_fix_time; if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } if (have_gps()) { // use GPS for positioning with any fix, even a 2D fix _last_lat = _gps->latitude; _last_lng = _gps->longitude; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we can update using // dead-reckoning from then on _have_position = true; } else { // update dead-reckoning position estimate _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; GA_e = Vector3f(0, 0, -1.0f); if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) { float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS); Vector3f vdelta = (velocity - _last_velocity) * v_scale; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f); GA_e += vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information return; } } // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS); float length = GA_b.length(); if (length > 1.0f) { GA_b /= length; if (GA_b.is_inf()) { // wait for some non-zero acceleration information return; } } Vector3f error = GA_b % GA_e; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = pythagorous2(GA_e.x, GA_e.y); // equation 11 float theta = atan2f(GA_b.y, GA_b.x); // equation 12 Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z); // step 6 error = GA_b % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (use_compass()) { if (have_gps() && gps_gain == 1.0f) { error.z *= sinf(fabsf(roll)); } else { error.z = 0; } } // convert the error term to body frame error = _dcm_matrix.mul_transpose(error); if (error.is_nan() || error.is_inf()) { // don't allow bad values check_matrix(); return; } _error_rp_sum += error.length(); _error_rp_count++; // base the P gain on the spin rate float spin_rate = _omega.length(); // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error * _P_gain(spin_rate) * _kp; if (_flags.fast_ground_gains) { _omega_P *= 8; } if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D && _gps->ground_speed_cm < GPS_SPEED_MIN && _accel_vector.x >= 7 && pitch_sensor > -3000 && pitch_sensor < 3000) { // assume we are in a launch acceleration, and reduce the // rp gain by 50% to reduce the impact of GPS lag on // takeoff attitude when using a catapult _omega_P *= 0.5f; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step _ra_sum.zero(); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; if (_have_gps_lock && _flags.fly_forward) { // update wind estimate estimate_wind(velocity); } }
// check for new magnetometer data and update store measurements if available void NavEKF3_core::readMagData() { if (!_ahrs->get_compass()) { allMagSensorsFailed = true; return; } // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable // magnetometer, then declare the magnetometers as failed for this flight uint8_t maxCount = _ahrs->get_compass()->get_count(); if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) { allMagSensorsFailed = true; return; } // limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer if (use_compass() && ((_ahrs->get_compass()->last_update_usec() - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) { frontend->logging.log_compass = true; // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) { // search through the list of magnetometers for (uint8_t i=1; i<maxCount; i++) { uint8_t tempIndex = magSelectIndex + i; // loop back to the start index if we have exceeded the bounds if (tempIndex >= maxCount) { tempIndex -= maxCount; } // if the magnetometer is allowed to be used for yaw and has a different index, we start using it if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) { magSelectIndex = tempIndex; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex); // reset the timeout flag and timer magTimeout = false; lastHealthyMagTime_ms = imuSampleTime_ms; // zero the learned magnetometer bias states stateStruct.body_magfield.zero(); // clear the measurement buffer storedMag.reset(); // clear the data waiting flag so that we do not use any data pending from the previous sensor magDataToFuse = false; // request a reset of the magnetic field states magStateResetRequest = true; // declare the field unlearned so that the reset request will be obeyed magFieldLearned = false; } } } // detect changes to magnetometer offset parameters and reset states Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex); bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets); if (changeDetected) { // zero the learned magnetometer bias states stateStruct.body_magfield.zero(); // clear the measurement buffer storedMag.reset(); } lastMagOffsets = nowMagOffsets; lastMagOffsetsValid = true; // store time of last measurement update lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex); // estimate of time magnetometer measurement was taken, allowing for delays magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms; // Correct for the average intersampling delay due to the filter updaterate magDataNew.time_ms -= localFilterTimeStep_ms/2; // read compass data and scale to improve numerical conditioning magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f; // check for consistent data between magnetometers consistentMagData = _ahrs->get_compass()->consistent(); // save magnetometer measurement to buffer to be fused later storedMag.push(magDataNew); } }
/* Monitor GPS data to see if quality is good enough to initialise the EKF Monitor magnetometer innovations to to see if the heading is good enough to use GPS Return true if all criteria pass for 10 seconds We also record the failure reason so that prearm_failure_reason() can give a good report to the user on why arming is failing */ bool NavEKF2_core::calcGpsGoodToAlign(void) { if (inFlight && assume_zero_sideslip() && !use_compass()) { // this is a special case where a plane has launched without magnetometer // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible return true; } // User defined multiplier to be applied to check thresholds float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler; // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states // This enables us to handle large changes to the external magnetic field environment that occur before arming if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) { magYawResetTimer_ms = imuSampleTime_ms; } if (imuSampleTime_ms - magYawResetTimer_ms > 5000) { // reset heading and field states Vector3f eulerAngles; getEulerAngles(eulerAngles); stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds magYawResetTimer_ms = imuSampleTime_ms; } // Check for significant change in GPS position if disarmed which indicates bad GPS // This check can only be used when the vehicle is stationary const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location const float posFiltTimeConst = 10.0f; // time constant used to decay position drift // calculate time lapsesd since last update and limit to prevent numerical errors float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms; // Sum distance moved gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length(); gpsloc_prev = gpsloc; // Decay distance moved exponentially to zero gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); // Clamp the fiter state to prevent excessive persistence of large transients gpsDriftNE = MIN(gpsDriftNE,10.0f); // Fail if more than 3 metres drift after filtering whilst on-ground // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT); // Report check result as a text string and bitmask if (gpsDriftFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); gpsCheckStatus.bad_horiz_drift = true; } else { gpsCheckStatus.bad_horiz_drift = false; } // Check that the vertical GPS vertical velocity is reasonable after noise filtering bool gpsVertVelFail; if (_ahrs->get_gps().have_vertical_velocity() && onGround) { // check that the average vertical GPS velocity is close to zero gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); } else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) { // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail gpsVertVelFail = true; // if we have a 3D fix with no vertical velocity and // EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not // capable of giving a vertical velocity if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { frontend->_fusionModeGPS.set(1); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1"); } } else { gpsVertVelFail = false; } // Report check result as a text string and bitmask if (gpsVertVelFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); gpsCheckStatus.bad_vert_vel = true; } else { gpsCheckStatus.bad_vert_vel = false; } // Check that the horizontal GPS vertical velocity is reasonable after noise filtering // This check can only be used if the vehicle is stationary bool gpsHorizVelFail; if (onGround) { gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); } else { gpsHorizVelFail = false; } // Report check result as a text string and bitmask if (gpsHorizVelFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); gpsCheckStatus.bad_horiz_vel = true; } else { gpsCheckStatus.bad_horiz_vel = false; } // fail if horiziontal position accuracy not sufficient float hAcc = 0.0f; bool hAccFail; if (_ahrs->get_gps().horizontal_accuracy(hAcc)) { hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); } else { hAccFail = false; } // Report check result as a text string and bitmask if (hAccFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); gpsCheckStatus.bad_hAcc = true; } else { gpsCheckStatus.bad_hAcc = false; } // fail if reported speed accuracy greater than threshold bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR); // Report check result as a text string and bitmask if (gpsSpdAccFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS speed error %.1f (needs %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); gpsCheckStatus.bad_sAcc = true; } else { gpsCheckStatus.bad_sAcc = false; } // fail if satellite geometry is poor bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP); // Report check result as a text string and bitmask if (hdopFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop())); gpsCheckStatus.bad_hdop = true; } else { gpsCheckStatus.bad_hdop = false; } // fail if not enough sats bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS); // Report check result as a text string and bitmask if (numSatsFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats()); gpsCheckStatus.bad_sats = true; } else { gpsCheckStatus.bad_sats = false; } // fail if magnetometer innovations are outside limits indicating bad yaw // with bad yaw we are unable to use GPS bool yawFail; if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) { yawFail = true; } else { yawFail = false; } // Report check result as a text string and bitmask if (yawFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Mag yaw error x=%.1f y=%.1f", (double)magTestRatio.x, (double)magTestRatio.y); gpsCheckStatus.bad_yaw = true; } else { gpsCheckStatus.bad_yaw = false; } // assume failed first time through and notify user checks have started if (lastGpsVelFail_ms == 0) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks"); lastGpsVelFail_ms = imuSampleTime_ms; } // record time of fail if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) { lastGpsVelFail_ms = imuSampleTime_ms; } // continuous period without fail required to return a healthy status if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { return true; } return false; }
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations void NavEKF2_core::setWindMagStateLearningMode() { // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE); if (!inhibitWindStates && setWindInhibit) { inhibitWindStates = true; } else if (inhibitWindStates && !setWindInhibit) { inhibitWindStates = false; // set states and variances if (yawAlignComplete && useAirspeed()) { // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading // which assumes the vehicle has launched into the wind Vector3f tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z); stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z); // set the wind sate variances to the measurement uncertainty for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f)); } } else { // set the variances using a typical wind speed for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(5.0f); } } } // determine if the vehicle is manoevring if (accNavMagHoriz > 0.5f) { manoeuvring = true; } else { manoeuvring = false; } // Determine if learning of magnetic field states has been requested by the user uint8_t magCal = effective_magCal(); bool magCalRequested = ((magCal == 0) && inFlight) || // when flying ((magCal == 1) && manoeuvring) || // when manoeuvring ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete (magCal == 4); // all the time // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user) bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4); // Inhibit the magnetic field calibration if not requested or denied bool setMagInhibit = !magCalRequested || magCalDenied; if (!inhibitMagStates && setMagInhibit) { inhibitMagStates = true; } else if (inhibitMagStates && !setMagInhibit) { inhibitMagStates = false; if (magFieldLearned) { // if we have already learned the field states, then retain the learned variances P[16][16] = earthMagFieldVar.x; P[17][17] = earthMagFieldVar.y; P[18][18] = earthMagFieldVar.z; P[19][19] = bodyMagFieldVar.x; P[20][20] = bodyMagFieldVar.y; P[21][21] = bodyMagFieldVar.z; } else { // set the variances equal to the observation variances for (uint8_t index=18; index<=21; index++) { P[index][index] = sq(frontend->_magNoise); } // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances alignMagStateDeclination(); } // request a reset of the yaw and magnetic field states if not done before if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) { magYawResetRequest = true; } } // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed // because we want it re-done for each takeoff if (onGround) { finalInflightYawInit = false; finalInflightMagInit = false; } // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations // if we are not using those states if (inhibitMagStates && inhibitWindStates) { stateIndexLim = 15; } else if (inhibitWindStates) { stateIndexLim = 21; } else { stateIndexLim = 23; } }
// select fusion of velocity, position and height measurements void NavEKF2_core::SelectVelPosFusion() { // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // If so, don't fuse measurements on this time step to reduce frame over-runs // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) { posVelFusionDelayed = true; return; } else { posVelFusionDelayed = false; } // Check for data at the fusion time horizon extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms); // read GPS data from the sensor and check for new data in the buffer readGpsData(); gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); // Determine if we need to fuse position and velocity data on this time step if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { // set fusion request flags if (frontend->_fusionModeGPS <= 1) { fuseVelData = true; } else { fuseVelData = false; } fusePosData = true; extNavUsedForPos = false; // correct GPS data for position offset of antenna phase centre relative to the IMU Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; if (!posOffsetBody.is_zero()) { // Don't fuse velocity data if GPS doesn't support it if (fuseVelData) { // TODO use a filtered angular rate with a group delay that matches the GPS delay Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); Vector3f velOffsetBody = angRate % posOffsetBody; Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); gpsDataDelayed.vel.x -= velOffsetEarth.x; gpsDataDelayed.vel.y -= velOffsetEarth.y; gpsDataDelayed.vel.z -= velOffsetEarth.z; } Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); gpsDataDelayed.pos.x -= posOffsetEarth.x; gpsDataDelayed.pos.y -= posOffsetEarth.y; gpsDataDelayed.hgt += posOffsetEarth.z; } // copy corrected GPS data to observation vector if (fuseVelData) { velPosObs[0] = gpsDataDelayed.vel.x; velPosObs[1] = gpsDataDelayed.vel.y; velPosObs[2] = gpsDataDelayed.vel.z; } velPosObs[3] = gpsDataDelayed.pos.x; velPosObs[4] = gpsDataDelayed.pos.y; } else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) { // This is a special case that uses and external nav system for position extNavUsedForPos = true; activeHgtSource = HGT_SOURCE_EV; fuseVelData = false; fuseHgtData = true; fusePosData = true; velPosObs[3] = extNavDataDelayed.pos.x; velPosObs[4] = extNavDataDelayed.pos.y; velPosObs[5] = extNavDataDelayed.pos.z; // if compass is disabled, also use it for yaw if (!use_compass()) { extNavUsedForYaw = true; if (!yawAlignComplete) { extNavYawResetRequest = true; magYawResetRequest = false; gpsYawResetRequest = false; controlMagYawReset(); finalInflightYawInit = true; } else { fuseEulerYaw(); } } else { extNavUsedForYaw = false; } } else { fuseVelData = false; fusePosData = false; } // we have GPS data to fuse and a request to align the yaw using the GPS course if (gpsYawResetRequest) { realignYawGPS(); } // Select height data to be fused from the available baro, range finder and GPS sources selectHeightForFusion(); // if we are using GPS, check for a change in receiver and reset position and height if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) { // record the ID of the GPS that we are using for the reset last_gps_idx = gpsDataDelayed.sensor_idx; // Store the position before the reset so that we can record the reset delta posResetNE.x = stateStruct.position.x; posResetNE.y = stateStruct.position.y; // Set the position states to the position from the new GPS stateStruct.position.x = gpsDataNew.pos.x; stateStruct.position.y = gpsDataNew.pos.y; // Calculate the position offset due to the reset posResetNE.x = stateStruct.position.x - posResetNE.x; posResetNE.y = stateStruct.position.y - posResetNE.y; // Add the offset to the output observer states for (uint8_t i=0; i<imu_buffer_length; i++) { storedOutput[i].position.x += posResetNE.x; storedOutput[i].position.y += posResetNE.y; } outputDataNew.position.x += posResetNE.x; outputDataNew.position.y += posResetNE.y; outputDataDelayed.position.x += posResetNE.x; outputDataDelayed.position.y += posResetNE.y; // store the time of the reset lastPosReset_ms = imuSampleTime_ms; // If we are alseo using GPS as the height reference, reset the height if (activeHgtSource == HGT_SOURCE_GPS) { // Store the position before the reset so that we can record the reset delta posResetD = stateStruct.position.z; // write to the state vector stateStruct.position.z = -hgtMea; // Calculate the position jump due to the reset posResetD = stateStruct.position.z - posResetD; // Add the offset to the output observer states outputDataNew.position.z += posResetD; outputDataDelayed.position.z += posResetD; for (uint8_t i=0; i<imu_buffer_length; i++) { storedOutput[i].position.z += posResetD; } // store the time of the reset lastPosResetD_ms = imuSampleTime_ms; } } // If we are operating without any aiding, fuse in the last known position // to constrain tilt drift. This assumes a non-manoeuvring vehicle // Do this to coincide with the height fusion if (fuseHgtData && PV_AidingMode == AID_NONE) { velPosObs[3] = lastKnownPositionNE.x; velPosObs[4] = lastKnownPositionNE.y; fusePosData = true; fuseVelData = false; } // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { FuseVelPosNED(); // clear the flags to prevent repeated fusion of the same data fuseVelData = false; fuseHgtData = false; fusePosData = false; } }