2016-07-14 02:08:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF3.h"
# include "AP_NavEKF3_core.h"
# include <AP_AHRS/AP_AHRS.h>
# include <AP_Vehicle/AP_Vehicle.h>
# include <GCS_MAVLink/GCS.h>
2019-11-26 02:45:14 -04:00
# include <AP_RangeFinder/AP_RangeFinder.h>
2019-11-10 22:48:14 -04:00
# include <AP_RangeFinder/AP_RangeFinder_Backend.h>
2019-06-13 23:44:39 -03:00
# include <AP_GPS/AP_GPS.h>
2019-06-26 23:34:12 -03:00
# include <AP_Baro/AP_Baro.h>
2020-03-11 05:05:16 -03:00
# include <AP_Compass/AP_Compass.h>
2016-07-14 02:08:43 -03:00
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 NavEKF3_core : : readRangeFinder ( void )
{
uint8_t midIndex ;
uint8_t maxIndex ;
uint8_t minIndex ;
// get theoretical correct range when the vehicle is on the ground
2017-02-09 06:27:52 -04:00
// don't allow range to go below 5cm because this can cause problems with optical flow processing
2019-11-26 02:45:14 -04:00
const RangeFinder * _rng = AP : : rangefinder ( ) ;
if ( _rng = = nullptr ) {
return ;
}
rngOnGnd = MAX ( _rng - > ground_clearance_cm_orient ( ROTATION_PITCH_270 ) * 0.01f , 0.05f ) ;
2016-07-14 02:08:43 -03:00
2016-12-17 18:02:38 -04:00
// limit update rate to maximum allowed by data buffers
if ( ( imuSampleTime_ms - lastRngMeasTime_ms ) > frontend - > sensorIntervalMin_ms ) {
2016-07-14 02:08:43 -03:00
// 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 + + ) {
2019-11-26 02:45:14 -04:00
AP_RangeFinder_Backend * sensor = _rng - > get_backend ( sensorIndex ) ;
2017-08-08 03:14:53 -03:00
if ( sensor = = nullptr ) {
continue ;
}
2019-11-01 02:11:26 -03:00
if ( ( sensor - > orientation ( ) = = ROTATION_PITCH_270 ) & & ( sensor - > status ( ) = = RangeFinder : : Status : : Good ) ) {
2016-07-14 02:08:43 -03:00
rngMeasIndex [ sensorIndex ] + + ;
if ( rngMeasIndex [ sensorIndex ] > 2 ) {
rngMeasIndex [ sensorIndex ] = 0 ;
}
storedRngMeasTime_ms [ sensorIndex ] [ rngMeasIndex [ sensorIndex ] ] = imuSampleTime_ms - 25 ;
2017-08-08 03:14:53 -03:00
storedRngMeas [ sensorIndex ] [ rngMeasIndex [ sensorIndex ] ] = sensor - > distance_cm ( ) * 0.01f ;
2016-07-14 02:08:43 -03:00
}
// 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 ;
}
}
}
}
2020-05-08 20:05:40 -03:00
void NavEKF3_core : : writeBodyFrameOdom ( float quality , const Vector3f & delPos , const Vector3f & delAng , float delTime , uint32_t timeStamp_ms , uint16_t delay_ms , const Vector3f & posOffset )
2017-03-16 02:59:19 -03:00
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if ( ( ( timeStamp_ms - bodyOdmMeasTime_ms ) < frontend - > sensorIntervalMin_ms ) | | ( delTime < dtEkfAvg ) | | ! statesInitialised ) {
return ;
}
2020-05-08 20:05:40 -03:00
// subtract delay from timestamp
timeStamp_ms - = delay_ms ;
2017-03-16 02:59:19 -03:00
bodyOdmDataNew . body_offset = & posOffset ;
bodyOdmDataNew . vel = delPos * ( 1.0f / delTime ) ;
bodyOdmDataNew . time_ms = timeStamp_ms ;
bodyOdmDataNew . angRate = delAng * ( 1.0f / delTime ) ;
bodyOdmMeasTime_ms = timeStamp_ms ;
// simple model of accuracy
// TODO move this calculation outside of EKF into the sensor driver
2017-07-27 01:51:17 -03:00
bodyOdmDataNew . velErr = frontend - > _visOdmVelErrMin + ( frontend - > _visOdmVelErrMax - frontend - > _visOdmVelErrMin ) * ( 1.0f - 0.01f * quality ) ;
2017-03-16 02:59:19 -03:00
storedBodyOdm . push ( bodyOdmDataNew ) ;
}
2017-07-27 02:01:48 -03:00
void NavEKF3_core : : writeWheelOdom ( float delAng , float delTime , uint32_t timeStamp_ms , const Vector3f & posOffset , float radius )
{
// This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
// It uses the exisiting body frame velocity fusion.
2019-02-22 19:35:24 -04:00
// TODO implement a dedicated wheel odometry observation model
2017-07-27 02:01:48 -03:00
2019-10-17 08:02:18 -03:00
// rate limiting to 50hz should be done by the caller
2017-07-27 02:01:48 -03:00
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
2019-10-17 08:02:18 -03:00
if ( ( delTime < dtEkfAvg ) | | ! statesInitialised ) {
2017-07-27 02:01:48 -03:00
return ;
}
wheelOdmDataNew . hub_offset = & posOffset ;
wheelOdmDataNew . delAng = delAng ;
wheelOdmDataNew . radius = radius ;
wheelOdmDataNew . delTime = delTime ;
wheelOdmMeasTime_ms = timeStamp_ms ;
2019-02-22 19:35:24 -04:00
// because we are currently converting to an equivalent velocity measurement before fusing
2017-07-27 02:01:48 -03:00
// the measurement time is moved back to the middle of the sampling period
2017-07-27 22:29:49 -03:00
wheelOdmDataNew . time_ms = timeStamp_ms - ( uint32_t ) ( 500.0f * delTime ) ;
2017-07-27 02:01:48 -03:00
storedWheelOdm . push ( wheelOdmDataNew ) ;
}
2016-07-14 02:08:43 -03:00
// write the raw optical flow measurements
// this needs to be called externally.
2018-09-02 08:25:38 -03:00
void NavEKF3_core : : writeOptFlowMeas ( const uint8_t rawFlowQuality , const Vector2f & rawFlowRates , const Vector2f & rawGyroRates , const uint32_t msecFlowMeas , const Vector3f & posOffset )
2016-07-14 02:08:43 -03:00
{
2016-12-17 18:02:38 -04:00
// limit update rate to maximum allowed by sensor buffers
if ( ( imuSampleTime_ms - flowMeaTime_ms ) < frontend - > sensorIntervalMin_ms ) {
return ;
}
2016-07-14 02:08:43 -03:00
// 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 ) ;
}
}
/********************************************************
* MAGNETOMETER *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// check for new magnetometer data and update store measurements if available
void NavEKF3_core : : readMagData ( )
{
if ( ! _ahrs - > get_compass ( ) ) {
allMagSensorsFailed = true ;
return ;
}
2020-03-11 05:05:16 -03:00
const Compass & compass = AP : : compass ( ) ;
2016-07-14 02:08:43 -03:00
// 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
2020-03-11 05:05:16 -03:00
uint8_t maxCount = compass . get_count ( ) ;
2016-07-14 02:08:43 -03:00
if ( allMagSensorsFailed | | ( magTimeout & & assume_zero_sideslip ( ) & & magSelectIndex > = maxCount - 1 & & inFlight ) ) {
allMagSensorsFailed = true ;
return ;
}
2020-03-11 05:05:16 -03:00
if ( compass . learn_offsets_enabled ( ) ) {
2018-10-19 04:26:01 -03:00
// while learning offsets keep all mag states reset
InitialiseVariablesMag ( ) ;
wasLearningCompass_ms = imuSampleTime_ms ;
} else if ( wasLearningCompass_ms ! = 0 & & imuSampleTime_ms - wasLearningCompass_ms > 1000 ) {
wasLearningCompass_ms = 0 ;
// force a new yaw alignment 1s after learning completes. The
// delay is to ensure any buffered mag samples are discarded
yawAlignComplete = false ;
InitialiseVariablesMag ( ) ;
}
2016-12-17 18:02:38 -04:00
// limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
2020-03-11 05:05:16 -03:00
if ( use_compass ( ) & & ( ( compass . last_update_usec ( ) - lastMagUpdate_us ) > 1000 * frontend - > sensorIntervalMin_ms ) ) {
2016-07-14 02:08:43 -03:00
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
2020-03-11 05:05:16 -03:00
if ( compass . use_for_yaw ( tempIndex ) & & tempIndex ! = magSelectIndex ) {
2016-07-14 02:08:43 -03:00
magSelectIndex = tempIndex ;
2017-07-09 01:17:14 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF3 IMU%u switching to compass %u " , ( unsigned ) imu_index , magSelectIndex ) ;
2016-07-14 02:08:43 -03:00
// 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 ;
2019-02-08 18:15:56 -04:00
break ;
2016-07-14 02:08:43 -03:00
}
}
}
// detect changes to magnetometer offset parameters and reset states
2020-03-11 05:05:16 -03:00
Vector3f nowMagOffsets = compass . get_offsets ( magSelectIndex ) ;
2016-07-14 02:08:43 -03:00
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
2020-03-11 05:05:16 -03:00
lastMagUpdate_us = compass . last_update_usec ( magSelectIndex ) ;
2016-07-14 02:08:43 -03:00
// 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
2020-03-11 05:05:16 -03:00
magDataNew . mag = compass . get_field ( magSelectIndex ) * 0.001f ;
2016-07-14 02:08:43 -03:00
// check for consistent data between magnetometers
2020-03-11 05:05:16 -03:00
consistentMagData = compass . consistent ( ) ;
2016-07-14 02:08:43 -03:00
// 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 100 Hz
* for storage in the data buffers used by the EKF . If the IMU data arrives at
* lower rate than 100 Hz , then no downsampling or upsampling will be performed .
* Downsampling is done using a method that does not introduce coning or sculling
* errors .
*/
void NavEKF3_core : : readIMUData ( )
{
2018-03-10 05:36:09 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2016-07-14 02:08:43 -03:00
2017-04-28 03:26:44 -03:00
// calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = 0.02f * constrain_float ( ins . get_loop_delta_t ( ) , 0.5f * dtIMUavg , 2.0f * dtIMUavg ) + 0.98f * dtIMUavg ;
2016-07-14 02:08:43 -03:00
// the imu sample time is used as a common time reference throughout the filter
2016-12-18 20:29:41 -04:00
imuSampleTime_ms = frontend - > imuSampleTime_us / 1000 ;
2016-07-14 02:08:43 -03:00
2019-07-05 05:29:19 -03:00
uint8_t accel_active , gyro_active ;
2016-07-14 02:08:43 -03:00
if ( ins . use_accel ( imu_index ) ) {
2019-07-05 05:29:19 -03:00
accel_active = imu_index ;
2016-07-14 02:08:43 -03:00
} else {
2019-07-05 05:29:19 -03:00
accel_active = ins . get_primary_accel ( ) ;
2016-07-14 02:08:43 -03:00
}
if ( ins . use_gyro ( imu_index ) ) {
2019-07-05 05:29:19 -03:00
gyro_active = imu_index ;
2016-07-14 02:08:43 -03:00
} else {
2019-07-05 05:29:19 -03:00
gyro_active = ins . get_primary_gyro ( ) ;
}
if ( gyro_active ! = gyro_index_active ) {
// we are switching active gyro at runtime. Copy over the
// bias we have learned from the previously inactive
// gyro. We don't re-init the bias uncertainty as it should
// have the same uncertainty as the previously active gyro
stateStruct . gyro_bias = inactiveBias [ gyro_active ] . gyro_bias ;
gyro_index_active = gyro_active ;
2016-07-14 02:08:43 -03:00
}
2019-07-05 05:29:19 -03:00
if ( accel_active ! = accel_index_active ) {
// switch to the learned accel bias for this IMU
stateStruct . accel_bias = inactiveBias [ accel_active ] . accel_bias ;
accel_index_active = accel_active ;
}
// update the inactive bias states
learnInactiveBiases ( ) ;
2020-04-08 07:07:22 -03:00
// run movement check using IMU data
updateMovementCheck ( ) ;
2020-01-22 05:56:57 -04:00
2019-07-05 05:29:19 -03:00
readDeltaVelocity ( accel_index_active , imuDataNew . delVel , imuDataNew . delVelDT ) ;
accelPosOffset = ins . get_imu_pos_offset ( accel_index_active ) ;
imuDataNew . accel_index = accel_index_active ;
// Get delta angle data from primary gyro or primary if not available
readDeltaAngle ( gyro_index_active , imuDataNew . delAng ) ;
imuDataNew . delAngDT = MAX ( ins . get_delta_angle_dt ( gyro_index_active ) , 1.0e-4 f ) ;
imuDataNew . gyro_index = gyro_index_active ;
2016-07-14 02:08:43 -03:00
// 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 ;
2019-07-05 05:29:19 -03:00
// use the most recent IMU index for the downsampled IMU
// data. This isn't strictly correct if we switch IMUs between
// samples
imuDataDownSampledNew . gyro_index = imuDataNew . gyro_index ;
imuDataDownSampledNew . accel_index = imuDataNew . accel_index ;
2016-07-14 02:08:43 -03:00
// 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 + + ;
2017-04-28 03:36:16 -03:00
/*
* 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
2017-05-01 00:50:44 -03:00
* than twice the target time has lapsed . Adjust the target EKF step time threshold to allow for timing jitter in the
* IMU data .
2017-04-28 03:36:16 -03:00
*/
2017-05-20 23:15:06 -03:00
if ( ( imuDataDownSampledNew . delAngDT > = ( EKF_TARGET_DT - ( dtIMUavg * 0.5f ) ) & & startPredictEnabled ) | |
2017-04-28 21:48:49 -03:00
( imuDataDownSampledNew . delAngDT > = 2.0f * EKF_TARGET_DT ) ) {
2016-07-14 02:08:43 -03:00
// 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 ) ;
2017-04-28 03:36:16 -03:00
// calculate the achieved average time step rate for the EKF using a combination spike and LPF
float dtNow = constrain_float ( 0.5f * ( imuDataDownSampledNew . delAngDT + imuDataDownSampledNew . delVelDT ) , 0.5f * dtEkfAvg , 2.0f * dtEkfAvg ) ;
2016-07-14 02:08:43 -03:00
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 ;
2017-05-01 07:15:42 -03:00
// set the flag to let the filter know it has new IMU data and needs to run
2016-07-14 02:08:43 -03:00
runUpdates = true ;
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU . pop_oldest_element ( ) ;
// protect against delta time going to zero
2017-04-28 03:36:16 -03:00
float minDT = 0.1f * dtEkfAvg ;
2016-07-14 02:08:43 -03:00
imuDataDelayed . delAngDT = MAX ( imuDataDelayed . delAngDT , minDT ) ;
imuDataDelayed . delVelDT = MAX ( imuDataDelayed . delVelDT , minDT ) ;
2017-04-27 22:33:41 -03:00
updateTimingStatistics ( ) ;
2016-07-14 02:08:43 -03:00
// correct the extracted IMU data for sensor errors
delAngCorrected = imuDataDelayed . delAng ;
delVelCorrected = imuDataDelayed . delVel ;
2019-07-05 05:29:19 -03:00
correctDeltaAngle ( delAngCorrected , imuDataDelayed . delAngDT , imuDataDelayed . gyro_index ) ;
correctDeltaVelocity ( delVelCorrected , imuDataDelayed . delVelDT , imuDataDelayed . accel_index ) ;
2016-07-14 02:08:43 -03:00
} 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 NavEKF3_core : : readDeltaVelocity ( uint8_t ins_index , Vector3f & dVel , float & dVel_dt ) {
2018-03-10 05:36:09 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2016-07-14 02:08:43 -03:00
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-4 f ) ;
return true ;
}
return false ;
}
/********************************************************
* Global Position Measurement *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// check for new valid GPS data and update stored measurement if available
void NavEKF3_core : : readGpsData ( )
{
// check for new GPS data
2016-12-17 18:02:38 -04:00
// limit update rate to avoid overflowing the FIFO buffer
2017-12-01 21:02:55 -04:00
const AP_GPS & gps = AP : : gps ( ) ;
if ( gps . last_message_time_ms ( ) - lastTimeGpsReceived_ms > frontend - > sensorIntervalMin_ms ) {
if ( gps . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
2016-07-14 02:08:43 -03:00
// report GPS fix status
gpsCheckStatus . bad_fix = false ;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms ;
// get current fix time
2017-12-01 21:02:55 -04:00
lastTimeGpsReceived_ms = gps . last_message_time_ms ( ) ;
2016-07-14 02:08:43 -03:00
// 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
2016-12-18 20:43:11 -04:00
// Use the driver specified delay
2017-05-21 22:08:52 -03:00
float gps_delay_sec = 0 ;
2017-12-01 21:02:55 -04:00
gps . get_lag ( gps_delay_sec ) ;
2017-05-21 22:08:52 -03:00
gpsDataNew . time_ms = lastTimeGpsReceived_ms - ( uint32_t ) ( gps_delay_sec * 1000.0f ) ;
2016-07-14 02:08:43 -03:00
// Correct for the average intersampling delay due to the filter updaterate
gpsDataNew . time_ms - = localFilterTimeStep_ms / 2 ;
2016-12-19 03:21:32 -04:00
// Prevent the time stamp falling outside the oldest and newest IMU data in the buffer
gpsDataNew . time_ms = MIN ( MAX ( gpsDataNew . time_ms , imuDataDelayed . time_ms ) , imuDataDownSampledNew . time_ms ) ;
2016-07-14 02:08:43 -03:00
// Get which GPS we are using for position information
2017-12-01 21:02:55 -04:00
gpsDataNew . sensor_idx = gps . primary_sensor ( ) ;
2016-07-14 02:08:43 -03:00
// read the NED velocity from the GPS
2017-12-01 21:02:55 -04:00
gpsDataNew . vel = gps . velocity ( ) ;
2016-07-14 02:08:43 -03:00
// 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 ;
2017-12-01 21:02:55 -04:00
if ( ! gps . speed_accuracy ( gpsSpdAccRaw ) ) {
2016-07-14 02:08:43 -03:00
gpsSpdAccuracy = 0.0f ;
} else {
gpsSpdAccuracy = MAX ( gpsSpdAccuracy , gpsSpdAccRaw ) ;
gpsSpdAccuracy = MIN ( gpsSpdAccuracy , 50.0f ) ;
}
gpsPosAccuracy * = ( 1.0f - alpha ) ;
float gpsPosAccRaw ;
2017-12-01 21:02:55 -04:00
if ( ! gps . horizontal_accuracy ( gpsPosAccRaw ) ) {
2016-07-14 02:08:43 -03:00
gpsPosAccuracy = 0.0f ;
} else {
gpsPosAccuracy = MAX ( gpsPosAccuracy , gpsPosAccRaw ) ;
gpsPosAccuracy = MIN ( gpsPosAccuracy , 100.0f ) ;
}
gpsHgtAccuracy * = ( 1.0f - alpha ) ;
float gpsHgtAccRaw ;
2017-12-01 21:02:55 -04:00
if ( ! gps . vertical_accuracy ( gpsHgtAccRaw ) ) {
2016-07-14 02:08:43 -03:00
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
2017-12-01 21:02:55 -04:00
if ( gps . num_sats ( ) > = 6 & & ( PV_AidingMode = = AID_ABSOLUTE ) ) {
2016-07-14 02:08:43 -03:00
gpsNoiseScaler = 1.0f ;
2017-12-01 21:02:55 -04:00
} else if ( gps . num_sats ( ) = = 5 & & ( PV_AidingMode = = AID_ABSOLUTE ) ) {
2016-07-14 02:08:43 -03:00
gpsNoiseScaler = 1.4f ;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f ;
}
2017-07-24 22:41:51 -03:00
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
2017-12-01 21:02:55 -04:00
if ( gps . have_vertical_velocity ( ) & & ( frontend - > _fusionModeGPS = = 0 ) & & ! frontend - > inhibitGpsVertVelUse ) {
2016-07-14 02:08:43 -03:00
useGpsVertVel = true ;
} else {
useGpsVertVel = false ;
}
2019-07-02 00:42:52 -03:00
// Monitor quality of the GPS velocity data before and after alignment
calcGpsGoodToAlign ( ) ;
2016-07-14 02:08:43 -03:00
// Post-alignment checks
calcGpsGoodForFlight ( ) ;
2019-07-27 04:00:10 -03:00
// see if we can get an origin from the frontend
if ( ! validOrigin & & frontend - > common_origin_valid ) {
setOrigin ( frontend - > common_EKF_origin ) ;
}
2017-05-01 07:15:42 -03:00
// Read the GPS location in WGS-84 lat,long,height coordinates
2017-12-01 21:02:55 -04:00
const struct Location & gpsloc = gps . location ( ) ;
2016-07-14 02:08:43 -03:00
2019-02-22 19:35:24 -04:00
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
2016-07-14 02:08:43 -03:00
if ( gpsGoodToAlign & & ! validOrigin ) {
2019-07-27 04:00:10 -03:00
setOrigin ( gpsloc ) ;
2016-07-14 02:08:43 -03:00
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination ( ) ;
2017-05-09 03:23:58 -03:00
// Set the height of the NED origin
ekfGpsRefHgt = ( double ) 0.01 * ( double ) gpsloc . alt + ( double ) outputDataNew . position . z ;
2016-07-14 02:08:43 -03:00
2017-05-01 07:15:42 -03:00
// Set the uncertainty of the GPS origin height
2016-07-14 02:08:43 -03:00
ekfOriginHgtVar = sq ( gpsHgtAccuracy ) ;
}
2019-09-22 09:28:19 -03:00
if ( gpsGoodToAlign & & ! have_table_earth_field ) {
2019-11-26 18:13:14 -04:00
const Compass * compass = _ahrs - > get_compass ( ) ;
2020-04-25 19:12:10 -03:00
if ( compass & & compass - > have_scale_factor ( magSelectIndex ) & & compass - > auto_declination_enabled ( ) ) {
2019-11-26 18:13:14 -04:00
table_earth_field_ga = AP_Declination : : get_earth_field_ga ( gpsloc ) ;
table_declination = radians ( AP_Declination : : get_declination ( gpsloc . lat * 1.0e-7 ,
2019-09-22 09:28:19 -03:00
gpsloc . lng * 1.0e-7 ) ) ;
2019-11-26 18:13:14 -04:00
have_table_earth_field = true ;
if ( frontend - > _mag_ef_limit > 0 ) {
// initialise earth field from tables
stateStruct . earth_magfield = table_earth_field_ga ;
}
2019-09-22 09:28:19 -03:00
}
}
2016-07-14 02:08:43 -03:00
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if ( validOrigin ) {
2019-04-08 10:16:20 -03:00
gpsDataNew . pos = EKF_origin . get_distance_NE ( gpsloc ) ;
2019-07-09 03:27:48 -03:00
if ( ( frontend - > _originHgtMode & ( 1 < < 2 ) ) = = 0 ) {
gpsDataNew . hgt = ( float ) ( ( double ) 0.01 * ( double ) gpsloc . alt - ekfGpsRefHgt ) ;
} else {
gpsDataNew . hgt = 0.01 * ( gpsloc . alt - EKF_origin . alt ) ;
}
2016-07-14 02:08:43 -03:00
storedGPS . push ( gpsDataNew ) ;
// declare GPS available for use
gpsNotAvailable = false ;
}
2017-11-10 23:29:48 -04:00
// if the GPS has yaw data then input that as well
float yaw_deg , yaw_accuracy_deg ;
if ( AP : : gps ( ) . gps_yaw_deg ( yaw_deg , yaw_accuracy_deg ) ) {
2019-09-08 21:32:00 -03:00
// GPS modules are rather too optimistic about their
// accuracy. Set to min of 5 degrees here to prevent
// the user constantly receiving warnings about high
// normalised yaw innovations
const float min_yaw_accuracy_deg = 5.0f ;
yaw_accuracy_deg = MAX ( yaw_accuracy_deg , min_yaw_accuracy_deg ) ;
2017-11-10 23:29:48 -04:00
writeEulerYawAngle ( radians ( yaw_deg ) , radians ( yaw_accuracy_deg ) , gpsDataNew . time_ms , 2 ) ;
}
2016-07-14 02:08:43 -03:00
} else {
// report GPS fix status
gpsCheckStatus . bad_fix = true ;
2019-09-09 04:57:37 -03:00
hal . util - > snprintf ( prearm_fail_string , sizeof ( prearm_fail_string ) , " Waiting for 3D fix " ) ;
2016-07-14 02:08:43 -03:00
}
}
}
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core : : readDeltaAngle ( uint8_t ins_index , Vector3f & dAng ) {
2018-03-10 05:36:09 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2016-07-14 02:08:43 -03:00
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 NavEKF3_core : : readBaroData ( )
{
// check to see if baro measurement has changed so we know if a new measurement has arrived
2016-12-17 18:02:38 -04:00
// limit update rate to avoid overflowing the FIFO buffer
2018-03-05 16:35:51 -04:00
const AP_Baro & baro = AP : : baro ( ) ;
if ( baro . get_last_update ( ) - lastBaroReceived_ms > frontend - > sensorIntervalMin_ms ) {
2016-07-14 02:08:43 -03:00
frontend - > logging . log_baro = true ;
2018-03-05 16:35:51 -04:00
baroDataNew . hgt = baro . get_altitude ( ) ;
2016-07-14 02:08:43 -03:00
// 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
2018-03-05 16:35:51 -04:00
lastBaroReceived_ms = baro . get_last_update ( ) ;
2016-07-14 02:08:43 -03:00
// 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 NavEKF3_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 ) ;
}
2017-05-09 03:23:58 -03:00
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void NavEKF3_core : : correctEkfOriginHeight ( )
2016-07-14 02:08:43 -03:00
{
// 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 ) ;
2017-05-09 03:23:58 -03:00
} else {
// by definition our height source is absolute so cannot run this filter
2016-07-14 02:08:43 -03:00
return ;
}
lastOriginHgtTime_ms = imuDataDelayed . time_ms ;
2017-05-01 07:15:42 -03:00
// calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
2016-07-14 02:08:43 -03:00
// when not using GPS as height source
2018-02-20 02:02:29 -04:00
float originHgtObsVar = sq ( gpsHgtAccuracy ) + P [ 9 ] [ 9 ] ;
2016-07-14 02:08:43 -03:00
// 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 ) ;
2017-05-09 03:23:58 -03:00
// correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if ( ratio < 25.0f & & gpsAccuracyGood ) {
ekfGpsRefHgt - = ( double ) ( gain * innovation ) ;
2016-12-19 19:23:23 -04:00
ekfOriginHgtVar - = MAX ( gain * ekfOriginHgtVar , 0.0f ) ;
2016-07-14 02:08:43 -03:00
}
}
/********************************************************
* Air Speed Measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// check for new airspeed data and update stored measurements if available
void NavEKF3_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 ( ) & &
2016-12-17 18:02:38 -04:00
( aspeed - > last_update_ms ( ) - timeTasReceived_ms ) > frontend - > sensorIntervalMin_ms ) {
2019-06-04 00:37:42 -03:00
tasDataNew . tas = aspeed - > get_raw_airspeed ( ) * AP : : ahrs ( ) . get_EAS2TAS ( ) ;
2016-07-14 02:08:43 -03:00
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 *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2017-05-01 07:15:42 -03:00
// check for new range beacon data and push to data buffer if available
2016-07-14 02:08:43 -03:00
void NavEKF3_core : : readRngBcnData ( )
{
// get the location of the beacon data
2019-06-26 23:00:02 -03:00
const AP_Beacon * beacon = AP : : beacon ( ) ;
2016-07-14 02:08:43 -03:00
// 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
2018-02-09 13:55:18 -04:00
const float posDiffSq = sq ( receiverPos . x - beaconVehiclePosNED . x ) + sq ( receiverPos . y - beaconVehiclePosNED . y ) ;
const float posDiffVar = sq ( beaconVehiclePosErr ) + receiverPosCov [ 0 ] [ 0 ] + receiverPosCov [ 1 ] [ 1 ] ;
if ( posDiffSq < 9.0f * posDiffVar ) {
2016-07-14 02:08:43 -03:00
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 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
2018-02-09 13:55:18 -04:00
rngBcnDataToFuse = storedRangeBeacon . recall ( rngBcnDataDelayed , imuDataDelayed . time_ms ) ;
2016-07-14 02:08:43 -03:00
2016-12-16 21:22:07 -04:00
// Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
if ( rngBcnDataToFuse ) {
2017-06-20 19:56:41 -03:00
rngBcnDataDelayed . beacon_posNED . x + = bcnPosOffsetNED . x ;
rngBcnDataDelayed . beacon_posNED . y + = bcnPosOffsetNED . y ;
2016-12-16 21:22:07 -04:00
}
2016-07-14 02:08:43 -03:00
}
2017-05-28 08:25:07 -03:00
/********************************************************
* Independant yaw sensor measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void NavEKF3_core : : writeEulerYawAngle ( float yawAngle , float yawAngleErr , uint32_t timeStamp_ms , uint8_t type )
{
2017-07-19 19:52:58 -03:00
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if ( ( ( timeStamp_ms - yawMeasTime_ms ) < frontend - > sensorIntervalMin_ms ) | | ! statesInitialised ) {
return ;
}
yawAngDataNew . yawAng = yawAngle ;
yawAngDataNew . yawAngErr = yawAngleErr ;
yawAngDataNew . type = type ;
yawAngDataNew . time_ms = timeStamp_ms ;
storedYawAng . push ( yawAngDataNew ) ;
yawMeasTime_ms = timeStamp_ms ;
2017-05-28 08:25:07 -03:00
}
2020-03-10 03:48:08 -03:00
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF3_core : : writeDefaultAirSpeed ( float airspeed )
{
defaultAirSpeed = airspeed ;
}
2017-05-28 08:25:07 -03:00
2020-04-14 21:56:28 -03:00
/********************************************************
* External Navigation Measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2020-05-08 19:46:18 -03:00
void NavEKF3_core : : writeExtNavData ( const Vector3f & pos , const Quaternion & quat , float posErr , float angErr , uint32_t timeStamp_ms , uint16_t delay_ms , uint32_t resetTime_ms )
2020-04-14 21:56:28 -03:00
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if ( ( ( timeStamp_ms - extNavMeasTime_ms ) < 70 ) | | ! statesInitialised ) {
return ;
} else {
extNavMeasTime_ms = timeStamp_ms ;
}
2020-05-03 08:33:29 -03:00
ext_nav_elements extNavDataNew { } ;
2020-04-14 21:56:28 -03:00
if ( resetTime_ms ! = extNavLastPosResetTime_ms ) {
extNavDataNew . posReset = true ;
extNavLastPosResetTime_ms = resetTime_ms ;
} else {
extNavDataNew . posReset = false ;
}
extNavDataNew . pos = pos ;
if ( posErr > 0 ) {
extNavDataNew . posErr = posErr ;
} else {
extNavDataNew . posErr = frontend - > _gpsHorizPosNoise ;
}
// calculate timestamp
2020-05-08 19:46:18 -03:00
timeStamp_ms = timeStamp_ms - delay_ms ;
2020-04-14 21:56:28 -03:00
// Correct for the average intersampling delay due to the filter update rate
timeStamp_ms - = localFilterTimeStep_ms / 2 ;
// Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX ( timeStamp_ms , imuDataDelayed . time_ms ) ;
extNavDataNew . time_ms = timeStamp_ms ;
// extract yaw from the attitude
float roll_rad , pitch_rad , yaw_rad ;
quat . to_euler ( roll_rad , pitch_rad , yaw_rad ) ;
2020-05-13 05:54:38 -03:00
// ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
const float yaw_accuracy_rad = MAX ( angErr , radians ( 5.0f ) ) ;
writeEulerYawAngle ( yaw_rad , yaw_accuracy_rad , timeStamp_ms , 2 ) ;
2020-04-14 21:56:28 -03:00
storedExtNav . push ( extNavDataNew ) ;
}
2020-05-18 02:22:20 -03:00
void NavEKF3_core : : writeExtNavVelData ( const Vector3f & vel , float err , uint32_t timeStamp_ms , uint16_t delay_ms )
{
if ( ( timeStamp_ms - extNavVelMeasTime_ms ) < 70 ) {
return ;
}
extNavVelMeasTime_ms = timeStamp_ms - delay_ms ;
useExtNavVel = true ;
extNavVelNew . vel = vel ;
extNavVelNew . err = err ;
// Correct for the average intersampling delay due to the filter updaterate
timeStamp_ms - = localFilterTimeStep_ms / 2 ;
// Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX ( timeStamp_ms , imuDataDelayed . time_ms ) ;
extNavVelNew . time_ms = timeStamp_ms ;
storedExtNavVel . push ( extNavVelNew ) ;
}
2017-04-27 22:33:41 -03:00
/*
update timing statistics structure
*/
void NavEKF3_core : : updateTimingStatistics ( void )
{
if ( timing . count = = 0 ) {
timing . dtIMUavg_max = dtIMUavg ;
timing . dtIMUavg_min = dtIMUavg ;
2017-04-28 04:14:34 -03:00
timing . dtEKFavg_max = dtEkfAvg ;
timing . dtEKFavg_min = dtEkfAvg ;
2017-04-27 22:33:41 -03:00
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 ) ;
2017-04-28 04:14:34 -03:00
timing . dtEKFavg_max = MAX ( timing . dtEKFavg_max , dtEkfAvg ) ;
timing . dtEKFavg_min = MIN ( timing . dtEKFavg_min , dtEkfAvg ) ;
2017-04-27 22:33:41 -03:00
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 NavEKF3_core : : getTimingStatistics ( struct ekf_timing & _timing )
{
_timing = timing ;
memset ( & timing , 0 , sizeof ( timing ) ) ;
}
2019-07-05 05:29:19 -03:00
/*
update estimates of inactive bias states . This keeps inactive IMUs
as hot - spares so we can switch to them without causing a jump in the
error
*/
void NavEKF3_core : : learnInactiveBiases ( void )
{
const AP_InertialSensor & ins = AP : : ins ( ) ;
// learn gyro biases
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES ; i + + ) {
if ( ! ins . use_gyro ( i ) ) {
// can't use this gyro
continue ;
}
if ( gyro_index_active = = i ) {
// use current estimates from main filter of gyro bias
inactiveBias [ i ] . gyro_bias = stateStruct . gyro_bias ;
} else {
// get filtered gyro and use the difference between the
// corrected gyro on the active IMU and the inactive IMU
// to move the inactive bias towards the right value
Vector3f filtered_gyro_active = ins . get_gyro ( gyro_index_active ) - ( stateStruct . gyro_bias / dtEkfAvg ) ;
Vector3f filtered_gyro_inactive = ins . get_gyro ( i ) - ( inactiveBias [ i ] . gyro_bias / dtEkfAvg ) ;
Vector3f error = filtered_gyro_active - filtered_gyro_inactive ;
// prevent a single large error from contaminating bias estimate
const float bias_limit = radians ( 5 ) ;
error . x = constrain_float ( error . x , - bias_limit , bias_limit ) ;
error . y = constrain_float ( error . y , - bias_limit , bias_limit ) ;
error . z = constrain_float ( error . z , - bias_limit , bias_limit ) ;
// slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
// gyro bias error in around 1 minute
inactiveBias [ i ] . gyro_bias - = error * ( 1.0e-4 f * dtEkfAvg ) ;
}
}
// learn accel biases
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES ; i + + ) {
if ( ! ins . use_accel ( i ) ) {
// can't use this accel
continue ;
}
if ( accel_index_active = = i ) {
// use current estimates from main filter of accel bias
inactiveBias [ i ] . accel_bias = stateStruct . accel_bias ;
} else {
// get filtered accel and use the difference between the
// corrected accel on the active IMU and the inactive IMU
// to move the inactive bias towards the right value
Vector3f filtered_accel_active = ins . get_accel ( accel_index_active ) - ( stateStruct . accel_bias / dtEkfAvg ) ;
Vector3f filtered_accel_inactive = ins . get_accel ( i ) - ( inactiveBias [ i ] . accel_bias / dtEkfAvg ) ;
Vector3f error = filtered_accel_active - filtered_accel_inactive ;
// prevent a single large error from contaminating bias estimate
const float bias_limit = 1.0 ; // m/s/s
error . x = constrain_float ( error . x , - bias_limit , bias_limit ) ;
error . y = constrain_float ( error . y , - bias_limit , bias_limit ) ;
error . z = constrain_float ( error . z , - bias_limit , bias_limit ) ;
// slowly bring the inactive accel in line with the active
// accel. This corrects a 0.5 m/s/s accel bias error in
// around 1 minute
inactiveBias [ i ] . accel_bias - = error * ( 1.0e-4 f * dtEkfAvg ) ;
}
}
}
2019-09-22 09:28:19 -03:00
/*
return declination in radians
*/
float NavEKF3_core : : MagDeclination ( void ) const
{
// if we are using the WMM tables then use the table declination
// to ensure consistency with the table mag field. Otherwise use
// the declination from the compass library
if ( have_table_earth_field & & frontend - > _mag_ef_limit > 0 ) {
return table_declination ;
}
if ( ! use_compass ( ) ) {
return 0 ;
}
return _ahrs - > get_compass ( ) - > get_declination ( ) ;
}
2020-01-22 05:56:57 -04:00
/*
2020-04-08 07:07:22 -03:00
Update the on ground and not moving check .
Should be called once per IMU update .
2020-04-10 21:43:46 -03:00
Only updates when on ground and when operating without a magnetometer
2020-04-08 07:07:22 -03:00
*/
void NavEKF3_core : : updateMovementCheck ( void )
2020-01-22 05:56:57 -04:00
{
2020-04-10 21:43:46 -03:00
if ( ! onGround & &
effectiveMagCal ! = MagCal : : EXTERNAL_YAW & &
effectiveMagCal ! = MagCal : : EXTERNAL_YAW_FALLBACK & &
effectiveMagCal ! = MagCal : : GSF_YAW )
{
2020-04-08 07:07:22 -03:00
onGroundNotMoving = false ;
return ;
}
const float gyro_limit = radians ( 3.0f ) ;
2020-04-08 20:32:46 -03:00
const float gyro_diff_limit = 0.1 ;
2020-04-08 07:07:22 -03:00
const float accel_limit = 1.0f ;
2020-04-08 20:32:46 -03:00
const float accel_diff_limit = 5.0f ;
2020-04-08 07:07:22 -03:00
const float hysteresis_ratio = 0.7f ;
const float dtEkfAvgInv = 1.0f / dtEkfAvg ;
2020-04-08 20:32:46 -03:00
// get latest bias corrected gyro and accelerometer data
2020-01-22 05:56:57 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2020-04-08 07:07:22 -03:00
Vector3f gyro = ins . get_gyro ( gyro_index_active ) - stateStruct . gyro_bias * dtEkfAvgInv ;
Vector3f accel = ins . get_accel ( accel_index_active ) - stateStruct . accel_bias * dtEkfAvgInv ;
if ( ! prevOnGround ) {
gyro_prev = gyro ;
accel_prev = accel ;
onGroundNotMoving = false ;
gyro_diff = gyro_diff_limit ;
accel_diff = accel_diff_limit ;
2020-01-22 05:56:57 -04:00
return ;
}
2020-04-08 07:07:22 -03:00
2020-04-10 22:44:24 -03:00
// calculate a gyro rate of change metric
Vector3f temp = ( gyro - gyro_prev ) * dtEkfAvgInv ;
2020-04-08 07:07:22 -03:00
gyro_prev = gyro ;
2020-04-10 22:44:24 -03:00
gyro_diff = 0.99f * gyro_diff + 0.01f * temp . length ( ) ;
2020-04-08 07:07:22 -03:00
2020-04-10 22:44:24 -03:00
// calculate a acceleration rate of change metric
temp = ( accel - accel_prev ) * dtEkfAvgInv ;
2020-04-08 07:07:22 -03:00
accel_prev = accel ;
2020-04-10 22:44:24 -03:00
accel_diff = 0.99f * accel_diff + 0.01f * temp . length ( ) ;
2020-04-08 07:07:22 -03:00
2020-04-08 20:32:46 -03:00
const float gyro_length_ratio = gyro . length ( ) / gyro_limit ;
const float accel_length_ratio = ( accel . length ( ) - GRAVITY_MSS ) / accel_limit ;
const float gyro_diff_ratio = gyro_diff / gyro_diff_limit ;
const float accel_diff_ratio = accel_diff / accel_diff_limit ;
bool logStatusChange = false ;
2020-04-08 07:07:22 -03:00
if ( onGroundNotMoving ) {
2020-04-08 20:32:46 -03:00
if ( gyro_length_ratio > 1.0f | |
fabsf ( accel_length_ratio ) > 1.0f | |
gyro_diff_ratio > 1.0f | |
accel_diff_ratio > 1.0f )
{
onGroundNotMoving = false ;
logStatusChange = true ;
}
} else if ( gyro_length_ratio < hysteresis_ratio & &
fabsf ( accel_length_ratio ) < hysteresis_ratio & &
gyro_diff_ratio < hysteresis_ratio & &
accel_diff_ratio < hysteresis_ratio )
{
2020-04-08 07:07:22 -03:00
onGroundNotMoving = true ;
2020-04-08 20:32:46 -03:00
logStatusChange = true ;
2020-04-08 07:07:22 -03:00
}
2020-04-08 20:32:46 -03:00
if ( logStatusChange | | imuSampleTime_ms - lastMoveCheckLogTime_ms > 200 ) {
2020-04-08 07:07:22 -03:00
lastMoveCheckLogTime_ms = imuSampleTime_ms ;
2020-04-15 03:00:43 -03:00
// @LoggerMessage: XKFM
// @Description: EKF3 diagnostic data for on-ground-and-not-moving check
// @Field: TimeUS: Time since system startup
// @Field: OGNM: True of on ground and not moving
// @Field: GLR: Gyroscope length ratio
// @Field: ALR: Accelerometer length ratio
// @Field: GDR: Gyroscope rate of change ratio
// @Field: ADR: Accelerometer rate of change ratio
2020-04-08 07:07:22 -03:00
AP : : logger ( ) . Write ( " XKFM " ,
2020-04-08 20:32:46 -03:00
" TimeUS,OGNM,GLR,ALR,GDR,ADR " ,
2020-04-08 07:07:22 -03:00
" s----- " ,
" F----- " ,
" QBffff " ,
AP_HAL : : micros64 ( ) ,
uint8_t ( onGroundNotMoving ) ,
2020-04-08 20:32:46 -03:00
float ( gyro_length_ratio ) ,
float ( accel_length_ratio ) ,
float ( gyro_diff_ratio ) ,
float ( accel_diff_ratio ) ) ;
2020-04-08 07:07:22 -03:00
}
2020-01-22 05:56:57 -04:00
}