Line data Source code
1 : /*
2 : 24 state EKF based on the derivation in https://github.com/PX4/ecl/
3 : blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
4 :
5 : Converted from Matlab to C++ by Paul Riseborough
6 :
7 : This program is free software: you can redistribute it and/or modify
8 : it under the terms of the GNU General Public License as published by
9 : the Free Software Foundation, either version 3 of the License, or
10 : (at your option) any later version.
11 :
12 : This program is distributed in the hope that it will be useful,
13 : but WITHOUT ANY WARRANTY; without even the implied warranty of
14 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 : GNU General Public License for more details.
16 :
17 : You should have received a copy of the GNU General Public License
18 : along with this program. If not, see <http://www.gnu.org/licenses/>.
19 : */
20 : #pragma once
21 :
22 : #include <AP_Common/Location.h>
23 : #include <AP_Math/AP_Math.h>
24 : #include <AP_Param/AP_Param.h>
25 : #include <AP_NavEKF/AP_Nav_Common.h>
26 : #include <AP_NavEKF/AP_NavEKF_Source.h>
27 :
28 : class NavEKF3_core;
29 : class EKFGSF_yaw;
30 :
31 : class NavEKF3 {
32 : friend class NavEKF3_core;
33 :
34 : public:
35 : NavEKF3();
36 :
37 : /* Do not allow copies */
38 : CLASS_NO_COPY(NavEKF3);
39 :
40 : static const struct AP_Param::GroupInfo var_info[];
41 : static const struct AP_Param::GroupInfo var_info2[];
42 :
43 : // allow logging to determine the number of active cores
44 9752004 : uint8_t activeCores(void) const {
45 9752004 : return num_cores;
46 : }
47 :
48 : // Initialise the filter
49 : bool InitialiseFilter(void);
50 :
51 : // Update Filter States - this should be called whenever new IMU data is available
52 : void UpdateFilter(void);
53 :
54 : // Check basic filter health metrics and return a consolidated health status
55 : bool healthy(void) const;
56 :
57 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
58 : // requires_position should be true if horizontal position configuration should be checked
59 : bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
60 :
61 : // returns the index of the primary core
62 : // return -1 if no primary core selected
63 : int8_t getPrimaryCoreIndex(void) const;
64 :
65 : // returns the index of the IMU of the primary core
66 : // return -1 if no primary core selected
67 : int8_t getPrimaryCoreIMUIndex(void) const;
68 :
69 : // Write the last calculated NE position relative to the reference point (m)
70 : // If a calculated solution is not available, use the best available data and return false
71 : // If false returned, do not use for flight control
72 : bool getPosNE(Vector2p &posNE) const;
73 :
74 : // Write the last calculated D position relative to the reference point (m)
75 : // If a calculated solution is not available, use the best available data and return false
76 : // If false returned, do not use for flight control
77 : bool getPosD(postype_t &posD) const;
78 :
79 : // return NED velocity in m/s
80 : void getVelNED(Vector3f &vel) const;
81 :
82 : // return estimate of true airspeed vector in body frame in m/s
83 : // returns false if estimate is unavailable
84 : bool getAirSpdVec(Vector3f &vel) const;
85 :
86 : // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed for a given sensor instance
87 : // returns false if the data is unavailable
88 : bool getAirSpdHealthData(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const;
89 :
90 : // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
91 : // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
92 : // but will always be kinematically consistent with the z component of the EKF position state
93 : float getPosDownDerivative() const;
94 :
95 : // return body axis gyro bias estimates in rad/sec for the specified instance
96 : // An out of range instance (eg -1) returns data for the primary instance
97 : void getGyroBias(int8_t instance, Vector3f &gyroBias) const;
98 :
99 : // return accelerometer bias estimate in m/s/s
100 : // An out of range instance (eg -1) returns data for the primary instance
101 : void getAccelBias(int8_t instance, Vector3f &accelBias) const;
102 :
103 : //returns index of the active source set used
104 : uint8_t get_active_source_set() const;
105 :
106 : // reset body axis gyro bias estimates
107 : void resetGyroBias(void);
108 :
109 : // Resets the baro so that it reads zero at the current height
110 : // Resets the EKF height to zero
111 : // Adjusts the EKF origin height so that the EKF height + origin height is the same as before
112 : // Returns true if the height datum reset has been performed
113 : // If using a range finder for height no reset is performed and it returns false
114 : bool resetHeightDatum(void);
115 :
116 : // return the horizontal speed limit in m/s set by optical flow sensor limits
117 : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
118 : void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
119 :
120 : // return the NED wind speed estimates in m/s (positive is air
121 : // moving in the direction of the axis) returns true if wind state
122 : // estimation is active
123 : bool getWind(Vector3f &wind) const;
124 :
125 : // return earth magnetic field estimates in measurement units / 1000
126 : void getMagNED(Vector3f &magNED) const;
127 :
128 : // return body magnetic field estimates in measurement units / 1000
129 : void getMagXYZ(Vector3f &magXYZ) const;
130 :
131 : // return the airspeed sensor in use
132 : uint8_t getActiveAirspeed() const;
133 :
134 : // Return estimated magnetometer offsets
135 : // Return true if magnetometer offsets are valid
136 : bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
137 :
138 : // Return the last calculated latitude, longitude and height in WGS-84
139 : // If a calculated location isn't available, return a raw GPS measurement
140 : // The status will return true if a calculation or raw measurement is available
141 : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
142 : bool getLLH(Location &loc) const;
143 :
144 : // Return the latitude and longitude and height used to set the NED origin
145 : // All NED positions calculated by the filter are relative to this location
146 : // Returns false if the origin has not been set
147 : bool getOriginLLH(Location &loc) const;
148 :
149 : // set the latitude and longitude and height used to set the NED origin
150 : // All NED positions calculated by the filter will be relative to this location
151 : // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
152 : // Returns false if the filter has rejected the attempt to set the origin
153 : bool setOriginLLH(const Location &loc);
154 :
155 : // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty
156 : // The altitude element of the location is not used.
157 : // Returns true if the set was successful
158 : bool setLatLng(const Location &loc, float posErr, uint32_t timestamp_ms);
159 :
160 : // return estimated height above ground level
161 : // return false if ground height is not being estimated.
162 : bool getHAGL(float &HAGL) const;
163 :
164 : // return the Euler roll, pitch and yaw angle in radians
165 : void getEulerAngles(Vector3f &eulers) const;
166 :
167 : // return the transformation matrix from XYZ (body) to NED axes
168 : void getRotationBodyToNED(Matrix3f &mat) const;
169 :
170 : // return the quaternions defining the rotation from XYZ (body) to NED axes
171 : void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const;
172 :
173 : // return the quaternions defining the rotation from NED to XYZ (autopilot) axes
174 : void getQuaternion(Quaternion &quat) const;
175 :
176 : // return the innovations
177 : bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
178 :
179 : // return the innovation consistency test ratios
180 : bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
181 :
182 : // return 1-sigma position and velocity uncertainty from the EKF state error covariance matrix P
183 : bool getPosVelUncertainty(float &pos_horiz_m, float &pos_vert_m, float &vel_m_s) const;
184 :
185 : // get a source's velocity innovations
186 : // returns true on success and results are placed in innovations and variances arguments
187 : bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
188 :
189 : // should we use the compass? This is public so it can be used for
190 : // reporting via ahrs.use_compass()
191 : bool use_compass(void) const;
192 :
193 : // write the raw optical flow measurements
194 : // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
195 : // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
196 : // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
197 : // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
198 : // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
199 : // posOffset is the XYZ flow sensor position in the body frame in m
200 : // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
201 : void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
202 :
203 : // retrieve latest corrected optical flow samples (used for calibration)
204 : bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
205 :
206 : /*
207 : * Write body frame linear and angular displacement measurements from a visual odometry sensor
208 : *
209 : * quality is a normalised confidence value from 0 to 100
210 : * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m)
211 : * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
212 : * delTime is the time interval for the measurement of delPos and delAng (sec)
213 : * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
214 : * delay_ms is the average delay of external nav system measurements relative to inertial measurements
215 : * posOffset is the XYZ body frame position of the camera focal point (m)
216 : */
217 : void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
218 :
219 : /*
220 : * Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis
221 : *
222 : * delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad)
223 : * delTime is the time interval for the measurement of delAng (sec)
224 : * timeStamp_ms is the time when the rotation was last measured (msec)
225 : * posOffset is the XYZ body frame position of the wheel hub (m)
226 : * radius is the effective rolling radius of the wheel (m)
227 : * this should not be called at more than the EKF's update rate (50hz or 100hz)
228 : */
229 : void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
230 :
231 : /*
232 : * Writes the measurement from a yaw angle sensor
233 : *
234 : * yawAngle: Yaw angle of the vehicle relative to true north in radians where a positive angle is
235 : * produced by a RH rotation about the Z body axis. The Yaw rotation is the first rotation in a
236 : * 321 (ZYX) or a 312 (ZXY) rotation sequence as specified by the 'type' argument.
237 : * yawAngleErr is the 1SD accuracy of the yaw angle measurement in radians.
238 : * timeStamp_ms: System time in msec when the yaw measurement was taken. This time stamp must include
239 : * all measurement lag and transmission delays.
240 : * type: An integer specifying Euler rotation order used to define the yaw angle.
241 : * type = 1 specifies a 312 (ZXY) rotation order, type = 2 specifies a 321 (ZYX) rotation order.
242 : */
243 : void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
244 :
245 : /*
246 : * Write position and quaternion data from an external navigation system
247 : *
248 : * pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
249 : * quat : quaternion desribing the rotation from navigation frame to body frame
250 : * posErr : 1-sigma spherical position error (m)
251 : * angErr : 1-sigma spherical angle error (rad)
252 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
253 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
254 : * resetTime_ms : system time of the last position reset request (mSec)
255 : *
256 : */
257 : void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
258 :
259 : /*
260 : * Write velocity data from an external navigation system
261 : *
262 : * vel : velocity in NED (m)
263 : * err : velocity error (m/s)
264 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
265 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
266 : */
267 : void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
268 :
269 : /*
270 : * Write terrain altitude (derived from SRTM) in meters above the origin
271 : * only used by optical flow when out of rangefinder range
272 : */
273 : void writeTerrainData(float alt_m);
274 :
275 : // Set to true if the terrain underneath is stable enough to be used as a height reference
276 : // in combination with a range finder. Set to false if the terrain underneath the vehicle
277 : // cannot be used as a height reference. Use to prevent range finder operation otherwise
278 : // enabled by the combination of EK3_RNG_USE_HGT and EK3_RNG_USE_SPD parameters.
279 : void setTerrainHgtStable(bool val);
280 :
281 : /*
282 : return the filter fault status as a bitmasked integer
283 : 0 = quaternions are NaN
284 : 1 = velocities are NaN
285 : 2 = badly conditioned X magnetometer fusion
286 : 3 = badly conditioned Y magnetometer fusion
287 : 4 = badly conditioned Z magnetometer fusion
288 : 5 = badly conditioned airspeed fusion
289 : 6 = badly conditioned synthetic sideslip fusion
290 : 7 = filter is not initialised
291 : */
292 : void getFilterFaults(uint16_t &faults) const;
293 :
294 : /*
295 : return filter status flags
296 : */
297 : void getFilterStatus(nav_filter_status &status) const;
298 :
299 : // return a terrain altitude variance
300 : bool getTerrainAltVariance(float &terrain_alt_variance) const;
301 :
302 : // provides the height limit to be observed by the control loops
303 : // returns false if no height limiting is required
304 : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
305 : bool getHeightControlLimit(float &height) const;
306 :
307 : // return a count of yaw reset events; incremented when the
308 : // primary core changes and when the primary core resets its yaw
309 : uint16_t getYawResetCount(void);
310 :
311 : // return a count of NE position reset events; incremented when the
312 : // primary core changes and when the primary core resets its NE position
313 : uint16_t getPosNorthEastResetCount(void);
314 :
315 : // return a count of D position reset events; incremented when the
316 : // primary core changes and when the primary core resets its D position
317 : uint16_t getPosDownResetCount(void);
318 :
319 : // set and save the _baroAltNoise parameter
320 82 : void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };
321 :
322 : // allow the enable flag to be set by Replay
323 0 : void set_enable(bool enable) { _enable.set_enable(enable); }
324 :
325 : // get the enable parameter
326 0 : bool get_enable(void) const { return bool(_enable.get()); }
327 :
328 : /*
329 : check if switching lanes will reduce the normalised
330 : innovations. This is called when the vehicle code is about to
331 : trigger an EKF failsafe, and it would like to avoid that by
332 : using a different EKF lane
333 : */
334 : void checkLaneSwitch(void);
335 :
336 : // switch to a new lane
337 : void switchLane(uint8_t new_lane_index);
338 :
339 : /*
340 : Request a reset of the EKF yaw. This is called when the vehicle code is about to
341 : trigger an EKF failsafe, and it would like to avoid that.
342 : */
343 : void requestYawReset(void);
344 :
345 : // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
346 : void setPosVelYawSourceSet(uint8_t source_set_idx);
347 :
348 : // write EKF information to on-board logs
349 : void Log_Write();
350 :
351 : // are we using (aka fusing) a non-compass yaw?
352 : bool using_noncompass_for_yaw() const;
353 :
354 : // are we using (aka fusing) external nav for yaw?
355 : bool using_extnav_for_yaw() const;
356 :
357 : // are we using a gps
358 : bool using_gps() const;
359 :
360 : // check if configured to use GPS for horizontal position estimation
361 : bool configuredToUseGPSForPosXY(void) const;
362 :
363 : // check if configured to use GPS for position estimation
364 : bool configuredToUseGPSForPos(void) const;
365 :
366 : // Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
367 : void writeDefaultAirSpeed(float airspeed, float uncertainty);
368 :
369 : // returns true when the yaw angle has been aligned
370 : bool yawAlignmentComplete(void) const;
371 :
372 : // returns true when the state estimates are significantly
373 : // degraded by vibration
374 : bool isVibrationAffected() const;
375 :
376 : // get a yaw estimator instance
377 : const EKFGSF_yaw *get_yawEstimator(void) const;
378 :
379 : // Do a reset and bootstrap alignment of all EKF cores
380 : // return true if successful for all cores
381 : bool InitialiseFilterBootstrap();
382 :
383 : private:
384 : class AP_DAL &dal;
385 :
386 : uint8_t num_cores; // number of allocated cores
387 : uint8_t primary; // current primary core
388 : NavEKF3_core *core = nullptr;
389 :
390 : uint32_t _frameTimeUsec; // time per IMU frame
391 : uint8_t _framesPerPrediction; // expected number of IMU frames per prediction
392 :
393 : // values for EK3_LOG_LEVEL
394 : enum class LogLevel {
395 : ALL = 0,
396 : XKF4 = 1,
397 : XKF4_GSF = 2,
398 : NONE = 3
399 : };
400 :
401 : // EKF Mavlink Tuneable Parameters
402 : AP_Int8 _enable; // zero to disable EKF3
403 : AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
404 : AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
405 : AP_Float _gpsHorizPosNoise; // GPS horizontal position measurement noise m
406 : AP_Float _baroAltNoise; // Baro height measurement noise : m
407 : AP_Float _magNoise; // magnetometer measurement noise : gauss
408 : AP_Float _easNoise; // equivalent airspeed measurement noise : m/s
409 : AP_Float _windVelProcessNoise; // wind velocity state process noise : m/s^2
410 : AP_Float _wndVarHgtRateScale; // scale factor applied to wind process noise due to height rate
411 : AP_Float _magEarthProcessNoise; // Earth magnetic field process noise : gauss/sec
412 : AP_Float _magBodyProcessNoise; // Body magnetic field process noise : gauss/sec
413 : AP_Float _gyrNoise; // gyro process noise : rad/s
414 : AP_Float _accNoise; // accelerometer process noise : m/s^2
415 : AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
416 : AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
417 : AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec)
418 : AP_Int16 _gpsVelInnovGate; // Percentage number of standard deviations applied to GPS velocity innovation consistency check
419 : AP_Int16 _gpsPosInnovGate; // Percentage number of standard deviations applied to GPS position innovation consistency check
420 : AP_Int16 _hgtInnovGate; // Percentage number of standard deviations applied to height innovation consistency check
421 : AP_Int16 _magInnovGate; // Percentage number of standard deviations applied to magnetometer innovation consistency check
422 : AP_Int16 _tasInnovGate; // Percentage number of standard deviations applied to true airspeed innovation consistency check
423 : AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration
424 : AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
425 : AP_Float _flowNoise; // optical flow rate measurement noise
426 : AP_Int16 _flowInnovGate; // Percentage number of standard deviations applied to optical flow innovation consistency check
427 : AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec)
428 : AP_Int16 _rngInnovGate; // Percentage number of standard deviations applied to range finder innovation consistency check
429 : AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
430 : AP_Float _rngNoise; // Range finder noise : m
431 : AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
432 : AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF3 for
433 : AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
434 : AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
435 : AP_Float _yawNoise; // magnetic yaw measurement noise : rad
436 : AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
437 : AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds)
438 : AP_Int8 _useRngSwHgt; // Maximum valid range of the range finder as a percentage of the maximum range specified by the sensor driver
439 : AP_Float _terrGradMax; // Maximum terrain gradient below the vehicle
440 : AP_Float _rngBcnNoise; // Range beacon measurement noise (m)
441 : AP_Int16 _rngBcnInnovGate; // Percentage number of standard deviations applied to range beacon innovation consistency check
442 : AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec)
443 : AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
444 : AP_Float _accBiasLim; // Accelerometer bias limit (m/s/s)
445 : AP_Int8 _magMask; // Bitmask forcing specific EKF core instances to use simple heading magnetometer fusion.
446 : AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
447 : AP_Float _visOdmVelErrMax; // Observation 1-STD velocity error assumed for visual odometry sensor at lowest reported quality (m/s)
448 : AP_Float _visOdmVelErrMin; // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)
449 : AP_Float _wencOdmVelErr; // Observation 1-STD velocity error assumed for wheel odometry sensor (m/s)
450 : AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
451 : AP_Float _hrt_filt_freq; // frequency of output observer height rate complementary filter in Hz
452 : AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field.
453 : AP_Int8 _gsfRunMask; // mask controlling which EKF3 instances run a separate EKF-GSF yaw estimator
454 : AP_Int8 _gsfUseMask; // mask controlling which EKF3 instances will use EKF-GSF yaw estimator data to assit with yaw resets
455 : AP_Int8 _gsfResetMaxCount; // maximum number of times the EKF3 is allowed to reset it's yaw to the EKF-GSF estimate
456 : AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError
457 : AP_Int32 _affinity; // bitmask of sensor affinity options
458 : AP_Float _dragObsNoise; // drag specific force observatoin noise (m/s/s)**2
459 : AP_Float _ballisticCoef_x; // ballistic coefficient measured for flow in X body frame directions
460 : AP_Float _ballisticCoef_y; // ballistic coefficient measured for flow in Y body frame directions
461 : AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient
462 : AP_Int8 _betaMask; // Bitmask controlling when sideslip angle fusion is used to estimate non wind states
463 : AP_Float _ognmTestScaleFactor; // Scale factor applied to the thresholds used by the on ground not moving test
464 : AP_Float _baroGndEffectDeadZone;// Dead zone applied to positive baro height innovations when in ground effect (m)
465 : AP_Int8 _primary_core; // initial core number
466 : AP_Enum<LogLevel> _log_level; // log verbosity level
467 : AP_Float _gpsVAccThreshold; // vertical accuracy threshold to use GPS as an altitude source
468 : AP_Int32 _options; // bit mask of processing options
469 :
470 : // enum for processing options
471 : enum class Option {
472 : JammingExpected = (1<<0),
473 : ManualLaneSwitch = (1<<1),
474 : OptflowMayUseTerrainAlt = (1<<2),
475 : AglKfForOptflow = (1<<3), // Use IMU-aided 2-state AGL KF for optflow scaling
476 : };
477 102936604 : bool option_is_enabled(Option option) const {
478 102936604 : return (_options & (uint32_t)option) != 0;
479 : }
480 :
481 : // Possible values for _flowUse
482 : #define FLOW_USE_NONE 0
483 : #define FLOW_USE_NAV 1
484 : #define FLOW_USE_TERRAIN 2
485 :
486 : // Tuning parameters
487 : const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
488 : const float gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
489 : const float gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
490 : const float extNavVelVarAccScale = 0.05f; // Scale factor applied to ext nav velocity measurement variance due to manoeuvre acceleration
491 : const uint16_t magDelay_ms = 60; // Magnetometer measurement delay (msec)
492 : const uint16_t tasDelay_ms = 100; // Airspeed measurement delay (msec)
493 : const uint16_t tiltDriftTimeMax_ms = 15000; // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)
494 : const uint16_t posRetryTimeUseVel_ms = 10000; // Position aiding retry time with velocity measurements (msec)
495 : const uint16_t posRetryTimeNoVel_ms = 7000; // Position aiding retry time without velocity measurements (msec)
496 : const uint16_t hgtRetryTimeMode0_ms = 10000; // Height retry time with vertical velocity measurement (msec)
497 : const uint16_t hgtRetryTimeMode12_ms = 5000; // Height retry time without vertical velocity measurement (msec)
498 : const uint16_t tasRetryTime_ms = 5000; // True airspeed timeout and retry interval (msec)
499 : const uint16_t dragFailTimeLimit_ms = 5000; // Drag timeout (msec)
500 : const uint32_t magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
501 : const float magVarRateScale = 0.005f; // scale factor applied to magnetometer variance due to angular rate
502 : const float gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground
503 : const uint16_t hgtAvg_ms = 100; // average number of msec between height measurements
504 : const uint16_t betaAvg_ms = 100; // average number of msec between synthetic sideslip measurements
505 : const float covTimeStepMax = 0.1f; // maximum time (sec) between covariance prediction updates
506 : const float covDelAngMax = 0.05f; // maximum delta angle between covariance prediction updates
507 : const float DCM33FlowMin = 0.71f; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
508 : const float fScaleFactorPnoise = 1e-10f; // Process noise added to focal length scale factor state variance at each time step
509 : const uint8_t flowTimeDeltaAvg_ms = 100; // average interval between optical flow measurements (msec)
510 : const uint32_t flowIntervalMax_ms = 100; // maximum allowable time between flow fusion events
511 : const float gndEffectBaroScaler = 4.0f; // scaler applied to the barometer observation variance when ground effect mode is active
512 : const uint8_t gndGradientSigma = 50; // RMS terrain gradient percentage assumed by the terrain height estimation
513 : const uint16_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec
514 : const uint8_t sensorIntervalMin_ms = 50; // The minimum allowed time between measurements from any non-IMU sensor (msec)
515 : const uint8_t flowIntervalMin_ms = 20; // The minimum allowed time between measurements from optical flow sensors (msec)
516 : const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec)
517 : const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
518 : const uint16_t deadReckonDeclare_ms = 1000; // Time without equivalent position or velocity observation to constrain drift before dead reckoning is declared (msec)
519 : const uint16_t gpsNoFixTimeout_ms = 2000; // Time without a fix required to reset GPS alignment checks when EK3_OPTIONS bit 0 is set (msec)
520 :
521 : // time at start of current filter update
522 : uint64_t imuSampleTime_us;
523 :
524 : // time of last lane switch
525 : uint32_t lastLaneSwitch_ms;
526 :
527 : // last time of Log_Write
528 : uint64_t lastLogWrite_us;
529 :
530 : struct {
531 : uint16_t count; // count of yaw reset events passed to consumers
532 : uint16_t last_core_count; // primary core's yaw reset count when count last changed
533 : } yaw_reset_data;
534 :
535 : struct {
536 : uint16_t count; // count of NE position reset events passed to consumers
537 : uint16_t last_core_count; // primary core's NE position reset count when count last changed
538 : } pos_reset_data;
539 :
540 : struct {
541 : uint16_t count; // count of D position reset events passed to consumers
542 : uint16_t last_core_count; // primary core's D position reset count when count last changed
543 : } pos_down_reset_data;
544 :
545 : #define CORE_ERR_LIM 1 // -LIM to LIM relative error range for a core
546 : #define BETTER_THRESH 0.5 // a lane should have this much relative error difference to be considered for overriding a healthy primary core
547 :
548 : bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
549 : bool coreSetupRequired[MAX_EKF_CORES]; // true when this core index needs to be setup
550 : uint8_t coreImuIndex[MAX_EKF_CORES]; // IMU index used by this core
551 : float coreRelativeErrors[MAX_EKF_CORES]; // relative errors of cores with respect to primary
552 : float coreErrorScores[MAX_EKF_CORES]; // the instance error values used to update relative core error
553 : uint64_t coreLastTimePrimary_us[MAX_EKF_CORES]; // last time we were using this core as primary
554 :
555 : // origin set by one of the cores
556 : Location common_EKF_origin;
557 : bool common_origin_valid;
558 :
559 : // update the yaw reset data to capture changes due to a lane switch
560 : // new_primary - index of the ekf instance that we are about to switch to as the primary
561 : // old_primary - index of the ekf instance that we are currently using as the primary
562 : void updateLaneSwitchYawResetData(uint8_t new_primary);
563 :
564 : // update the position reset data to capture changes due to a lane switch
565 : // new_primary - index of the ekf instance that we are about to switch to as the primary
566 : // old_primary - index of the ekf instance that we are currently using as the primary
567 : void updateLaneSwitchPosResetData(uint8_t new_primary);
568 :
569 : // update the position down reset data to capture changes due to a lane switch
570 : // new_primary - index of the ekf instance that we are about to switch to as the primary
571 : // old_primary - index of the ekf instance that we are currently using as the primary
572 : void updateLaneSwitchPosDownResetData(uint8_t new_primary);
573 :
574 : // Update instance error scores for all available cores
575 : float updateCoreErrorScores(void);
576 :
577 : // Update relative error scores for all alternate available cores
578 : void updateCoreRelativeErrors(void);
579 :
580 : // Reset error scores for all available cores
581 : void resetCoreErrors(void);
582 :
583 : // return true if a new core has a better score than an existing core, including
584 : // checks for alignment
585 : bool coreBetterScore(uint8_t new_core, uint8_t current_core) const;
586 :
587 : // position, velocity and yaw source control
588 : AP_NavEKF_Source sources;
589 : };
|
