mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: initial import of new maths EKF
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
583c1fc229
commit
f500474a86
|
|
@ -0,0 +1,384 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||||
|
||||
#include "AP_NavEKF2_core.h"
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
/*
|
||||
parameter defaults for different types of vehicle. The
|
||||
APM_BUILD_DIRECTORY is taken from the main vehicle directory name
|
||||
where the code is built. Note that this trick won't work for arduino
|
||||
builds on APM2, but NavEKF2 doesn't run on APM2, so that's OK
|
||||
*/
|
||||
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
||||
// copter defaults
|
||||
#define VELNE_NOISE_DEFAULT 0.5f
|
||||
#define VELD_NOISE_DEFAULT 0.7f
|
||||
#define POSNE_NOISE_DEFAULT 0.5f
|
||||
#define ALT_NOISE_DEFAULT 1.0f
|
||||
#define MAG_NOISE_DEFAULT 0.05f
|
||||
#define GYRO_PNOISE_DEFAULT 0.01f
|
||||
#define ACC_PNOISE_DEFAULT 0.25f
|
||||
#define GBIAS_PNOISE_DEFAULT 1E-05f
|
||||
#define ABIAS_PNOISE_DEFAULT 0.00005f
|
||||
#define MAG_PNOISE_DEFAULT 0.0003f
|
||||
#define VEL_GATE_DEFAULT 5
|
||||
#define POS_GATE_DEFAULT 5
|
||||
#define HGT_GATE_DEFAULT 10
|
||||
#define MAG_GATE_DEFAULT 3
|
||||
#define MAG_CAL_DEFAULT 3
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 10
|
||||
#define FLOW_NOISE_DEFAULT 0.25f
|
||||
#define FLOW_GATE_DEFAULT 3
|
||||
|
||||
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
|
||||
// rover defaults
|
||||
#define VELNE_NOISE_DEFAULT 0.5f
|
||||
#define VELD_NOISE_DEFAULT 0.7f
|
||||
#define POSNE_NOISE_DEFAULT 0.5f
|
||||
#define ALT_NOISE_DEFAULT 1.0f
|
||||
#define MAG_NOISE_DEFAULT 0.05f
|
||||
#define GYRO_PNOISE_DEFAULT 0.01f
|
||||
#define ACC_PNOISE_DEFAULT 0.25f
|
||||
#define GBIAS_PNOISE_DEFAULT 8E-06f
|
||||
#define ABIAS_PNOISE_DEFAULT 0.00005f
|
||||
#define MAG_PNOISE_DEFAULT 0.0003f
|
||||
#define VEL_GATE_DEFAULT 5
|
||||
#define POS_GATE_DEFAULT 5
|
||||
#define HGT_GATE_DEFAULT 10
|
||||
#define MAG_GATE_DEFAULT 3
|
||||
#define MAG_CAL_DEFAULT 2
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 25
|
||||
#define FLOW_NOISE_DEFAULT 0.15f
|
||||
#define FLOW_GATE_DEFAULT 5
|
||||
|
||||
#else
|
||||
// generic defaults (and for plane)
|
||||
#define VELNE_NOISE_DEFAULT 0.5f
|
||||
#define VELD_NOISE_DEFAULT 0.7f
|
||||
#define POSNE_NOISE_DEFAULT 0.5f
|
||||
#define ALT_NOISE_DEFAULT 0.5f
|
||||
#define MAG_NOISE_DEFAULT 0.05f
|
||||
#define GYRO_PNOISE_DEFAULT 0.015f
|
||||
#define ACC_PNOISE_DEFAULT 0.5f
|
||||
#define GBIAS_PNOISE_DEFAULT 8E-06f
|
||||
#define ABIAS_PNOISE_DEFAULT 0.00005f
|
||||
#define MAG_PNOISE_DEFAULT 0.0003f
|
||||
#define VEL_GATE_DEFAULT 6
|
||||
#define POS_GATE_DEFAULT 30
|
||||
#define HGT_GATE_DEFAULT 20
|
||||
#define MAG_GATE_DEFAULT 3
|
||||
#define MAG_CAL_DEFAULT 0
|
||||
#define GLITCH_RADIUS_DEFAULT 25
|
||||
#define FLOW_MEAS_DELAY 25
|
||||
#define FLOW_NOISE_DEFAULT 0.3f
|
||||
#define FLOW_GATE_DEFAULT 3
|
||||
|
||||
#endif // APM_BUILD_DIRECTORY
|
||||
|
||||
// Define tuning parameters
|
||||
const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
|
||||
|
||||
// @Param: VELNE_NOISE
|
||||
// @DisplayName: GPS horizontal velocity measurement noise scaler
|
||||
// @Description: This is the scaler that is applied to the speed accuracy reported by the receiver to estimate the horizontal velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then a speed acuracy of 1 is assumed. Increasing it reduces the weighting on these measurements.
|
||||
// @Range: 0.05 5.0
|
||||
// @Increment: 0.05
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("VELNE_NOISE", 0, NavEKF2, _gpsHorizVelNoise, VELNE_NOISE_DEFAULT),
|
||||
|
||||
// @Param: VELD_NOISE
|
||||
// @DisplayName: GPS vertical velocity measurement noise scaler
|
||||
// @Description: This is the scaler that is applied to the speed accuracy reported by the receiver to estimate the vertical velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then a speed acuracy of 1 is assumed. Increasing it reduces the weighting on this measurement.
|
||||
// @Range: 0.05 5.0
|
||||
// @Increment: 0.05
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("VELD_NOISE", 1, NavEKF2, _gpsVertVelNoise, VELD_NOISE_DEFAULT),
|
||||
|
||||
// @Param: POSNE_NOISE
|
||||
// @DisplayName: GPS horizontal position measurement noise (m)
|
||||
// @Description: This is the RMS value of noise in the GPS horizontal position measurements. Increasing it reduces the weighting on these measurements.
|
||||
// @Range: 0.1 10.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
// @Units: meters
|
||||
AP_GROUPINFO("POSNE_NOISE", 2, NavEKF2, _gpsHorizPosNoise, POSNE_NOISE_DEFAULT),
|
||||
|
||||
// @Param: ALT_NOISE
|
||||
// @DisplayName: Altitude measurement noise (m)
|
||||
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting on this measurement.
|
||||
// @Range: 0.1 10.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
// @Units: meters
|
||||
AP_GROUPINFO("ALT_NOISE", 3, NavEKF2, _baroAltNoise, ALT_NOISE_DEFAULT),
|
||||
|
||||
// @Param: MAG_NOISE
|
||||
// @DisplayName: Magnetometer measurement noise (Gauss)
|
||||
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
|
||||
// @Range: 0.01 0.5
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MAG_NOISE", 4, NavEKF2, _magNoise, MAG_NOISE_DEFAULT),
|
||||
|
||||
// @Param: EAS_NOISE
|
||||
// @DisplayName: Equivalent airspeed measurement noise (m/s)
|
||||
// @Description: This is the RMS value of noise in equivalent airspeed measurements. Increasing it reduces the weighting on these measurements.
|
||||
// @Range: 0.5 5.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
// @Units: m/s
|
||||
AP_GROUPINFO("EAS_NOISE", 5, NavEKF2, _easNoise, 1.4f),
|
||||
|
||||
// @Param: WIND_PNOISE
|
||||
// @DisplayName: Wind velocity process noise (m/s^2)
|
||||
// @Description: This noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
|
||||
// @Range: 0.01 1.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("WIND_PNOISE", 6, NavEKF2, _windVelProcessNoise, 0.1f),
|
||||
|
||||
// @Param: WIND_PSCALE
|
||||
// @DisplayName: Height rate to wind procss noise scaler
|
||||
// @Description: Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind speed estimation noiser.
|
||||
// @Range: 0.0 1.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("WIND_PSCALE", 7, NavEKF2, _wndVarHgtRateScale, 0.5f),
|
||||
|
||||
// @Param: GYRO_PNOISE
|
||||
// @DisplayName: Rate gyro noise (rad/s)
|
||||
// @Description: This noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
|
||||
// @Range: 0.001 0.05
|
||||
// @Increment: 0.001
|
||||
// @User: Advanced
|
||||
// @Units: rad/s
|
||||
AP_GROUPINFO("GYRO_PNOISE", 8, NavEKF2, _gyrNoise, GYRO_PNOISE_DEFAULT),
|
||||
|
||||
// @Param: ACC_PNOISE
|
||||
// @DisplayName: Accelerometer noise (m/s^2)
|
||||
// @Description: This noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
|
||||
// @Range: 0.05 1.0
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
// @Units: m/s/s
|
||||
AP_GROUPINFO("ACC_PNOISE", 9, NavEKF2, _accNoise, ACC_PNOISE_DEFAULT),
|
||||
|
||||
// @Param: GBIAS_PNOISE
|
||||
// @DisplayName: Rate gyro bias process noise (rad/s)
|
||||
// @Description: This noise controls the growth of gyro bias state error estimates. Increasing it makes rate gyro bias estimation faster and noisier.
|
||||
// @Range: 0.0000001 0.00001
|
||||
// @User: Advanced
|
||||
// @Units: rad/s
|
||||
AP_GROUPINFO("GBIAS_PNOISE", 10, NavEKF2, _gyroBiasProcessNoise, GBIAS_PNOISE_DEFAULT),
|
||||
|
||||
// @Param: ABIAS_PNOISE
|
||||
// @DisplayName: Accelerometer bias process noise (m/s^2)
|
||||
// @Description: This noise controls the growth of the vertical acelerometer bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
|
||||
// @Range: 0.00001 0.001
|
||||
// @User: Advanced
|
||||
// @Units: m/s/s
|
||||
AP_GROUPINFO("ABIAS_PNOISE", 11, NavEKF2, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT),
|
||||
|
||||
// @Param: MAG_PNOISE
|
||||
// @DisplayName: Magnetic field process noise (gauss/s)
|
||||
// @Description: This noise controls the growth of magnetic field state error estimates. Increasing it makes magnetic field bias estimation faster and noisier.
|
||||
// @Range: 0.0001 0.01
|
||||
// @User: Advanced
|
||||
// @Units: gauss/s
|
||||
AP_GROUPINFO("MAG_PNOISE", 12, NavEKF2, _magProcessNoise, MAG_PNOISE_DEFAULT),
|
||||
|
||||
// @Param: GSCL_PNOISE
|
||||
// @DisplayName: Rate gyro scale factor process noise (1/s)
|
||||
// @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
|
||||
// @Range: 0.0000001 0.00001
|
||||
// @User: Advanced
|
||||
// @Units: 1/s
|
||||
AP_GROUPINFO("GSCL_PNOISE", 13, NavEKF2, _gyroScaleProcessNoise, 1e-6f),
|
||||
|
||||
// @Param: GPS_DELAY
|
||||
// @DisplayName: GPS measurement delay (msec)
|
||||
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
|
||||
// @Range: 0 500
|
||||
// @Increment: 10
|
||||
// @User: Advanced
|
||||
// @Units: milliseconds
|
||||
AP_GROUPINFO("VEL_DELAY", 14, NavEKF2, _msecGpsDelay, 220),
|
||||
|
||||
// this slot has been deprecated and reserved for later use
|
||||
|
||||
// @Param: GPS_TYPE
|
||||
// @DisplayName: GPS mode control
|
||||
// @Description: This parameter controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = use no GPS (optical flow will be used if available)
|
||||
// @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS use optical flow
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GPS_TYPE", 16, NavEKF2, _fusionModeGPS, 0),
|
||||
|
||||
// @Param: VEL_GATE
|
||||
// @DisplayName: GPS velocity measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("VEL_GATE", 17, NavEKF2, _gpsVelInnovGate, VEL_GATE_DEFAULT),
|
||||
|
||||
// @Param: POS_GATE
|
||||
// @DisplayName: GPS position measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("POS_GATE", 18, NavEKF2, _gpsPosInnovGate, POS_GATE_DEFAULT),
|
||||
|
||||
// @Param: HGT_GATE
|
||||
// @DisplayName: Height measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("HGT_GATE", 19, NavEKF2, _hgtInnovGate, HGT_GATE_DEFAULT),
|
||||
|
||||
// @Param: MAG_GATE
|
||||
// @DisplayName: Magnetometer measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MAG_GATE", 20, NavEKF2, _magInnovGate, MAG_GATE_DEFAULT),
|
||||
|
||||
// @Param: EAS_GATE
|
||||
// @DisplayName: Airspeed measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("EAS_GATE", 21, NavEKF2, _tasInnovGate, 10),
|
||||
|
||||
// @Param: MAG_CAL
|
||||
// @DisplayName: Magnetometer calibration mode
|
||||
// @Description: EKF_MAG_CAL = 0 enables calibration based on flying speed and altitude and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration based on manoeuvre level and is the default setting for Copter and Rover users. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition and is recommended if in-flight magnetometer calibration is unreliable.
|
||||
// @Values: 0:Speed and Height,1:Acceleration,2:Never,3:Always
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MAG_CAL", 22, NavEKF2, _magCal, MAG_CAL_DEFAULT),
|
||||
|
||||
// this slot has been deprecated and reserved for later use
|
||||
|
||||
// @Param: GLITCH_RAD
|
||||
// @DisplayName: GPS glitch radius gate size (m)
|
||||
// @Description: This parameter controls the maximum amount of difference in horizontal position (in m) between the value predicted by the filter and the value measured by the GPS before the long term glitch protection logic is activated and an offset is applied to the GPS measurement to compensate. Position steps smaller than this value will be temporarily ignored, but will then be accepted and the filter will move to the new position. Position steps larger than this value will be ignored initially, but the filter will then apply an offset to the GPS position measurement.
|
||||
// @Range: 10 50
|
||||
// @Increment: 5
|
||||
// @User: Advanced
|
||||
// @Units: meters
|
||||
AP_GROUPINFO("GLITCH_RAD", 24, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
|
||||
|
||||
// @Param: GND_GRADIENT
|
||||
// @DisplayName: Terrain Gradient % RMS
|
||||
// @Description: This parameter sets the RMS terrain gradient percentage assumed by the terrain height estimation. Terrain height can be estimated using optical flow and/or range finder sensor data if fitted. Smaller values cause the terrain height estimate to be slower to respond to changes in measurement. Larger values casue the terrain height estimate to be faster to respond, but also more noisy. Generally this value can be reduced if operating over very flat terrain and increased if operating over uneven terrain.
|
||||
// @Range: 1 - 50
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GND_GRADIENT", 25, NavEKF2, _gndGradientSigma, 2),
|
||||
|
||||
// @Param: FLOW_NOISE
|
||||
// @DisplayName: Optical flow measurement noise (rad/s)
|
||||
// @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
|
||||
// @Range: 0.05 - 1.0
|
||||
// @Increment: 0.05
|
||||
// @User: Advanced
|
||||
// @Units: rad/s
|
||||
AP_GROUPINFO("FLOW_NOISE", 26, NavEKF2, _flowNoise, FLOW_NOISE_DEFAULT),
|
||||
|
||||
// @Param: FLOW_GATE
|
||||
// @DisplayName: Optical Flow measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 - 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("FLOW_GATE", 27, NavEKF2, _flowInnovGate, FLOW_GATE_DEFAULT),
|
||||
|
||||
// @Param: FLOW_DELAY
|
||||
// @DisplayName: Optical Flow measurement delay (msec)
|
||||
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
|
||||
// @Range: 0 - 500
|
||||
// @Increment: 10
|
||||
// @User: Advanced
|
||||
// @Units: milliseconds
|
||||
AP_GROUPINFO("FLOW_DELAY", 28, NavEKF2, _msecFlowDelay, FLOW_MEAS_DELAY),
|
||||
|
||||
// @Param: RNG_GATE
|
||||
// @DisplayName: Range finder measurement gate size
|
||||
// @Description: This parameter sets the number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
|
||||
// @Range: 1 - 100
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("RNG_GATE", 29, NavEKF2, _rngInnovGate, 5),
|
||||
|
||||
// @Param: MAX_FLOW
|
||||
// @DisplayName: Maximum valid optical flow rate
|
||||
// @Description: This parameter sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
|
||||
// @Range: 1.0 - 4.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MAX_FLOW", 30, NavEKF2, _maxFlowRate, 2.5f),
|
||||
|
||||
// @Param: FALLBACK
|
||||
// @DisplayName: Fallback strictness
|
||||
// @Description: This parameter controls the conditions necessary to trigger a fallback to DCM and INAV. A value of 1 will cause fallbacks to occur on loss of GPS and other conditions. A value of 0 will trust the EKF more.
|
||||
// @Values: 0:Trust EKF more, 1:Trust DCM more
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("FALLBACK", 31, NavEKF2, _fallback, 1),
|
||||
|
||||
// @Param: ALT_SOURCE
|
||||
// @DisplayName: Primary height source
|
||||
// @Description: This parameter controls which height sensor is used by the EKF during optical flow navigation (when EKF_GPS_TYPE = 3). A value of will 0 cause it to always use baro altitude. A value of 1 will casue it to use range finder if available.
|
||||
// @Values: 0:Use Baro, 1:Use Range Finder
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ALT_SOURCE", 32, NavEKF2, _altSource, 1),
|
||||
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
NavEKF2::NavEKF2() :
|
||||
gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
|
||||
gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
|
||||
gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
|
||||
msecHgtDelay(60), // Height measurement delay (msec)
|
||||
msecMagDelay(60), // Magnetometer measurement delay (msec)
|
||||
msecTasDelay(240), // Airspeed measurement delay (msec)
|
||||
gpsRetryTimeUseTAS(10000), // GPS retry time with airspeed measurements (msec)
|
||||
gpsRetryTimeNoTAS(7000), // GPS retry time without airspeed measurements (msec)
|
||||
gpsFailTimeWithFlow(1000), // If we have no GPS for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
|
||||
hgtRetryTimeMode0(10000), // Height retry time with vertical velocity measurement (msec)
|
||||
hgtRetryTimeMode12(5000), // Height retry time without vertical velocity measurement (msec)
|
||||
tasRetryTime(5000), // True airspeed timeout and retry interval (msec)
|
||||
magFailTimeLimit_ms(10000), // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
|
||||
magVarRateScale(0.05f), // scale factor applied to magnetometer variance due to angular rate
|
||||
gyroBiasNoiseScaler(2.0f), // scale factor applied to imu gyro bias learning before the vehicle is armed
|
||||
accelBiasNoiseScaler(1.0f), // scale factor applied to imu accel bias learning before the vehicle is armed
|
||||
msecGpsAvg(200), // average number of msec between GPS measurements
|
||||
msecHgtAvg(100), // average number of msec between height measurements
|
||||
msecMagAvg(100), // average number of msec between magnetometer measurements
|
||||
msecBetaAvg(100), // average number of msec between synthetic sideslip measurements
|
||||
msecBetaMax(200), // maximum number of msec between synthetic sideslip measurements
|
||||
msecFlowAvg(100), // average number of msec between optical flow measurements
|
||||
dtVelPos(0.2f), // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
|
||||
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
|
||||
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
|
||||
TASmsecMax(200), // maximum allowed interval between airspeed measurement updates
|
||||
DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
|
||||
fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step
|
||||
flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec)
|
||||
flowIntervalMax_ms(100), // maximum allowable time between flow fusion events
|
||||
gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation
|
||||
gndEffectBaroScaler(4.0f) // scaler applied to the barometer observation variance when operating in ground effect
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
#endif //HAL_CPU_CLASS
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
24 state EKF based on https://github.com/priseborough/InertialNav
|
||||
Converted from Matlab to C++ by Paul Riseborough
|
||||
|
||||
EKF Tuning parameters refactored by Tom Cauchois
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef AP_NavEKF2_Tuning
|
||||
#define AP_NavEKF2_Tuning
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
|
||||
class NavEKF2
|
||||
{
|
||||
public:
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
NavEKF2();
|
||||
|
||||
// EKF Mavlink Tuneable Parameters
|
||||
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
|
||||
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
|
||||
AP_Float _gpsHorizPosNoise; // GPS horizontal position measurement noise m
|
||||
AP_Float _baroAltNoise; // Baro height measurement noise : m^2
|
||||
AP_Float _magNoise; // magnetometer measurement noise : gauss
|
||||
AP_Float _easNoise; // equivalent airspeed measurement noise : m/s
|
||||
AP_Float _windVelProcessNoise; // wind velocity state process noise : m/s^2
|
||||
AP_Float _wndVarHgtRateScale; // scale factor applied to wind process noise due to height rate
|
||||
AP_Float _magProcessNoise; // magnetic field process noise : gauss/sec
|
||||
AP_Float _gyrNoise; // gyro process noise : rad/s
|
||||
AP_Float _accNoise; // accelerometer process noise : m/s^2
|
||||
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
|
||||
AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
|
||||
AP_Int16 _msecGpsDelay; // effective average delay of GPS measurements relative to time of receipt (msec)
|
||||
AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
|
||||
AP_Int8 _gpsVelInnovGate; // Number of standard deviations applied to GPS velocity innovation consistency check
|
||||
AP_Int8 _gpsPosInnovGate; // Number of standard deviations applied to GPS position innovation consistency check
|
||||
AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check
|
||||
AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check
|
||||
AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check
|
||||
AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration
|
||||
AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
|
||||
AP_Int8 _gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation.
|
||||
AP_Float _flowNoise; // optical flow rate measurement noise
|
||||
AP_Int8 _flowInnovGate; // Number of standard deviations applied to optical flow innovation consistency check
|
||||
AP_Int8 _msecFlowDelay; // effective average delay of optical flow measurements rel to IMU (msec)
|
||||
AP_Int8 _rngInnovGate; // Number of standard deviations applied to range finder innovation consistency check
|
||||
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
|
||||
AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
|
||||
AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
|
||||
AP_Float _gyroScaleProcessNoise;// gyro scale factor state process noise : 1/s
|
||||
|
||||
// Tuning parameters
|
||||
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
|
||||
const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
|
||||
const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
|
||||
const uint16_t msecHgtDelay; // Height measurement delay (msec)
|
||||
const uint16_t msecMagDelay; // Magnetometer measurement delay (msec)
|
||||
const uint16_t msecTasDelay; // Airspeed measurement delay (msec)
|
||||
const uint16_t gpsRetryTimeUseTAS; // GPS retry time with airspeed measurements (msec)
|
||||
const uint16_t gpsRetryTimeNoTAS; // GPS retry time without airspeed measurements (msec)
|
||||
const uint16_t gpsFailTimeWithFlow; // If we have no GPs for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
|
||||
const uint16_t hgtRetryTimeMode0; // Height retry time with vertical velocity measurement (msec)
|
||||
const uint16_t hgtRetryTimeMode12; // Height retry time without vertical velocity measurement (msec)
|
||||
const uint16_t tasRetryTime; // True airspeed timeout and retry interval (msec)
|
||||
const uint32_t magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
|
||||
const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
|
||||
const float gyroBiasNoiseScaler; // scale factor applied to gyro bias state process noise when on ground
|
||||
const float accelBiasNoiseScaler; // scale factor applied to accel bias state process noise when on ground
|
||||
const uint16_t msecGpsAvg; // average number of msec between GPS measurements
|
||||
const uint16_t msecHgtAvg; // average number of msec between height measurements
|
||||
const uint16_t msecMagAvg; // average number of msec between magnetometer measurements
|
||||
const uint16_t msecBetaAvg; // average number of msec between synthetic sideslip measurements
|
||||
const uint16_t msecBetaMax; // maximum number of msec between synthetic sideslip measurements
|
||||
const uint16_t msecFlowAvg; // average number of msec between optical flow measurements
|
||||
const float dtVelPos; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
|
||||
const float covTimeStepMax; // maximum time (sec) between covariance prediction updates
|
||||
const float covDelAngMax; // maximum delta angle between covariance prediction updates
|
||||
const uint32_t TASmsecMax; // maximum allowed interval between airspeed measurement updates
|
||||
const float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
|
||||
const float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step
|
||||
const uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec)
|
||||
const uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events
|
||||
const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
|
||||
const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active
|
||||
};
|
||||
|
||||
#endif //AP_NavEKF2
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,864 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
24 state EKF based on https://github.com/priseborough/InertialNav
|
||||
|
||||
Converted from Matlab to C++ by Paul Riseborough
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef AP_NavEKF2_core
|
||||
#define AP_NavEKF2_core
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_NavEKF/AP_Nav_Common.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include "AP_NavEKF2.h"
|
||||
|
||||
// #define MATH_CHECK_INDEXES 1
|
||||
|
||||
#include <AP_Math/vectorN.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
#include <systemlib/perf_counter.h>
|
||||
#endif
|
||||
|
||||
class AP_AHRS;
|
||||
|
||||
class NavEKF2_core
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
NavEKF2_core(NavEKF2 &frontend, const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng);
|
||||
|
||||
// Initialise the states from accelerometer and magnetometer data (if present)
|
||||
// This method can only be used when the vehicle is static
|
||||
bool InitialiseFilterBootstrap(void);
|
||||
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void UpdateFilter(void);
|
||||
|
||||
// Check basic filter health metrics and return a consolidated health status
|
||||
bool healthy(void) const;
|
||||
|
||||
// Return the last calculated NED position relative to the reference point (m).
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosNED(Vector3f &pos) const;
|
||||
|
||||
// return NED velocity in m/s
|
||||
void getVelNED(Vector3f &vel) const;
|
||||
|
||||
// This returns the specific forces in the NED frame
|
||||
void getAccelNED(Vector3f &accelNED) const;
|
||||
|
||||
// return body axis gyro bias estimates in rad/sec
|
||||
void getGyroBias(Vector3f &gyroBias) const;
|
||||
|
||||
// return body axis gyro scale factor estimates
|
||||
void getGyroScale(Vector3f &gyroScale) const;
|
||||
|
||||
// return tilt error convergence metric
|
||||
void getTiltError(float &ang) const;
|
||||
|
||||
// reset body axis gyro bias estimates
|
||||
void resetGyroBias(void);
|
||||
|
||||
// Resets the baro so that it reads zero at the current height
|
||||
// Resets the EKF height to zero
|
||||
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
|
||||
// Returns true if the height datum reset has been performed
|
||||
// If using a range finder for height no reset is performed and it returns false
|
||||
bool resetHeightDatum(void);
|
||||
|
||||
// Commands the EKF to not use GPS.
|
||||
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
|
||||
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
|
||||
// Returns 0 if command rejected
|
||||
// Returns 1 if attitude, vertical velocity and vertical position will be provided
|
||||
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
||||
uint8_t setInhibitGPS(void);
|
||||
|
||||
// return the horizontal speed limit in m/s set by optical flow sensor limits
|
||||
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
|
||||
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
|
||||
|
||||
// return weighting of first IMU in blending function
|
||||
void getIMU1Weighting(float &ret) const;
|
||||
|
||||
// return the individual Z-accel bias estimates in m/s^2
|
||||
void getAccelZBias(float &zbias1, float &zbias2) const;
|
||||
|
||||
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
|
||||
void getWind(Vector3f &wind) const;
|
||||
|
||||
// return earth magnetic field estimates in measurement units / 1000
|
||||
void getMagNED(Vector3f &magNED) const;
|
||||
|
||||
// return body magnetic field estimates in measurement units / 1000
|
||||
void getMagXYZ(Vector3f &magXYZ) const;
|
||||
|
||||
// Return estimated magnetometer offsets
|
||||
// Return true if magnetometer offsets are valid
|
||||
bool getMagOffsets(Vector3f &magOffsets) const;
|
||||
|
||||
// Return the last calculated latitude, longitude and height in WGS-84
|
||||
// If a calculated location isn't available, return a raw GPS measurement
|
||||
// The status will return true if a calculation or raw measurement is available
|
||||
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
|
||||
bool getLLH(struct Location &loc) const;
|
||||
|
||||
// return the latitude and longitude and height used to set the NED origin
|
||||
// All NED positions calculated by the filter are relative to this location
|
||||
// Returns false if the origin has not been set
|
||||
bool getOriginLLH(struct Location &loc) const;
|
||||
|
||||
// set the latitude and longitude and height used to set the NED origin
|
||||
// All NED positions calcualted by the filter will be relative to this location
|
||||
// The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
|
||||
// Returns false if the filter has rejected the attempt to set the origin
|
||||
bool setOriginLLH(struct Location &loc);
|
||||
|
||||
// return estimated height above ground level
|
||||
// return false if ground height is not being estimated.
|
||||
bool getHAGL(float &HAGL) const;
|
||||
|
||||
// return the Euler roll, pitch and yaw angle in radians
|
||||
void getEulerAngles(Vector3f &eulers) const;
|
||||
|
||||
// return the transformation matrix from XYZ (body) to NED axes
|
||||
void getRotationBodyToNED(Matrix3f &mat) const;
|
||||
|
||||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
||||
void getQuaternion(Quaternion &quat) const;
|
||||
|
||||
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
|
||||
void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
|
||||
|
||||
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
|
||||
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
|
||||
|
||||
// should we use the compass? This is public so it can be used for
|
||||
// reporting via ahrs.use_compass()
|
||||
bool use_compass(void) const;
|
||||
|
||||
// write the raw optical flow measurements
|
||||
// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
|
||||
// rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
|
||||
// rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
|
||||
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
|
||||
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
|
||||
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas);
|
||||
|
||||
// return data for debugging optical flow fusion
|
||||
void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
|
||||
|
||||
// called by vehicle code to specify that a takeoff is happening
|
||||
// causes the EKF to compensate for expected barometer errors due to ground effect
|
||||
void setTakeoffExpected(bool val);
|
||||
|
||||
// called by vehicle code to specify that a touchdown is expected to happen
|
||||
// causes the EKF to compensate for expected barometer errors due to ground effect
|
||||
void setTouchdownExpected(bool val);
|
||||
|
||||
/*
|
||||
return the filter fault status as a bitmasked integer
|
||||
0 = quaternions are NaN
|
||||
1 = velocities are NaN
|
||||
2 = badly conditioned X magnetometer fusion
|
||||
3 = badly conditioned Y magnetometer fusion
|
||||
5 = badly conditioned Z magnetometer fusion
|
||||
6 = badly conditioned airspeed fusion
|
||||
7 = badly conditioned synthetic sideslip fusion
|
||||
7 = filter is not initialised
|
||||
*/
|
||||
void getFilterFaults(uint8_t &faults) const;
|
||||
|
||||
/*
|
||||
return filter timeout status as a bitmasked integer
|
||||
0 = position measurement timeout
|
||||
1 = velocity measurement timeout
|
||||
2 = height measurement timeout
|
||||
3 = magnetometer measurement timeout
|
||||
5 = unassigned
|
||||
6 = unassigned
|
||||
7 = unassigned
|
||||
7 = unassigned
|
||||
*/
|
||||
void getFilterTimeouts(uint8_t &timeouts) const;
|
||||
|
||||
/*
|
||||
return filter status flags
|
||||
*/
|
||||
void getFilterStatus(nav_filter_status &status) const;
|
||||
|
||||
// send an EKF_STATUS_REPORT message to GCS
|
||||
void send_status_report(mavlink_channel_t chan);
|
||||
|
||||
// provides the height limit to be observed by the control loops
|
||||
// returns false if no height limiting is required
|
||||
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
|
||||
bool getHeightControlLimit(float &height) const;
|
||||
|
||||
// provides the quaternion that was used by the INS calculation to rotate from the previous orientation to the orientaion at the current time step
|
||||
// returns a zero rotation quaternion if the INS calculation was not performed on that time step.
|
||||
Quaternion getDeltaQuaternion(void) const;
|
||||
|
||||
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
||||
// returns true if a reset yaw angle has been updated and not queried
|
||||
// this function should not have more than one client
|
||||
bool getLastYawResetAngle(float &yawAng);
|
||||
|
||||
// Reference to the global EKF frontend for parameters
|
||||
NavEKF2 &frontend;
|
||||
|
||||
private:
|
||||
typedef float ftype;
|
||||
#if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
|
||||
typedef VectorN<ftype,2> Vector2;
|
||||
typedef VectorN<ftype,3> Vector3;
|
||||
typedef VectorN<ftype,4> Vector4;
|
||||
typedef VectorN<ftype,5> Vector5;
|
||||
typedef VectorN<ftype,6> Vector6;
|
||||
typedef VectorN<ftype,7> Vector7;
|
||||
typedef VectorN<ftype,8> Vector8;
|
||||
typedef VectorN<ftype,9> Vector9;
|
||||
typedef VectorN<ftype,10> Vector10;
|
||||
typedef VectorN<ftype,11> Vector11;
|
||||
typedef VectorN<ftype,13> Vector13;
|
||||
typedef VectorN<ftype,14> Vector14;
|
||||
typedef VectorN<ftype,15> Vector15;
|
||||
typedef VectorN<ftype,22> Vector22;
|
||||
typedef VectorN<ftype,23> Vector23;
|
||||
typedef VectorN<ftype,24> Vector24;
|
||||
typedef VectorN<ftype,25> Vector25;
|
||||
typedef VectorN<ftype,31> Vector31;
|
||||
typedef VectorN<VectorN<ftype,3>,3> Matrix3;
|
||||
typedef VectorN<VectorN<ftype,24>,24> Matrix24;
|
||||
typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
|
||||
typedef VectorN<uint32_t,50> Vector_u32_50;
|
||||
#else
|
||||
typedef ftype Vector2[2];
|
||||
typedef ftype Vector3[3];
|
||||
typedef ftype Vector4[4];
|
||||
typedef ftype Vector5[5];
|
||||
typedef ftype Vector6[6];
|
||||
typedef ftype Vector7[7];
|
||||
typedef ftype Vector8[8];
|
||||
typedef ftype Vector9[9];
|
||||
typedef ftype Vector10[10];
|
||||
typedef ftype Vector11[11];
|
||||
typedef ftype Vector13[13];
|
||||
typedef ftype Vector14[14];
|
||||
typedef ftype Vector15[15];
|
||||
typedef ftype Vector22[22];
|
||||
typedef ftype Vector23[23];
|
||||
typedef ftype Vector24[24];
|
||||
typedef ftype Vector25[25];
|
||||
typedef ftype Vector28[28];
|
||||
typedef ftype Matrix3[3][3];
|
||||
typedef ftype Matrix24[24][24];
|
||||
typedef ftype Matrix34_50[34][50];
|
||||
typedef uint32_t Vector_u32_50[50];
|
||||
#endif
|
||||
|
||||
const AP_AHRS *_ahrs;
|
||||
AP_Baro &_baro;
|
||||
const RangeFinder &_rng;
|
||||
|
||||
// the states are available in two forms, either as a Vector31, or
|
||||
// broken down as individual elements. Both are equivalent (same
|
||||
// memory)
|
||||
Vector28 statesArray;
|
||||
struct state_elements {
|
||||
Vector3f angErr; // 0..2
|
||||
Vector3f velocity; // 3..5
|
||||
Vector3f position; // 6..8
|
||||
Vector3f gyro_bias; // 9..11
|
||||
Vector3f gyro_scale; // 12..14
|
||||
float accel_zbias; // 15
|
||||
Vector3f earth_magfield; // 16..18
|
||||
Vector3f body_magfield; // 19..21
|
||||
Vector2f wind_vel; // 22..23
|
||||
Quaternion quat; // 24..27
|
||||
} &stateStruct;
|
||||
|
||||
struct output_elements {
|
||||
Quaternion quat; // 0..3
|
||||
Vector3f velocity; // 4..6
|
||||
Vector3f position; // 7..9
|
||||
};
|
||||
|
||||
struct imu_elements {
|
||||
Vector3f delAng; // 0..2
|
||||
Vector3f delVel; // 3..5
|
||||
float delAngDT; // 6
|
||||
float delVelDT; // 7
|
||||
uint32_t frame; // 8
|
||||
uint32_t time_ms; // 9
|
||||
};
|
||||
|
||||
struct gps_elements {
|
||||
Vector2f pos; // 0..1
|
||||
float hgt; // 2
|
||||
Vector3f vel; // 3..5
|
||||
uint32_t time_ms; // 6
|
||||
};
|
||||
|
||||
struct mag_elements {
|
||||
Vector3f mag; // 0..2
|
||||
uint32_t time_ms; // 3
|
||||
};
|
||||
|
||||
struct baro_elements {
|
||||
float hgt; // 0
|
||||
uint32_t time_ms; // 1
|
||||
};
|
||||
|
||||
struct tas_elements {
|
||||
float tas; // 0
|
||||
uint32_t time_ms; // 1
|
||||
};
|
||||
|
||||
struct of_elements {
|
||||
Vector2f flowRadXY; // 0..1
|
||||
Vector2f flowRadXYcomp; // 2..3
|
||||
uint32_t time_ms; // 4
|
||||
};
|
||||
|
||||
// update the quaternion, velocity and position states using IMU measurements
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
// calculate the predicted state covariance matrix
|
||||
void CovariancePrediction();
|
||||
|
||||
// force symmetry on the state covariance matrix
|
||||
void ForceSymmetry();
|
||||
|
||||
// copy covariances across from covariance prediction calculation and fix numerical errors
|
||||
void CopyCovariances();
|
||||
|
||||
// constrain variances (diagonal terms) in the state covariance matrix
|
||||
void ConstrainVariances();
|
||||
|
||||
// constrain states
|
||||
void ConstrainStates();
|
||||
|
||||
// fuse selected position, velocity and height measurements
|
||||
void FuseVelPosNED();
|
||||
|
||||
// fuse magnetometer measurements
|
||||
void FuseMagnetometer();
|
||||
|
||||
// fuse true airspeed measurements
|
||||
void FuseAirspeed();
|
||||
|
||||
// fuse sythetic sideslip measurement of zero
|
||||
void FuseSideslip();
|
||||
|
||||
// zero specified range of rows in the state covariance matrix
|
||||
void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last);
|
||||
|
||||
// zero specified range of columns in the state covariance matrix
|
||||
void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);
|
||||
|
||||
// store imu data in the FIFO
|
||||
void StoreIMU(void);
|
||||
|
||||
// Reset the stored IMU history to current data
|
||||
void StoreIMU_reset(void);
|
||||
|
||||
// recall IMU data from the FIFO
|
||||
void RecallIMU();
|
||||
|
||||
// store output data in the FIFO
|
||||
void StoreOutput(void);
|
||||
|
||||
// Reset the stored output history to current data
|
||||
void StoreOutputReset(void);
|
||||
|
||||
// Reset the stored output quaternion history to current EKF state
|
||||
void StoreQuatReset(void);
|
||||
|
||||
// recall output data from the FIFO
|
||||
void RecallOutput();
|
||||
|
||||
// store altimeter data
|
||||
void StoreBaro();
|
||||
|
||||
// recall altimeter data at the fusion time horizon
|
||||
// return true if data found
|
||||
bool RecallBaro();
|
||||
|
||||
// store magnetometer data
|
||||
void StoreMag();
|
||||
|
||||
// recall magetometer data at the fusion time horizon
|
||||
// return true if data found
|
||||
bool RecallMag();
|
||||
|
||||
// store GPS data
|
||||
void StoreGPS();
|
||||
|
||||
// recall GPS data at the fusion time horizon
|
||||
// return true if data found
|
||||
bool RecallGPS();
|
||||
|
||||
// store true airspeed data
|
||||
void StoreTAS();
|
||||
|
||||
// recall true airspeed data at the fusion time horizon
|
||||
// return true if data found
|
||||
bool RecallTAS();
|
||||
|
||||
// store optical flow data
|
||||
void StoreOF();
|
||||
|
||||
// recall optical flow data at the fusion time horizon
|
||||
// return true if data found
|
||||
bool RecallOF();
|
||||
|
||||
// calculate nav to body quaternions from body to nav rotation matrix
|
||||
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
|
||||
|
||||
// calculate the NED earth spin vector in rad/sec
|
||||
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
|
||||
|
||||
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
|
||||
void SetFlightAndFusionModes();
|
||||
|
||||
// initialise the covariance matrix
|
||||
void CovarianceInit();
|
||||
|
||||
// helper functions for readIMUData
|
||||
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
|
||||
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng);
|
||||
|
||||
// update IMU delta angle and delta velocity measurements
|
||||
void readIMUData();
|
||||
|
||||
// check for new valid GPS data and update stored measurement if available
|
||||
void readGpsData();
|
||||
|
||||
// check for new altitude measurement data and update stored measurement if available
|
||||
void readHgtData();
|
||||
|
||||
// check for new magnetometer data and update store measurements if available
|
||||
void readMagData();
|
||||
|
||||
// check for new airspeed data and update stored measurements if available
|
||||
void readAirSpdData();
|
||||
|
||||
// determine when to perform fusion of GPS position and velocity measurements
|
||||
void SelectVelPosFusion();
|
||||
|
||||
// determine when to perform fusion of magnetometer measurements
|
||||
void SelectMagFusion();
|
||||
|
||||
// determine when to perform fusion of true airspeed measurements
|
||||
void SelectTasFusion();
|
||||
|
||||
// determine when to perform fusion of synthetic sideslp measurements
|
||||
void SelectBetaFusion();
|
||||
|
||||
// force alignment of the yaw angle using GPS velocity data
|
||||
void alignYawGPS();
|
||||
|
||||
// initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements
|
||||
// and return attitude quaternion
|
||||
Quaternion calcQuatAndFieldStates(float roll, float pitch);
|
||||
|
||||
// zero stored variables
|
||||
void InitialiseVariables();
|
||||
|
||||
// reset the horizontal position states uing the last GPS measurement
|
||||
void ResetPosition(void);
|
||||
|
||||
// reset velocity states using the last GPS measurement
|
||||
void ResetVelocity(void);
|
||||
|
||||
// reset the vertical position state using the last height measurement
|
||||
void ResetHeight(void);
|
||||
|
||||
// return true if we should use the airspeed sensor
|
||||
bool useAirspeed(void) const;
|
||||
|
||||
// return true if the vehicle code has requested the filter to be ready for flight
|
||||
bool readyToUseGPS(void) const;
|
||||
|
||||
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s
|
||||
// this allows large GPS position jumps to be accomodated gradually
|
||||
void decayGpsOffset(void);
|
||||
|
||||
// Check for filter divergence
|
||||
void checkDivergence(void);
|
||||
|
||||
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
|
||||
void calcIMU_Weighting(float K1, float K2);
|
||||
|
||||
// return true if optical flow data is available
|
||||
bool optFlowDataPresent(void) const;
|
||||
|
||||
// return true if we should use the range finder sensor
|
||||
bool useRngFinder(void) const;
|
||||
|
||||
// determine when to perform fusion of optical flow measurements
|
||||
void SelectFlowFusion();
|
||||
|
||||
// Estimate terrain offset using a single state EKF
|
||||
void EstimateTerrainOffset();
|
||||
|
||||
// fuse optical flow measurements into the main filter
|
||||
void FuseOptFlow();
|
||||
|
||||
// Check arm status and perform required checks and mode changes
|
||||
void performArmingChecks();
|
||||
|
||||
// Set the NED origin to be used until the next filter reset
|
||||
void setOrigin();
|
||||
|
||||
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
|
||||
bool getTakeoffExpected();
|
||||
|
||||
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
|
||||
bool getTouchdownExpected();
|
||||
|
||||
// Assess GPS data quality and return true if good enough to align the EKF
|
||||
bool calcGpsGoodToAlign(void);
|
||||
|
||||
// Read the range finder and take new measurements if available
|
||||
// Apply a median filter to range finder data
|
||||
void readRangeFinder();
|
||||
|
||||
// check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
|
||||
void detectOptFlowTakeoff(void);
|
||||
|
||||
// align the NE earth magnetic field states with the published declination
|
||||
void alignMagStateDeclination();
|
||||
|
||||
// correct the quaternion using an attitude error vector
|
||||
void correctQuatStates(Vector3f &errVec);
|
||||
|
||||
// Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
|
||||
void fuseCompass();
|
||||
|
||||
// Calculate compass heading innovation
|
||||
float calcMagHeadingInnov();
|
||||
|
||||
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
|
||||
// using buffered IMU data
|
||||
void calcOutputStates();
|
||||
|
||||
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
|
||||
// using a simple observer
|
||||
void calcOutputStatesFast();
|
||||
|
||||
// measurement buffer sizes
|
||||
static const uint32_t IMU_BUFFER_LENGTH = 100; // number of IMU samples stored in the buffer. Samples*delta_time must be > max sensor delay
|
||||
static const uint32_t OBS_BUFFER_LENGTH = 5; // number of non-IMU sensor samples stored in the buffer.
|
||||
|
||||
// Variables
|
||||
bool statesInitialised; // boolean true when filter states have been initialised
|
||||
bool velHealth; // boolean true if velocity measurements have passed innovation consistency check
|
||||
bool posHealth; // boolean true if position measurements have passed innovation consistency check
|
||||
bool hgtHealth; // boolean true if height measurements have passed innovation consistency check
|
||||
bool magHealth; // boolean true if magnetometer has passed innovation consistency check
|
||||
bool tasHealth; // boolean true if true airspeed has passed innovation consistency check
|
||||
bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out
|
||||
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
|
||||
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
|
||||
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
|
||||
bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
|
||||
bool badMag; // boolean true if the magnetometer is declared to be producing bad data
|
||||
bool badIMUdata; // boolean true if the bad IMU data is detected
|
||||
|
||||
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
|
||||
Vector28 Kfusion; // Kalman gain vector
|
||||
Matrix24 KH; // intermediate result used for covariance updates
|
||||
Matrix24 KHP; // intermediate result used for covariance updates
|
||||
Matrix24 P; // covariance matrix
|
||||
imu_elements storedIMU[IMU_BUFFER_LENGTH]; // IMU data buffer
|
||||
gps_elements storedGPS[OBS_BUFFER_LENGTH]; // GPS data buffer
|
||||
mag_elements storedMag[OBS_BUFFER_LENGTH]; // Magnetometer data buffer
|
||||
baro_elements storedBaro[OBS_BUFFER_LENGTH]; // Baro data buffer
|
||||
tas_elements storedTAS[OBS_BUFFER_LENGTH]; // TAS data buffer
|
||||
output_elements storedOutput[IMU_BUFFER_LENGTH];// output state buffer
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
|
||||
Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s)
|
||||
Vector3f lastGyroBias; // previous gyro bias vector used by filter divergence check
|
||||
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
|
||||
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
|
||||
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
ftype dtIMUavg; // expected time between IMU measurements (sec)
|
||||
ftype dt; // time lapsed since the last covariance prediction (sec)
|
||||
ftype hgtRate; // state for rate of change of height filter
|
||||
bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool prevOnGround; // value of onGround from previous update
|
||||
bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity
|
||||
uint32_t airborneDetectTime_ms; // last time flight movement was detected
|
||||
Vector6 innovVelPos; // innovation output for a group of measurements
|
||||
Vector6 varInnovVelPos; // innovation variance output for a group of measurements
|
||||
bool fuseVelData; // this boolean causes the velNED measurements to be fused
|
||||
bool fusePosData; // this boolean causes the posNE measurements to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
|
||||
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
|
||||
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
|
||||
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
||||
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
bool covPredStep; // boolean set to true when a covariance prediction step has been performed
|
||||
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
|
||||
bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step
|
||||
bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed
|
||||
bool tasFuseStep; // boolean set to true when airspeed fusion is being performed
|
||||
uint32_t TASmsecPrev; // time stamp of last TAS fusion step
|
||||
uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip fusion step
|
||||
uint32_t MAGmsecPrev; // time stamp of last compass fusion step
|
||||
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
|
||||
bool constPosMode; // true when fusing a constant position to maintain attitude reference for planned operation without GPS or optical flow data
|
||||
uint32_t lastMagUpdate; // last time compass was updated
|
||||
Vector3f velDotNED; // rate of change of velocity in NED frame
|
||||
Vector3f velDotNEDfilt; // low pass filtered velDotNED
|
||||
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
|
||||
bool newDataMag; // true when new magnetometer data has arrived
|
||||
bool newDataTas; // true when new airspeed data has arrived
|
||||
bool tasDataWaiting; // true when new airspeed data is waiting to be fused
|
||||
uint32_t lastHgtReceived_ms; // time last time we received height data
|
||||
uint16_t hgtRetryTime; // time allowed without use of height measurements before a height timeout is declared
|
||||
uint32_t lastVelPassTime; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
|
||||
uint32_t lastPosPassTime; // time stamp when GPS position measurement last passed innovation consistency check (msec)
|
||||
uint32_t lastPosFailTime; // time stamp when GPS position measurement last failed innovation consistency check (msec)
|
||||
uint32_t lastHgtPassTime; // time stamp when height measurement last passed innovation consistency check (msec)
|
||||
uint32_t lastTasPassTime; // time stamp when airspeed measurement last passed innovation consistency check (msec)
|
||||
uint32_t lastStateStoreTime_ms; // time of last state vector storage
|
||||
uint32_t lastTimeGpsReceived_ms;// last time we recieved GPS data
|
||||
uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements
|
||||
uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update
|
||||
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
|
||||
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
|
||||
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator
|
||||
Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator
|
||||
Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator
|
||||
Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
|
||||
Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix
|
||||
Vector25 SF; // intermediate variables used to calculate predicted covariance matrix
|
||||
Vector5 SG; // intermediate variables used to calculate predicted covariance matrix
|
||||
Vector8 SQ; // intermediate variables used to calculate predicted covariance matrix
|
||||
Vector23 SPP; // intermediate variables used to calculate predicted covariance matrix
|
||||
float IMU1_weighting; // Weighting applied to use of IMU1. Varies between 0 and 1.
|
||||
bool yawAligned; // true when the yaw angle has been aligned
|
||||
Vector2f gpsPosGlitchOffsetNE; // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution
|
||||
Vector2f lastKnownPositionNE; // last known position
|
||||
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
|
||||
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
|
||||
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
|
||||
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
|
||||
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
|
||||
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool firstArmComplete; // true when first transition out of static mode has been performed after start up
|
||||
bool firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed
|
||||
bool secondMagYawInit; // true when the second post takeoff initialisation of earth field and yaw angle has been performed
|
||||
bool flowTimeout; // true when optical flow measurements have time out
|
||||
Vector2f gpsVelGlitchOffset; // Offset applied to the GPS velocity when the gltch radius is being decayed back to zero
|
||||
bool gpsNotAvailable; // bool true when valid GPS data is not available
|
||||
bool filterArmed; // true when the vehicle is disarmed
|
||||
bool prevFilterArmed; // vehicleArmed from previous frame
|
||||
struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset
|
||||
bool validOrigin; // true when the EKF origin is valid
|
||||
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver
|
||||
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
|
||||
Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update
|
||||
bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter
|
||||
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
|
||||
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
|
||||
bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
|
||||
float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
|
||||
bool useGpsVertVel; // true if GPS vertical velocity should be used
|
||||
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
|
||||
bool yawResetAngleWaiting; // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
|
||||
Vector3f tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion
|
||||
float tiltErrFilt; // Filtered tilt error metric
|
||||
bool tiltAlignComplete; // true when tilt alignment is complete
|
||||
bool yawAlignComplete; // true when yaw alignment is complete
|
||||
uint8_t stateIndexLim; // Max state index used during matrix and array operations
|
||||
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
|
||||
imu_elements imuDataNew; // IMU data at the current time horizon
|
||||
uint8_t fifoIndexNow; // Global index for inertial and output solution at current time horizon
|
||||
uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon
|
||||
uint32_t hgtMeasTime_ms; // Effective measurement time of last received height measurement
|
||||
uint32_t magMeasTime_ms; // Effective measurement time of last received magnetometer measurement
|
||||
baro_elements baroDataNew; // Baro data at the current time horizon
|
||||
baro_elements baroDataDelayed; // Baro data at the fusion time horizon
|
||||
uint8_t baroStoreIndex; // Baro data storage index
|
||||
tas_elements tasDataNew; // TAS data at the current time horizon
|
||||
tas_elements tasDataDelayed; // TAS data at the fusion time horizon
|
||||
uint8_t tasStoreIndex; // TAS data storage index
|
||||
mag_elements magDataNew; // Magnetometer data at the current time horizon
|
||||
mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
|
||||
uint8_t magStoreIndex; // Magnetometer data storage index
|
||||
gps_elements gpsDataNew; // GPS data at the current time horizon
|
||||
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
|
||||
uint8_t gpsStoreIndex; // GPS data storage index
|
||||
output_elements outputDataNew; // output state data at the current time step
|
||||
output_elements outputDataDelayed; // output state data at the current time step
|
||||
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
|
||||
Vector3f delVelCorrection; // correction applied to earth frame delta velocities used by output observer to track the EKF
|
||||
Vector3f velCorrection; // correction applied to velocities used by the output observer to track the EKF
|
||||
float innovYaw; // compass yaw angle innovation (rad)
|
||||
uint32_t timeTasReceived_ms; // tie last TAS data was received (msec)
|
||||
bool gpsQualGood; // true when the GPS quality can be used to initialise the navigation system
|
||||
|
||||
// variables added for optical flow fusion
|
||||
of_elements storedOF[OBS_BUFFER_LENGTH]; // OF data buffer
|
||||
of_elements ofDataNew; // OF data at the current time horizon
|
||||
of_elements ofDataDelayed; // OF data at the fusion time horizon
|
||||
uint8_t ofStoreIndex; // OF data storage index
|
||||
bool newDataFlow; // true when new optical flow data has arrived
|
||||
bool flowFusePerformed; // true when optical flow fusion has been performed in that time step
|
||||
bool flowDataValid; // true while optical flow data is still fresh
|
||||
bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused
|
||||
float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
float auxFlowObsInnovVar; // innovation variance for optical flow observations from 1-state terrain offset estimator
|
||||
Vector2 flowRadXYcomp; // motion compensated optical flow angular rates(rad/sec)
|
||||
Vector2 flowRadXY; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
|
||||
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
|
||||
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
|
||||
uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality.
|
||||
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
|
||||
Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period
|
||||
Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period
|
||||
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
|
||||
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
|
||||
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
|
||||
float Popt; // Optical flow terrain height state covariance (m^2)
|
||||
float terrainState; // terrain position state (m)
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
bool fuseRngData; // true when fusion of range data is demanded
|
||||
float varInnovRng; // range finder observation innovation variance (m^2)
|
||||
float innovRng; // range finder observation innovation (m)
|
||||
float rngMea; // range finder measurement (m)
|
||||
bool inhibitGndState; // true when the terrain position state is to remain constant
|
||||
uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
|
||||
Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
|
||||
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
|
||||
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
|
||||
bool newDataRng; // true when new valid range finder data has arrived.
|
||||
bool constVelMode; // true when fusing a constant velocity to maintain attitude reference when either optical flow or GPS measurements are lost after arming
|
||||
bool lastConstVelMode; // last value of holdVelocity
|
||||
Vector2f heldVelNE; // velocity held when no aiding is available
|
||||
enum AidingMode {AID_ABSOLUTE=0, // GPS aiding is being used (optical flow may also be used) so position estimates are absolute.
|
||||
AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
|
||||
AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative
|
||||
};
|
||||
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
|
||||
bool gndOffsetValid; // true when the ground offset state can still be considered valid
|
||||
bool flowXfailed; // true when the X optical flow measurement has failed the innovation consistency check
|
||||
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
|
||||
float delTimeOF; // time that delAngBodyOF is summed across
|
||||
|
||||
// Range finder
|
||||
float baroHgtOffset; // offset applied when baro height used as a backup height reference if range-finder fails
|
||||
float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
|
||||
float storedRngMeas[3]; // Ringbuffer of stored range measurements
|
||||
uint32_t storedRngMeasTime_ms[3]; // Ringbuffer of stored range measurement times
|
||||
uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
|
||||
uint8_t rngMeasIndex; // Current range measurement ringbuffer index
|
||||
|
||||
// Movement detector
|
||||
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
|
||||
float rangeAtArming; // range finder measurement when armed
|
||||
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
|
||||
|
||||
// IMU processing
|
||||
float dtDelVel;
|
||||
float dtDelVel2;
|
||||
|
||||
// baro ground effect
|
||||
bool expectGndEffectTakeoff; // external state from ArduCopter - takeoff expected
|
||||
uint32_t takeoffExpectedSet_ms; // system time at which expectGndEffectTakeoff was set
|
||||
bool expectGndEffectTouchdown; // external state from ArduCopter - touchdown expected
|
||||
uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
|
||||
float meaHgtAtTakeOff; // height measured at commencement of takeoff
|
||||
|
||||
struct {
|
||||
bool bad_xmag:1;
|
||||
bool bad_ymag:1;
|
||||
bool bad_zmag:1;
|
||||
bool bad_airspeed:1;
|
||||
bool bad_sideslip:1;
|
||||
} faultStatus;
|
||||
|
||||
// states held by magnetomter fusion across time steps
|
||||
// magnetometer X,Y,Z measurements are fused across three time steps
|
||||
// to level computational load as this is an expensive operation
|
||||
struct {
|
||||
ftype q0;
|
||||
ftype q1;
|
||||
ftype q2;
|
||||
ftype q3;
|
||||
ftype magN;
|
||||
ftype magE;
|
||||
ftype magD;
|
||||
ftype magXbias;
|
||||
ftype magYbias;
|
||||
ftype magZbias;
|
||||
uint8_t obsIndex;
|
||||
Matrix3f DCM;
|
||||
Vector3f MagPred;
|
||||
ftype R_MAG;
|
||||
Vector9 SH_MAG;
|
||||
} mag_state;
|
||||
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
// performance counters
|
||||
perf_counter_t _perf_UpdateFilter;
|
||||
perf_counter_t _perf_CovariancePrediction;
|
||||
perf_counter_t _perf_FuseVelPosNED;
|
||||
perf_counter_t _perf_FuseMagnetometer;
|
||||
perf_counter_t _perf_FuseAirspeed;
|
||||
perf_counter_t _perf_FuseSideslip;
|
||||
perf_counter_t _perf_OpticalFlowEKF;
|
||||
perf_counter_t _perf_FuseOptFlow;
|
||||
#endif
|
||||
|
||||
// should we assume zero sideslip?
|
||||
bool assume_zero_sideslip(void) const;
|
||||
|
||||
// vehicle specific initial gyro bias uncertainty
|
||||
float InitialGyroBiasUncertainty(void) const;
|
||||
};
|
||||
|
||||
#if CONFIG_HAL_BOARD != HAL_BOARD_PX4 && CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
|
||||
#define perf_begin(x)
|
||||
#define perf_end(x)
|
||||
#endif
|
||||
|
||||
#endif // AP_NavEKF2_core
|
||||
Loading…
Reference in New Issue