#include #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 #include "AP_NavEKF2.h" #include "AP_NavEKF2_core.h" #include #include #include #include extern const AP_HAL::HAL& hal; /******************************************************** * OPT FLOW AND RANGE FINDER * ********************************************************/ // Read the range finder and take new measurements if available // Apply a median filter void NavEKF2_core::readRangeFinder(void) { uint8_t midIndex; uint8_t maxIndex; uint8_t minIndex; // get theoretical correct range when the vehicle is on the ground // don't allow range to go below 5cm because this can cause problems with optical flow processing rngOnGnd = MAX(frontend->_rng.ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); // read range finder at 20Hz // TODO better way of knowing if it has new data if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) { // reset the timer used to control the measurement rate lastRngMeasTime_ms = imuSampleTime_ms; // store samples and sample time into a ring buffer if valid // use data from two range finders if available for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) { if ((frontend->_rng.get_orientation(sensorIndex) == ROTATION_PITCH_270) && (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good)) { rngMeasIndex[sensorIndex] ++; if (rngMeasIndex[sensorIndex] > 2) { rngMeasIndex[sensorIndex] = 0; } storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = frontend->_rng.distance_cm(sensorIndex) * 0.01f; } // check for three fresh samples bool sampleFresh[2][3] = {}; for (uint8_t index = 0; index <= 2; index++) { sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500; } // find the median value if we have three fresh samples if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) { if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) { minIndex = 1; maxIndex = 0; } else { minIndex = 0; maxIndex = 1; } if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) { midIndex = maxIndex; } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) { midIndex = minIndex; } else { midIndex = 2; } // don't allow time to go backwards if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) { rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex]; } // limit the measured range to be no less than the on-ground range rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd); // get position in body frame for the current sensor rangeDataNew.sensor_idx = sensorIndex; // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it storedRange.push(rangeDataNew); // indicate we have updated the measurement rngValidMeaTime_ms = imuSampleTime_ms; } else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) { // before takeoff we assume on-ground range value if there is no data rangeDataNew.time_ms = imuSampleTime_ms; rangeDataNew.rng = rngOnGnd; rangeDataNew.time_ms = imuSampleTime_ms; // don't allow time to go backwards if (imuSampleTime_ms > rangeDataNew.time_ms) { rangeDataNew.time_ms = imuSampleTime_ms; } // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it storedRange.push(rangeDataNew); // indicate we have updated the measurement rngValidMeaTime_ms = imuSampleTime_ms; } } } } // write the raw optical flow measurements // this needs to be called externally. void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset) { // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: // A positive X rate is produced by a positive sensor rotation about the X axis // A positive Y rate is produced by a positive sensor rotation about the Y axis // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate flowMeaTime_ms = imuSampleTime_ms; // calculate bias errors on flow sensor gyro rates, but protect against spikes in data // reset the accumulated body delta angle and time // don't do the calculation if not enough time lapsed for a reliable body rate measurement if (delTimeOF > 0.01f) { flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f); flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f); delAngBodyOF.zero(); delTimeOF = 0.0f; } // by definition if this function is called, then flow measurements have been provided so we // need to run the optical flow takeoff detection detectOptFlowTakeoff(); // calculate rotation matrices at mid sample time for flow observations stateStruct.quat.rotation_matrix(Tbn_flow); // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data) if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) { // correct flow sensor body rates for bias and write ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x; ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y; // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead if (delTimeOF > 0.001f) { // first preference is to use the rate averaged over the same sampling period as the flow sensor ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF; } else if (imuDataNew.delAngDT > 0.001f){ // second preference is to use most recent IMU data ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT; } else { // third preference is use zero ofDataNew.bodyRadXYZ.z = 0.0f; } // write uncorrected flow rate measurements // note correction for different axis and sign conventions used by the px4flow sensor ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) // write the flow sensor position in body frame ofDataNew.body_offset = &posOffset; // write flow rate measurements corrected for body rates ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; // record time last observation was received so we can detect loss of data elsewhere flowValidMeaTime_ms = imuSampleTime_ms; // estimate sample time of the measurement ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2; // Correct for the average intersampling delay due to the filter updaterate ofDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms); // Save data to buffer storedOF.push(ofDataNew); // Check for data at the fusion time horizon flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); } } /******************************************************** * MAGNETOMETER * ********************************************************/ // check for new magnetometer data and update store measurements if available void NavEKF2_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; } // 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) { 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) { 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, "EKF2 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); } } /******************************************************** * Inertial Measurements * ********************************************************/ /* * Read IMU delta angle and delta velocity measurements and downsample to 100Hz * for storage in the data buffers used by the EKF. If the IMU data arrives at * lower rate than 100Hz, then no downsampling or upsampling will be performed. * Downsampling is done using a method that does not introduce coning or sculling * errors. */ void NavEKF2_core::readIMUData() { const AP_InertialSensor &ins = _ahrs->get_ins(); // average IMU sampling rate dtIMUavg = ins.get_loop_delta_t(); // the imu sample time is used as a common time reference throughout the filter imuSampleTime_ms = AP_HAL::millis(); // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); accelPosOffset = ins.get_imu_pos_offset(imu_index); } else { readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel()); } // Get delta angle data from primary gyro or primary if not available if (ins.use_gyro(imu_index)) { readDeltaAngle(imu_index, imuDataNew.delAng); } else { readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); } imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f); // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; // Accumulate the measurement time interval for the delta velocity and angle data imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT; imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT; // Rotate quaternon atitude from previous to new and normalise. // Accumulation using quaternions prevents introduction of coning errors due to downsampling imuQuatDownSampleNew.rotate(imuDataNew.delAng); imuQuatDownSampleNew.normalize(); // Rotate the latest delta velocity into body frame at the start of accumulation Matrix3f deltaRotMat; imuQuatDownSampleNew.rotation_matrix(deltaRotMat); // Apply the delta velocity to the delta velocity accumulator imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel; // Keep track of the number of IMU frames since the last state prediction framesSincePredict++; /* * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle, * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the * IMU data. */ if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) { // convert the accumulated quaternion to an equivalent delta angle imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); // Time stamp the data imuDataDownSampledNew.time_ms = imuSampleTime_ms; // Write data to the FIFO IMU buffer storedIMU.push_youngest_element(imuDataDownSampledNew); // calculate the achieved average time step rate for the EKF float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT); dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; // zero the accumulated IMU data and quaternion imuDataDownSampledNew.delAng.zero(); imuDataDownSampledNew.delVel.zero(); imuDataDownSampledNew.delAngDT = 0.0f; imuDataDownSampledNew.delVelDT = 0.0f; imuQuatDownSampleNew[0] = 1.0f; imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f; // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle framesSincePredict = 0; // set the flag to let the filter know it has new IMU data nad needs to run runUpdates = true; // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU.pop_oldest_element(); // protect against delta time going to zero // TODO - check if calculations can tolerate 0 float minDT = 0.1f*dtEkfAvg; imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); updateTimingStatistics(); // correct the extracted IMU data for sensor errors delAngCorrected = imuDataDelayed.delAng; delVelCorrected = imuDataDelayed.delVel; correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT); correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT); } else { // we don't have new IMU data in the buffer so don't run filter updates on this time step runUpdates = false; } } // read the delta velocity and corresponding time interval from the IMU // return false if data is not available bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { const AP_InertialSensor &ins = _ahrs->get_ins(); if (ins_index < ins.get_accel_count()) { ins.get_delta_velocity(ins_index,dVel); dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f); return true; } return false; } /******************************************************** * Global Position Measurement * ********************************************************/ // check for new valid GPS data and update stored measurement if available void NavEKF2_core::readGpsData() { // check for new GPS data // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) { if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // report GPS fix status gpsCheckStatus.bad_fix = false; // store fix time from previous read secondLastGpsTime_ms = lastTimeGpsReceived_ms; // get current fix time lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms(); // estimate when the GPS fix was valid, allowing for GPS processing and other delays // ideally we should be using a timing signal from the GPS receiver to set this time gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms; // Correct for the average intersampling delay due to the filter updaterate gpsDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms); // Get which GPS we are using for position information gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor(); // read the NED velocity from the GPS gpsDataNew.vel = _ahrs->get_gps().velocity(); // Use the speed and position accuracy from the GPS if available, otherwise set it to zero. // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f); gpsSpdAccuracy *= (1.0f - alpha); float gpsSpdAccRaw; if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { gpsSpdAccuracy = 0.0f; } else { gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f); } gpsPosAccuracy *= (1.0f - alpha); float gpsPosAccRaw; if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) { gpsPosAccuracy = 0.0f; } else { gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); } gpsHgtAccuracy *= (1.0f - alpha); float gpsHgtAccRaw; if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) { gpsHgtAccuracy = 0.0f; } else { gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f); } // check if we have enough GPS satellites and increase the gps noise scaler if we don't if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) { gpsNoiseScaler = 1.0f; } else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) { gpsNoiseScaler = 1.4f; } else { // <= 4 satellites or in constant position mode gpsNoiseScaler = 2.0f; } // Check if GPS can output vertical velocity and set GPS fusion mode accordingly if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) { useGpsVertVel = true; } else { useGpsVertVel = false; } // Monitor quality of the GPS velocity data before and after alignment using separate checks if (PV_AidingMode != AID_ABSOLUTE) { // Pre-alignment checks gpsGoodToAlign = calcGpsGoodToAlign(); } else { gpsGoodToAlign = false; } // Post-alignment checks calcGpsGoodForFlight(); // Read the GPS locaton in WGS-84 lat,long,height coordinates const struct Location &gpsloc = _ahrs->get_gps().location(); // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed if (gpsGoodToAlign && !validOrigin) { setOrigin(); // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances alignMagStateDeclination(); // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' EKF_origin.alt = gpsloc.alt - baroDataNew.hgt; // Set the uncertinty of the GPS origin height ekfOriginHgtVar = sq(gpsHgtAccuracy); } // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin if (validOrigin) { gpsDataNew.pos = location_diff(EKF_origin, gpsloc); gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt); storedGPS.push(gpsDataNew); // declare GPS available for use gpsNotAvailable = false; } frontend->logging.log_gps = true; } else { // report GPS fix status gpsCheckStatus.bad_fix = true; } } } // read the delta angle and corresponding time interval from the IMU // return false if data is not available bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { const AP_InertialSensor &ins = _ahrs->get_ins(); if (ins_index < ins.get_gyro_count()) { ins.get_delta_angle(ins_index,dAng); frontend->logging.log_imu = true; return true; } return false; } /******************************************************** * Height Measurements * ********************************************************/ // check for new pressure altitude measurement data and update stored measurement if available void NavEKF2_core::readBaroData() { // check to see if baro measurement has changed so we know if a new measurement has arrived // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) { frontend->logging.log_baro = true; baroDataNew.hgt = frontend->_baro.get_altitude(); // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent if (getTakeoffExpected()) { baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff); } // time stamp used to check for new measurement lastBaroReceived_ms = frontend->_baro.get_last_update(); // estimate of time height measurement was taken, allowing for delays baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms; // Correct for the average intersampling delay due to the filter updaterate baroDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms); // save baro measurement to buffer to be fused later storedBaro.push(baroDataNew); } } // calculate filtered offset between baro height measurement and EKF height estimate // offset should be subtracted from baro measurement to match filter estimate // offset is used to enable reversion to baro from alternate height data source void NavEKF2_core::calcFiltBaroOffset() { // Apply a first order LPF with spike protection baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f); } // calculate filtered offset between GPS height measurement and EKF height estimate // offset should be subtracted from GPS measurement to match filter estimate // offset is used to switch reversion to GPS from alternate height data source void NavEKF2_core::calcFiltGpsHgtOffset() { // Estimate the WGS-84 height of the EKF's origin using a Bayes filter // calculate the variance of our a-priori estimate of the ekf origin height float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f); if (activeHgtSource == HGT_SOURCE_BARO) { // Use the baro drift rate const float baroDriftRate = 0.05f; ekfOriginHgtVar += sq(baroDriftRate * deltaTime); } else if (activeHgtSource == HGT_SOURCE_RNG) { // use the worse case expected terrain gradient and vehicle horizontal speed const float maxTerrGrad = 0.25f; ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime); } else if (activeHgtSource == HGT_SOURCE_GPS) { // by definition we are using GPS height as the EKF datum in this mode // so cannot run this filter return; } lastOriginHgtTime_ms = imuDataDelayed.time_ms; // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error // when not using GPS as height source float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8]; // calculate the correction gain float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); // calculate the innovation float innovation = - stateStruct.position.z - gpsDataDelayed.hgt; // check the innovation variance ratio float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar); // correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma if (ratio < 5.0f) { EKF_origin.alt -= (int)(100.0f * gain * innovation); ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f); } } /******************************************************** * Air Speed Measurements * ********************************************************/ // check for new airspeed data and update stored measurements if available void NavEKF2_core::readAirSpdData() { // if airspeed reading is valid and is set by the user to be used and has been updated then // we take a new reading, convert from EAS to TAS and set the flag letting other functions // know a new measurement is available const AP_Airspeed *aspeed = _ahrs->get_airspeed(); if (aspeed && aspeed->use() && aspeed->last_update_ms() != timeTasReceived_ms) { tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); timeTasReceived_ms = aspeed->last_update_ms(); tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; // Correct for the average intersampling delay due to the filter update rate tasDataNew.time_ms -= localFilterTimeStep_ms/2; // Save data into the buffer to be fused when the fusion time horizon catches up with it storedTAS.push(tasDataNew); } // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); } /******************************************************** * Range Beacon Measurements * ********************************************************/ // check for new range beacon data and push to data buffer if available void NavEKF2_core::readRngBcnData() { // get the location of the beacon data const AP_Beacon *beacon = _ahrs->get_beacon(); // exit immediately if no beacon object if (beacon == nullptr) { return; } // get the number of beacons in use N_beacons = beacon->count(); // search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer bool newDataToPush = false; uint8_t numRngBcnsChecked = 0; // start the search one index up from where we left it last time uint8_t index = lastRngBcnChecked; while (!newDataToPush && numRngBcnsChecked < N_beacons) { // track the number of beacons checked numRngBcnsChecked++; // move to next beacon, wrap index if necessary index++; if (index >= N_beacons) { index = 0; } // check that the beacon is healthy and has new data if (beacon->beacon_healthy(index) && beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index]) { // set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index); rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2; // set the range noise // TODO the range library should provide the noise/accuracy estimate for each beacon rngBcnDataNew.rngErr = frontend->_rngBcnNoise; // set the range measurement rngBcnDataNew.rng = beacon->beacon_distance(index); // set the beacon position rngBcnDataNew.beacon_posNED = beacon->beacon_position(index); // identify the beacon identifier rngBcnDataNew.beacon_ID = index; // indicate we have new data to push to the buffer newDataToPush = true; // update the last checked index lastRngBcnChecked = index; } } // Check if the beacon system has returned a 3D fix if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) { rngBcnLast3DmeasTime_ms = imuSampleTime_ms; } // Check if the range beacon data can be used to align the vehicle position if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) { // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y); float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; if (posDiffSq < 9.0f*posDiffVar) { rngBcnGoodToAlign = true; // Set the EKF origin and magnetic field declination if not previously set if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) { // get origin from beacon system Location origin_loc; if (beacon->get_origin(origin_loc)) { setOriginLLH(origin_loc); // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances alignMagStateDeclination(); // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' EKF_origin.alt = origin_loc.alt - baroDataNew.hgt; // Set the uncertainty of the origin height ekfOriginHgtVar = sq(beaconVehiclePosErr); } } } else { rngBcnGoodToAlign = false; } } else { rngBcnGoodToAlign = false; } // Save data into the buffer to be fused when the fusion time horizon catches up with it if (newDataToPush) { storedRangeBeacon.push(rngBcnDataNew); } // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms); } /* update timing statistics structure */ void NavEKF2_core::updateTimingStatistics(void) { if (timing.count == 0) { timing.dtIMUavg_max = dtIMUavg; timing.dtIMUavg_min = dtIMUavg; timing.dtEKFavg_max = dtEkfAvg; timing.dtEKFavg_min = dtEkfAvg; timing.delAngDT_max = imuDataDelayed.delAngDT; timing.delAngDT_min = imuDataDelayed.delAngDT; timing.delVelDT_max = imuDataDelayed.delVelDT; timing.delVelDT_min = imuDataDelayed.delVelDT; } else { timing.dtIMUavg_max = MAX(timing.dtIMUavg_max, dtIMUavg); timing.dtIMUavg_min = MIN(timing.dtIMUavg_min, dtIMUavg); timing.dtEKFavg_max = MAX(timing.dtEKFavg_max, dtEkfAvg); timing.dtEKFavg_min = MIN(timing.dtEKFavg_min, dtEkfAvg); timing.delAngDT_max = MAX(timing.delAngDT_max, imuDataDelayed.delAngDT); timing.delAngDT_min = MIN(timing.delAngDT_min, imuDataDelayed.delAngDT); timing.delVelDT_max = MAX(timing.delVelDT_max, imuDataDelayed.delVelDT); timing.delVelDT_min = MIN(timing.delVelDT_min, imuDataDelayed.delVelDT); } timing.count++; } // get timing statistics structure void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing) { _timing = timing; memset(&timing, 0, sizeof(timing)); } #endif // HAL_CPU_CLASS