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 :
23 : #if !defined(HAL_DEBUG_BUILD) || !HAL_DEBUG_BUILD
24 : #pragma GCC optimize("O2")
25 : #endif
26 :
27 : #include "AP_NavEKF3_feature.h"
28 : #include <AP_Common/Location.h>
29 : #include <AP_Math/AP_Math.h>
30 : #include <AP_Math/vectorN.h>
31 : #include <AP_NavEKF/AP_NavEKF_core_common.h>
32 : #include <AP_NavEKF/AP_NavEKF_Source.h>
33 : #include <AP_NavEKF/EKF_Buffer.h>
34 : #include <AP_InertialSensor/AP_InertialSensor.h>
35 : #include <AP_RangeFinder/AP_RangeFinder.h>
36 :
37 : #include "AP_NavEKF/EKFGSF_yaw.h"
38 :
39 : // GPS pre-flight check bit locations
40 : #define MASK_GPS_NSATS (1<<0)
41 : #define MASK_GPS_HDOP (1<<1)
42 : #define MASK_GPS_SPD_ERR (1<<2)
43 : #define MASK_GPS_POS_ERR (1<<3)
44 : #define MASK_GPS_YAW_ERR (1<<4)
45 : #define MASK_GPS_POS_DRIFT (1<<5)
46 : #define MASK_GPS_VERT_SPD (1<<6)
47 : #define MASK_GPS_HORIZ_SPD (1<<7)
48 :
49 : #define earthRate 0.000072921f // earth rotation rate (rad/sec)
50 :
51 : // initial accel bias uncertainty as a fraction of the state limit
52 : #define ACCEL_BIAS_LIM_SCALER 0.2f
53 :
54 : // target update time for the EKF in msec and sec
55 : #define EKF_TARGET_DT_MS 12
56 : #define EKF_TARGET_DT 0.012f
57 :
58 : // mag fusion final reset altitude (using NED frame so altitude is negative)
59 : #define EKF3_MAG_FINAL_RESET_ALT 2.5f
60 : #define EKF3_MAG_FINAL_RESET_ALT_SUB 0.5f
61 :
62 : // learning rate for mag biases when using GPS yaw
63 : #define EK3_GPS_MAG_LEARN_RATE 0.005f
64 :
65 : // learning limit for mag biases when using GPS yaw (Gauss)
66 : #define EK3_GPS_MAG_LEARN_LIMIT 0.02f
67 :
68 : // maximum number of yaw resets due to detected magnetic anomaly allowed per flight
69 : #define MAG_ANOMALY_RESET_MAX 2
70 :
71 : // number of seconds a request to reset the yaw to the GSF estimate is active before it times out
72 : #define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
73 :
74 : // accuracy threshold applied to GSF yaw estimate use
75 : #define GSF_YAW_ACCURACY_THRESHOLD_DEG 15.0f
76 :
77 : // number of continuous valid GSF yaw estimates required to confirm valid hostory
78 : #define GSF_YAW_VALID_HISTORY_THRESHOLD 5
79 :
80 : // minimum variances allowed for velocity and position states
81 : #define VEL_STATE_MIN_VARIANCE 1E-4
82 : #define POS_STATE_MIN_VARIANCE 1E-4
83 :
84 : // maximum number of times the vertical velocity variance can hit the lower limit before the
85 : // associated states, variances and covariances are reset
86 : #define EKF_TARGET_RATE_HZ uint32_t(1.0 / EKF_TARGET_DT)
87 : #define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ)
88 :
89 : // limit on horizontal position states
90 : #if HAL_WITH_EKF_DOUBLE
91 : #define EK3_POSXY_STATE_LIMIT 50.0e6
92 : #else
93 : #define EK3_POSXY_STATE_LIMIT 1.0e6
94 : #endif
95 :
96 : // IMU acceleration process noise in m/s/s used when bad vibration affected IMU accel is detected
97 : #define BAD_IMU_DATA_ACC_P_NSE 5.0f
98 :
99 : // Number of milliseconds of bad IMU data before a reset to vertical position and velocity height sources is performed
100 : #define BAD_IMU_DATA_TIMEOUT_MS 1000
101 :
102 : // number of milliseconds the bad IMU data response settings will be held after the last bad IMU data is detected
103 : #define BAD_IMU_DATA_HOLD_MS 10000
104 :
105 : // wind state variance limits
106 : #define WIND_VEL_VARIANCE_MAX 400.0f
107 : #define WIND_VEL_VARIANCE_MIN 0.25f
108 :
109 : // maximum number of downward facing rangefinder instances available
110 : #if EK3_FEATURE_RANGEFINDER_MEASUREMENTS
111 : #if RANGEFINDER_MAX_INSTANCES > 1
112 : #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 2
113 : #else
114 : #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 1
115 : #endif
116 : #endif
117 :
118 : // number of continuous valid GPS velocity samples required to reset yaw
119 : #define GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD 5
120 :
121 : // maximum GPs ground course uncertainty allowed for yaw alignment (deg)
122 : #define GPS_VEL_YAW_ALIGN_MAX_ANG_ERR 15.0F
123 :
124 : class NavEKF3_core : public NavEKF_core_common
125 : {
126 : public:
127 : // Constructor
128 : NavEKF3_core(class NavEKF3 *_frontend, class AP_DAL &dal);
129 :
130 : // setup this core backend
131 : bool setup_core(uint8_t _imu_index, uint8_t _core_index);
132 :
133 : // Initialise the states from accelerometer and magnetometer data (if present)
134 : // This method can only be used when the vehicle is static
135 : bool InitialiseFilterBootstrap(void);
136 :
137 : // Update Filter States - this should be called whenever new IMU data is available
138 : // The predict flag is set true when a new prediction cycle can be started
139 : void UpdateFilter(bool predict);
140 :
141 : // Check basic filter health metrics and return a consolidated health status
142 : bool healthy(void) const;
143 :
144 : // Return a consolidated error score where higher numbers are less healthy
145 : // Intended to be used by the front-end to determine which is the primary EKF
146 : float errorScore(void) const;
147 :
148 : // Write the last calculated NE position relative to the reference point (m).
149 : // If a calculated solution is not available, use the best available data and return false
150 : // If false returned, do not use for flight control
151 : bool getPosNE(Vector2p &posNE) const;
152 :
153 : // get position D from local origin
154 : bool getPosD_local(postype_t &posD) const;
155 :
156 : // Write the last calculated D position relative to the public origin
157 : // If a calculated solution is not available, use the best available data and return false
158 : // If false returned, do not use for flight control
159 : bool getPosD(postype_t &posD) const;
160 :
161 : // return NED velocity in m/s
162 : void getVelNED(Vector3f &vel) const;
163 :
164 : // return estimate of true airspeed vector in body frame in m/s
165 : // returns false if estimate is unavailable
166 : bool getAirSpdVec(Vector3f &vel) const;
167 :
168 : // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed
169 : // returns false if the data is unavailable
170 : bool getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const;
171 :
172 : // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
173 : // 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
174 : // but will always be kinematically consistent with the z component of the EKF position state
175 : float getPosDownDerivative(void) const;
176 :
177 : // return body axis gyro bias estimates in rad/sec
178 : void getGyroBias(Vector3f &gyroBias) const;
179 :
180 : // return accelerometer bias in m/s/s
181 : void getAccelBias(Vector3f &accelBias) const;
182 :
183 : // reset body axis gyro bias estimates
184 : void resetGyroBias(void);
185 :
186 : // Resets the baro so that it reads zero at the current height
187 : // Resets the EKF height to zero
188 : // Adjusts the EKF origin height so that the EKF height + origin height is the same as before
189 : // Returns true if the height datum reset has been performed
190 : // If using a range finder for height no reset is performed and it returns false
191 : bool resetHeightDatum(void);
192 :
193 : // return the horizontal speed limit in m/s set by optical flow sensor limits
194 : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
195 : void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
196 :
197 : // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
198 : // returns true if wind state estimation is active
199 : bool getWind(Vector3f &wind) const;
200 :
201 : // return earth magnetic field estimates in measurement units / 1000
202 : void getMagNED(Vector3f &magNED) const;
203 :
204 : // return body magnetic field estimates in measurement units / 1000
205 : void getMagXYZ(Vector3f &magXYZ) const;
206 :
207 : // return the index for the active sensors
208 : uint8_t getActiveAirspeed() const;
209 :
210 : // Return estimated magnetometer offsets
211 : // Return true if magnetometer offsets are valid
212 : bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
213 :
214 : // Return the last calculated latitude, longitude and height in WGS-84
215 : // If a calculated location isn't available and position source is GPS, return a raw GPS measurement
216 : // The status will return true if a calculation or raw measurement is available
217 : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
218 : bool getLLH(Location &loc) const;
219 :
220 : // return the latitude and longitude and height used to set the NED origin
221 : // All NED positions calculated by the filter are relative to this location
222 : // Returns false if the origin has not been set
223 : bool getOriginLLH(Location &loc) const;
224 :
225 : // set the latitude and longitude and height used to set the NED origin
226 : // All NED positions calculated by the filter will be relative to this location
227 : // returns false if the origin has already been set
228 : bool setOriginLLH(const Location &loc);
229 :
230 : // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty
231 : // The altitude element of the location is not used.
232 : // Returns true if the set was successful
233 : bool setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
234 :
235 : // Popoluates the WMM data structure with the field at the given location
236 : void setEarthFieldFromLocation(const Location &loc);
237 :
238 : // return estimated height above ground level
239 : // return false if ground height is not being estimated.
240 : bool getHAGL(float &HAGL) const;
241 :
242 : // return the Euler roll, pitch and yaw angle in radians
243 : void getEulerAngles(Vector3f &eulers) const;
244 :
245 : // return the transformation matrix from XYZ (body) to NED axes
246 : void getRotationBodyToNED(Matrix3f &mat) const;
247 :
248 : // return the quaternions defining the rotation from NED to XYZ (body) axes
249 : void getQuaternion(Quaternion &quat) const;
250 :
251 : // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
252 : bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
253 :
254 : // return the synthetic air data drag and sideslip innovations
255 : void getSynthAirDataInnovations(Vector2f &dragInnov, float &betaInnov) const;
256 :
257 : // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
258 : bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
259 :
260 : // return 1-sigma position and velocity uncertainty derived from the EKF state error covariance matrix P
261 : // pos_horiz_m: 2D RMS horizontal position uncertainty (m)
262 : // pos_vert_m: 1-sigma vertical position uncertainty (m)
263 : // vel_m_s: 1-sigma worst-case NED velocity uncertainty (m/s)
264 : // returns false if EKF is not yet initialised
265 : bool getPosVelUncertainty(float &pos_horiz_m, float &pos_vert_m, float &vel_m_s) const;
266 :
267 : // get a particular source's velocity innovations
268 : // returns true on success and results are placed in innovations and variances arguments
269 : bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
270 :
271 : // should we use the compass? This is public so it can be used for
272 : // reporting via ahrs.use_compass()
273 : bool use_compass(void) const;
274 :
275 : // write the raw optical flow measurements
276 : // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
277 : // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
278 : // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
279 : // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
280 : // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
281 : // posOffset is the XYZ flow sensor position in the body frame in m
282 : // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
283 : void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
284 :
285 : // retrieve latest corrected optical flow samples (used for calibration)
286 : bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
287 :
288 : /*
289 : * Write body frame linear and angular displacement measurements from a visual odometry sensor
290 : *
291 : * quality is a normalised confidence value from 0 to 100
292 : * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at time_ms (m)
293 : * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad)
294 : * delTime is the time interval for the measurement of delPos and delAng (sec)
295 : * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
296 : * delay_ms is the average delay of external nav system measurements relative to inertial measurements
297 : * posOffset is the XYZ body frame position of the camera focal point (m)
298 : */
299 : void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
300 :
301 : /*
302 : * Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis
303 : *
304 : * 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)
305 : * delTime is the time interval for the measurement of delAng (sec)
306 : * timeStamp_ms is the time when the rotation was last measured (msec)
307 : * posOffset is the XYZ body frame position of the wheel hub (m)
308 : * radius is the effective rolling radius of the wheel (m)
309 : */
310 : void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
311 :
312 : /*
313 : * Return data for debugging body frame odometry fusion:
314 : *
315 : * velInnov are the XYZ body frame velocity innovations (m/s)
316 : * velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
317 : *
318 : * Return the time stamp of the last odometry fusion update (msec)
319 : */
320 : uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar);
321 :
322 : /*
323 : * Writes the measurement from a yaw angle sensor
324 : *
325 : * yawAngle: Yaw angle of the vehicle relative to true north in radians where a positive angle is
326 : * produced by a RH rotation about the Z body axis. The Yaw rotation is the first rotation in a
327 : * 321 (ZYX) or a 312 (ZXY) rotation sequence as specified by the 'type' argument.
328 : * yawAngleErr is the 1SD accuracy of the yaw angle measurement in radians.
329 : * timeStamp_ms: System time in msec when the yaw measurement was taken. This time stamp must include
330 : * all measurement lag and transmission delays.
331 : * type: An integer specifying Euler rotation order used to define the yaw angle.
332 : * type = 1 specifies a 312 (ZXY) rotation order, type = 2 specifies a 321 (ZYX) rotation order.
333 : * antOffset: body-frame antenna offset the yaw angle was calculated from assuming the vehicle was
334 : * level, zero (the default) when the measurement does not require attitude correction (m)
335 : */
336 : void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type, const Vector3f &antOffset=Vector3f());
337 :
338 : /*
339 : * Write position and quaternion data from an external navigation system
340 : *
341 : * pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
342 : * quat : quaternion desribing the rotation from navigation frame to body frame
343 : * posErr : 1-sigma spherical position error (m)
344 : * angErr : 1-sigma spherical angle error (rad)
345 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
346 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
347 : * resetTime_ms : system time of the last position reset request (mSec)
348 : *
349 : */
350 : void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
351 :
352 : /*
353 : * Write velocity data from an external navigation system
354 : *
355 : * vel : velocity in NED (m)
356 : * err : velocity error (m/s)
357 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
358 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
359 : */
360 : void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
361 :
362 : /*
363 : * Write terrain altitude (derived from SRTM) in meters above the origin
364 : * only used by optical flow when out of rangefinder range
365 : */
366 : #if EK3_FEATURE_OPTFLOW_SRTM
367 : void writeTerrainData(float alt_m);
368 : #endif
369 :
370 : // Set to true if the terrain underneath is stable enough to be used as a height reference
371 : // in combination with a range finder. Set to false if the terrain underneath the vehicle
372 : // cannot be used as a height reference. Use to prevent range finder operation otherwise
373 : // enabled by the combination of EK3_RNG_USE_HGT and EK3_RNG_USE_SPD parameters.
374 : void setTerrainHgtStable(bool val);
375 :
376 : /*
377 : return the filter fault status as a bitmasked integer
378 : 0 = quaternions are NaN
379 : 1 = velocities are NaN
380 : 2 = badly conditioned X magnetometer fusion
381 : 3 = badly conditioned Y magnetometer fusion
382 : 5 = badly conditioned Z magnetometer fusion
383 : 6 = badly conditioned airspeed fusion
384 : 7 = badly conditioned synthetic sideslip fusion
385 : 7 = filter is not initialised
386 : */
387 : void getFilterFaults(uint16_t &faults) const;
388 :
389 : /*
390 : Return a filter function status that indicates:
391 : Which outputs are valid
392 : If the filter has detected takeoff
393 : If the filter has activated the mode that mitigates against ground effect static pressure errors
394 : If GPS data is being used
395 : */
396 : void getFilterStatus(nav_filter_status &status) const;
397 :
398 : // return a terrain altitude variance
399 : bool getTerrainAltVariance(float &terrain_alt_variance) const;
400 :
401 : // provides the height limit to be observed by the control loops
402 : // returns false if no height limiting is required
403 : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
404 : bool getHeightControlLimit(float &height) const;
405 :
406 : // return the number of yaw resets performed by this core
407 39298429 : uint16_t getYawResetCount(void) const { return yawResetCount; }
408 :
409 : // return the number of NE position resets performed by this core
410 39298429 : uint16_t getPosNorthEastResetCount(void) const { return posNEResetCount; }
411 :
412 : // return the number of D position resets performed by this core
413 39298429 : uint16_t getPosDownResetCount(void) const { return posDResetCount; }
414 :
415 : // report any reason for why the backend is refusing to initialise
416 : const char *prearm_failure_reason(void) const;
417 :
418 : // report the number of frames lapsed since the last state prediction
419 : // this is used by other instances to level load
420 : uint8_t getFramesSincePredict(void) const;
421 :
422 : // get the IMU index. For now we return the gyro index, as that is most
423 : // critical for use by other subsystems.
424 80106038 : uint8_t getIMUIndex(void) const { return gyro_index_active; }
425 :
426 : // values for EK3_MAG_CAL
427 : enum class MagCal {
428 : WHEN_FLYING = 0,
429 : WHEN_MANOEUVRING = 1,
430 : NEVER = 2,
431 : AFTER_FIRST_CLIMB = 3,
432 : ALWAYS = 4,
433 : // 5 was EXTERNAL_YAW (do not use)
434 : // 6 was EXTERNAL_YAW_FALLBACK (do not use)
435 : GROUND_AND_INFLIGHT = 7,
436 : };
437 :
438 : // magnetometer fusion selections
439 : enum class MagFuseSel {
440 : NOT_FUSING = 0,
441 : FUSE_YAW = 1,
442 : FUSE_MAG = 2,
443 : FUSE_MAG_ANCHORED = 3
444 : };
445 :
446 : // are we using (aka fusing) a non-compass yaw?
447 : bool using_noncompass_for_yaw(void) const;
448 :
449 : // are we using (aka fusing) external nav for yaw?
450 : bool using_extnav_for_yaw() const;
451 :
452 : // are we using a gps?
453 : bool using_gps() const;
454 :
455 : // 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.
456 : void writeDefaultAirSpeed(float airspeed, float uncertainty);
457 :
458 : // request a reset the yaw to the EKF-GSF value
459 : void EKFGSF_requestYawReset();
460 :
461 : // return true if we are tilt aligned
462 53084008 : bool have_aligned_tilt(void) const {
463 53084008 : return tiltAlignComplete;
464 : }
465 :
466 : // return true if we are yaw aligned
467 53209147 : bool have_aligned_yaw(void) const {
468 53209147 : return yawAlignComplete;
469 : }
470 :
471 : void Log_Write(uint64_t time_us);
472 :
473 : // returns true when the state estimates are significantly degraded by vibration
474 40053019 : bool isVibrationAffected() const { return badIMUdata; }
475 :
476 : // get a yaw estimator instance
477 479 : const EKFGSF_yaw *get_yawEstimator(void) const { return yawEstimator; }
478 :
479 : // per-core pre-arm checks. returns false if we fail arming
480 : // checks, in which case the buffer will be populated with a
481 : // failure message
482 : // requires_position should be true if horizontal position configuration should be checked
483 : bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
484 :
485 : // clear the statesInitialised status which allows a reset and bootstrap alignment
486 2 : void clearStatesInitialised(void) { statesInitialised = false; }
487 :
488 : // return true if states have been initialised by a bootstrap alignment
489 2 : bool isStatesInitialised(void) const { return statesInitialised; }
490 :
491 : private:
492 : EKFGSF_yaw *yawEstimator;
493 : AP_DAL &dal;
494 :
495 : // Reference to the global EKF frontend for parameters
496 : class NavEKF3 *frontend;
497 : uint8_t imu_index; // preferred IMU index
498 : uint8_t gyro_index_active; // active gyro index (in case preferred fails)
499 : uint8_t accel_index_active; // active accel index (in case preferred fails)
500 : uint8_t core_index;
501 : uint8_t imu_buffer_length;
502 : uint8_t obs_buffer_length;
503 :
504 : #if MATH_CHECK_INDEXES
505 : class Vector9 : public VectorN<ftype, 9> {
506 : public:
507 : Vector9(ftype p0, ftype p1, ftype p2,
508 : ftype p3, ftype p4, ftype p5,
509 : ftype p6, ftype p7, ftype p8) {
510 : _v[0] = p0; _v[1] = p1; _v[2] = p2;
511 : _v[3] = p3; _v[4] = p4; _v[5] = p5;
512 : _v[6] = p6; _v[7] = p7; _v[8] = p8;
513 : }
514 : };
515 : class Vector5 : public VectorN<ftype, 5> {
516 : public:
517 : Vector5(ftype p0, ftype p1, ftype p2,
518 : ftype p3, ftype p4) {
519 : _v[0] = p0; _v[1] = p1; _v[2] = p2;
520 : _v[3] = p3; _v[4] = p4;
521 : }
522 : };
523 :
524 : typedef VectorN<ftype,2> Vector2;
525 : typedef VectorN<ftype,3> Vector3;
526 : typedef VectorN<ftype,4> Vector4;
527 : typedef VectorN<ftype,6> Vector6;
528 : typedef VectorN<ftype,7> Vector7;
529 : typedef VectorN<ftype,8> Vector8;
530 : typedef VectorN<ftype,10> Vector10;
531 : typedef VectorN<ftype,11> Vector11;
532 : typedef VectorN<ftype,13> Vector13;
533 : typedef VectorN<ftype,14> Vector14;
534 : typedef VectorN<ftype,15> Vector15;
535 : typedef VectorN<ftype,21> Vector21;
536 : typedef VectorN<ftype,22> Vector22;
537 : typedef VectorN<ftype,23> Vector23;
538 : typedef VectorN<ftype,24> Vector24;
539 : typedef VectorN<ftype,25> Vector25;
540 : typedef VectorN<ftype,31> Vector31;
541 : typedef VectorN<VectorN<ftype,3>,3> Matrix3;
542 : typedef VectorN<VectorN<ftype,24>,24> Matrix24;
543 : typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
544 : typedef VectorN<uint32_t,50> Vector_u32_50;
545 : #else
546 : typedef ftype Vector2[2];
547 : typedef ftype Vector3[3];
548 : typedef ftype Vector4[4];
549 : typedef ftype Vector5[5];
550 : typedef ftype Vector6[6];
551 : typedef ftype Vector7[7];
552 : typedef ftype Vector8[8];
553 : typedef ftype Vector9[9];
554 : typedef ftype Vector10[10];
555 : typedef ftype Vector11[11];
556 : typedef ftype Vector13[13];
557 : typedef ftype Vector14[14];
558 : typedef ftype Vector15[15];
559 : typedef ftype Vector21[21];
560 : typedef ftype Vector22[22];
561 : typedef ftype Vector23[23];
562 : typedef ftype Vector24[24];
563 : typedef ftype Vector25[25];
564 : typedef ftype Matrix3[3][3];
565 : typedef ftype Matrix24[24][24];
566 : typedef ftype Matrix34_50[34][50];
567 : typedef uint32_t Vector_u32_50[50];
568 : #endif
569 :
570 : // the states are available in two forms, either as a Vector24, or
571 : // broken down as individual elements. Both are equivalent (same
572 : // memory)
573 : struct state_elements {
574 : QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame 0..3
575 : Vector3F velocity; // velocity of IMU in local NED earth frame (m/sec) 4..6
576 : Vector3F position; // position of IMU in local NED earth frame (m) 7..9
577 : Vector3F gyro_bias; // body frame delta angle IMU bias vector (rad) 10..12
578 : Vector3F accel_bias; // body frame delta velocity IMU bias vector (m/sec) 13..15
579 : Vector3F earth_magfield; // earth frame magnetic field vector (Gauss) 16..18
580 : Vector3F body_magfield; // body frame magnetic field vector (Gauss) 19..21
581 : Vector2F wind_vel; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
582 : };
583 :
584 : union {
585 : Vector24 statesArray;
586 : struct state_elements stateStruct;
587 : };
588 :
589 : struct output_elements {
590 : QuaternionF quat; // quaternion defining rotation from local NED earth frame to body frame
591 : Vector3F velocity; // velocity of body frame origin in local NED earth frame (m/sec)
592 : Vector3F position; // position of body frame origin in local NED earth frame (m)
593 : };
594 :
595 : struct imu_elements {
596 : Vector3F delAng; // IMU delta angle measurements in body frame (rad)
597 : Vector3F delVel; // IMU delta velocity measurements in body frame (m/sec)
598 : ftype delAngDT; // time interval over which delAng has been measured (sec)
599 : ftype delVelDT; // time interval over which delVelDT has been measured (sec)
600 : uint32_t time_ms; // measurement timestamp (msec)
601 : uint8_t gyro_index;
602 : uint8_t accel_index;
603 : };
604 :
605 : struct gps_elements : EKF_obs_element_t {
606 : int32_t lat, lng; // latitude and longitude in 1e7 degrees
607 : ftype hgt; // height of the GPS antenna in local NED earth frame (m)
608 : Vector3F vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
609 : uint8_t sensor_idx; // unique integer identifying the GPS sensor
610 : bool corrected; // true when the position and velocity have been corrected for sensor position
611 : bool have_vz; // true when vertical velocity is valid
612 : };
613 :
614 : struct mag_elements : EKF_obs_element_t {
615 : Vector3F mag; // body frame magnetic field measurements (Gauss)
616 : };
617 :
618 : struct baro_elements : EKF_obs_element_t {
619 : ftype hgt; // height of the pressure sensor in local NED earth frame (m)
620 : };
621 :
622 : struct range_elements : EKF_obs_element_t {
623 : ftype rng; // distance measured by the range sensor (m)
624 : uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors
625 : };
626 :
627 : struct rng_bcn_elements : EKF_obs_element_t {
628 : ftype rng; // range measurement to each beacon (m)
629 : Vector3F beacon_posNED; // NED position of the beacon (m)
630 : ftype rngErr; // range measurement error 1-std (m)
631 : uint8_t beacon_ID; // beacon identification number
632 : };
633 :
634 : struct tas_elements : EKF_obs_element_t {
635 : ftype tas; // true airspeed measurement (m/sec)
636 : ftype tasVariance; // variance of true airspeed measurement (m/sec)^2
637 : bool allowFusion; // true if measurement can be allowed to modify EKF states.
638 : };
639 :
640 : struct of_elements : EKF_obs_element_t {
641 : Vector2F flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
642 : Vector2F flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
643 : Vector3F bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
644 : Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m)
645 : float heightOverride; // The fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
646 : };
647 :
648 : struct vel_odm_elements : EKF_obs_element_t {
649 : Vector3F vel; // XYZ velocity measured in body frame (m/s)
650 : ftype velErr; // velocity measurement error 1-std (m/s)
651 : Vector3F body_offset;// XYZ position of the velocity sensor in body frame (m)
652 : Vector3F angRate; // angular rate estimated from odometry (rad/sec)
653 : };
654 :
655 : struct wheel_odm_elements : EKF_obs_element_t {
656 : ftype delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
657 : ftype radius; // wheel radius (m)
658 : Vector3F hub_offset; // XYZ position of the wheel hub in body frame (m)
659 : ftype delTime; // time interval that the measurement was accumulated over (sec)
660 : };
661 :
662 : // Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available
663 : enum class rotationOrder {
664 : TAIT_BRYAN_321=0,
665 : TAIT_BRYAN_312=1
666 : };
667 :
668 : struct yaw_elements : EKF_obs_element_t {
669 : ftype yawAng; // yaw angle measurement (rad)
670 : ftype yawAngErr; // yaw angle 1SD measurement accuracy (rad)
671 : rotationOrder order; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY)
672 : #if EK3_FEATURE_MOVING_BASELINE
673 : Vector3F antOffset; // body-frame antenna offset the yaw measurement was calculated from assuming the
674 : // vehicle was level, zero when the measurement does not require attitude correction (m)
675 : #endif
676 : };
677 :
678 : struct ext_nav_elements : EKF_obs_element_t {
679 : Vector3F pos; // XYZ position measured in a RH navigation frame (m)
680 : ftype posErr; // spherical position measurement error 1-std (m)
681 : bool posReset; // true when the position measurement has been reset
682 : bool corrected; // true when the position has been corrected for sensor position
683 : };
684 :
685 : struct ext_nav_vel_elements : EKF_obs_element_t {
686 : Vector3F vel; // velocity in NED (m/s)
687 : ftype err; // velocity measurement error (m/s)
688 : bool corrected; // true when the velocity has been corrected for sensor position
689 : };
690 :
691 : struct drag_elements : EKF_obs_element_t {
692 : Vector2f accelXY; // measured specific force along the X and Y body axes (m/sec**2)
693 : };
694 :
695 : // bias estimates for the IMUs that are enabled but not being used
696 : // by this core.
697 : struct {
698 : Vector3F gyro_bias;
699 : Vector3F accel_bias;
700 : } inactiveBias[INS_MAX_INSTANCES];
701 :
702 : // Specify source of data to be used for a partial state reset
703 : // Checking the availability and quality of the data source specified is the responsibility of the caller
704 : enum class resetDataSource {
705 : DEFAULT=0, // Use data source selected by reset function internal rules
706 : GPS=1, // Use GPS
707 : RNGBCN=2, // Use beacon range data
708 : FLOW=3, // Use optical flow rates
709 : BARO=4, // Use Baro height
710 : MAG=5, // Use magnetometer data
711 : RNGFND=6, // Use rangefinder data
712 : EXTNAV=7 // Use external nav data
713 : };
714 :
715 : // specifies the method to be used when fusing yaw observations
716 : enum class yawFusionMethod {
717 : MAGNETOMETER=0,
718 : GPS=1,
719 : GSF=2,
720 : STATIC=3,
721 : PREDICTED=4,
722 : EXTNAV=5,
723 : };
724 :
725 : // update the navigation filter status
726 : void updateFilterStatus(void);
727 :
728 : // update the quaternion, velocity and position states using IMU measurements
729 : void UpdateStrapdownEquationsNED();
730 :
731 : // calculate the predicted state covariance matrix
732 : // Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states
733 : // used to perform a reset of the quaternion state covariances only. Set to null for normal operation.
734 : void CovariancePrediction(Vector3F *rotVarVecPtr);
735 :
736 : // constrain variances (diagonal terms) in the state covariance matrix
737 : void ConstrainVariances();
738 :
739 : // actually do fusion to update statesArray from Kfusion and P from KHP.
740 : // returns true and skips fusion if variances would be driven negative.
741 : // force skips this negative check; passing true is probably a bug!
742 : bool FinishFusion(ftype innov, bool force = false);
743 :
744 : // constrain states
745 : void ConstrainStates();
746 :
747 : // constrain earth field using WMM tables
748 : void MagTableConstrain(void);
749 :
750 : // fuse selected position, velocity and height measurements
751 : void FuseVelPosNED();
752 :
753 : // fuse body frame velocity measurements
754 : void FuseBodyVel();
755 :
756 : #if EK3_FEATURE_BEACON_FUSION
757 : // fuse range beacon measurements
758 : void FuseRngBcn();
759 : #endif
760 :
761 : // use range beacon measurements to calculate a static position
762 : void FuseRngBcnStatic();
763 :
764 : // calculate the offset from EKF vertical position datum to the range beacon system datum
765 : void CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning);
766 :
767 : // fuse magnetometer measurements
768 : void FuseMagnetometer();
769 :
770 : // fuse true airspeed measurements
771 : void FuseAirspeed();
772 :
773 : // fuse synthetic sideslip measurement of zero
774 : void FuseSideslip();
775 :
776 : // zero specified state variances and covariances in state covariance matrix
777 : void zeroStatesVarCov(uint8_t first, uint8_t last);
778 :
779 : // Reset the stored output history to current data
780 : void StoreOutputReset(void);
781 :
782 : // Reset the stored output quaternion history to current EKF state
783 : void StoreQuatReset(void);
784 :
785 : // Rotate the stored output quaternion history through a quaternion rotation
786 : void StoreQuatRotate(const QuaternionF &deltaQuat);
787 :
788 : // calculate the NED earth spin vector in rad/sec
789 : void calcEarthRateNED(Vector3F &omega, int32_t latitude) const;
790 :
791 : // initialise the covariance matrix
792 : void CovarianceInit();
793 :
794 : // helper functions for readIMUData
795 : bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
796 : bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
797 :
798 : // helper functions for correcting IMU data
799 : void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
800 : void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
801 :
802 : // update IMU delta angle and delta velocity measurements
803 : void readIMUData(bool startPredictEnabled);
804 :
805 : // update estimate of inactive bias states
806 : void learnInactiveBiases();
807 :
808 : // check for new valid GPS data and update stored measurement if available
809 : void readGpsData();
810 :
811 : // check for new valid GPS yaw data
812 : void readGpsYawData();
813 :
814 : // check for new altitude measurement data and update stored measurement if available
815 : void readBaroData();
816 :
817 : // check for new magnetometer data and update store measurements if available
818 : void readMagData();
819 :
820 : // try changing compasses on compass failure or timeout
821 : void tryChangeCompass(void);
822 :
823 : // try changing to a specific compass index
824 : void tryChangeCompass(uint8_t compass_index);
825 :
826 : // check for new airspeed data and update stored measurements if available
827 : void readAirSpdData();
828 :
829 : #if EK3_FEATURE_BEACON_FUSION
830 : // check for new range beacon data and update stored measurements if available
831 : void readRngBcnData();
832 : #endif
833 :
834 : // determine when to perform fusion of GPS position and velocity measurements
835 : void SelectVelPosFusion();
836 :
837 : #if EK3_FEATURE_BEACON_FUSION
838 : // determine when to perform fusion of range measurements take relative to a beacon at a known NED position
839 : void SelectRngBcnFusion();
840 : #endif
841 :
842 : // determine when to perform fusion of magnetometer measurements
843 : void SelectMagFusion();
844 :
845 : // determine when to perform fusion of true airspeed measurements
846 : void SelectTasFusion();
847 :
848 : // determine when to perform fusion of drag or synthetic sideslip measurements
849 : void SelectBetaDragFusion();
850 :
851 : // force alignment of the yaw angle using GPS velocity data
852 : void realignYawGPS(bool emergency_reset);
853 :
854 : // initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
855 :
856 : // align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
857 : void alignYawAngle(const yaw_elements &yawAngData);
858 :
859 : // build the body-to-earth rotation matrix at the current state attitude with yaw
860 : // set to zero, using the given Euler rotation order. Optionally returns the yaw
861 : // angle removed. Returns false if the rotation order is not supported
862 : bool buildTbnZeroYaw(rotationOrder order, Matrix3F &Tbn, ftype *yawAng=nullptr) const;
863 :
864 : #if EK3_FEATURE_MOVING_BASELINE
865 : // correct a yaw measurement calculated from a moving baseline antenna offset for
866 : // vehicle attitude using this core's attitude estimate at the fusion time horizon,
867 : // inflating yawAngErr by the attitude uncertainty the correction introduces.
868 : // returns false when the baseline is too close to vertical at the estimated
869 : // attitude for the measurement to contain usable yaw information, or when the
870 : // rotation order is not supported
871 : bool correctGPSYawForAntennaOffset(yaw_elements &yawAngData) const;
872 : #endif // EK3_FEATURE_MOVING_BASELINE
873 :
874 : // update mag field states and associated variances using magnetomer and declination data
875 : void resetMagFieldStates();
876 :
877 : // reset yaw based on magnetic field sample
878 : void setYawFromMag();
879 :
880 : // zero stored variables
881 : void InitialiseVariables();
882 :
883 : // zero stored variables related to mag
884 : void InitialiseVariablesMag();
885 :
886 : // reset the horizontal position states uing the last GPS measurement
887 : void ResetPosition(resetDataSource posResetSource);
888 :
889 : // reset the stateStruct's NE position to the specified position
890 : void ResetPositionNE(ftype posN, ftype posE);
891 :
892 : // reset the stateStruct's D position
893 : void ResetPositionD(ftype posD);
894 :
895 : // reset velocity states using the last GPS measurement
896 : void ResetVelocity(resetDataSource velResetSource);
897 :
898 : // reset the vertical position state using the last height measurement
899 : void ResetHeight(void);
900 :
901 : // return true if we should use the airspeed sensor
902 : bool useAirspeed(void) const;
903 :
904 : // return true if the vehicle code has requested the filter to be ready for flight
905 : bool readyToUseGPS(void) const;
906 :
907 : // return true if the filter to be ready to use the beacon range measurements
908 : bool readyToUseRangeBeacon(void) const;
909 :
910 : // Check for filter divergence
911 : void checkDivergence(void);
912 :
913 : // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
914 : void calcIMU_Weighting(ftype K1, ftype K2);
915 :
916 : #if EK3_FEATURE_OPTFLOW_FUSION
917 : // return true if the filter is ready to start using optical flow measurements for position and velocity estimation
918 : bool readyToUseOptFlow(void) const;
919 : #endif
920 :
921 : // return true if the filter is ready to start using body frame odometry measurements
922 : bool readyToUseBodyOdm(void) const;
923 :
924 : // return true if the filter to be ready to use external nav data
925 : bool readyToUseExtNav(void) const;
926 :
927 : // return true if we should use the range finder sensor
928 : bool useRngFinder(void) const;
929 :
930 : #if EK3_FEATURE_OPTFLOW_FUSION
931 : // determine when to perform fusion of optical flow measurements
932 : void SelectFlowFusion();
933 : #endif
934 :
935 : // determine when to perform fusion of body frame odometry measurements
936 : void SelectBodyOdomFusion();
937 :
938 : // Estimate terrain offset using a single state EKF
939 : void EstimateTerrainOffset(const of_elements &ofDataDelayed);
940 :
941 : #if EK3_FEATURE_OPTFLOW_AGL_KF
942 : // Update the 2-state IMU-aided AGL Kalman filter (height + vertical velocity above ground)
943 : void UpdateAglKf();
944 : #endif
945 :
946 : #if EK3_FEATURE_OPTFLOW_FUSION
947 : // fuse optical flow measurements into the main filter
948 : // really_fuse should be true to actually fuse into the main filter, false to only calculate variances
949 : void FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse);
950 : #endif
951 :
952 : // Control filter mode changes
953 : void controlFilterModes();
954 :
955 : // Determine if we are flying or on the ground
956 : void detectFlight();
957 :
958 : // set the default yaw source
959 : void setYawSource();
960 :
961 : // Set inertial navigation aiding mode
962 : void setAidingMode();
963 :
964 : // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
965 : // avoid unnecessary operations
966 : void setWindMagStateLearningMode();
967 :
968 : // Check the alignmnent status of the tilt attitude
969 : // Used during initial bootstrap alignment of the filter
970 : void checkAttitudeAlignmentStatus();
971 :
972 : // Control reset of yaw and magnetic field states
973 : void controlMagYawReset();
974 :
975 : // Assess GPS data quality and set gpsGoodToAlign
976 : void calcGpsGoodToAlign(void);
977 :
978 : // set the class variable true if the delta angle bias variances are sufficiently small
979 : void checkGyroCalStatus(void);
980 :
981 : // update inflight calculaton that determines if GPS data is good enough for reliable navigation
982 : void calcGpsGoodForFlight(void);
983 :
984 : // Read the range finder and take new measurements if available
985 : // Apply a median filter to range finder data
986 : void readRangeFinder();
987 :
988 : #if EK3_FEATURE_OPTFLOW_FUSION
989 : // check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
990 : void detectOptFlowTakeoff(void);
991 : #endif
992 :
993 : // align the NE earth magnetic field states with the published declination
994 : void alignMagStateDeclination();
995 :
996 : // Fuse compass measurements using a direct yaw angle observation (doesn't require magnetic field states)
997 : // Returns true if the fusion was successful
998 : bool fuseEulerYaw(yawFusionMethod method);
999 :
1000 : // return the best Tait-Bryan rotation order to use
1001 : void bestRotationOrder(rotationOrder &order);
1002 :
1003 : // Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
1004 : // Input is 1-sigma uncertainty in published declination
1005 : void FuseDeclination(ftype declErr);
1006 :
1007 : // return magnetic declination in radians
1008 : ftype MagDeclination(void) const;
1009 :
1010 : // Propagate PVA solution forward from the fusion time horizon to the current time horizon
1011 : // using a simple observer
1012 : void calcOutputStates();
1013 :
1014 : // calculate a filtered offset between baro height measurement and EKF height estimate
1015 : void calcFiltBaroOffset();
1016 :
1017 : // correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
1018 : void correctEkfOriginHeight();
1019 :
1020 : // Select height data to be fused from the available baro, range finder and GPS sources
1021 : void selectHeightForFusion();
1022 :
1023 : // record all requested yaw resets completed
1024 : void recordYawResetsCompleted();
1025 :
1026 : // record a magnetic field state reset event
1027 : void recordMagReset();
1028 :
1029 : // effective value of MAG_CAL
1030 : MagCal effective_magCal(void) const;
1031 :
1032 : // calculate the tilt error variance
1033 : void calcTiltErrorVariance(void);
1034 :
1035 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1036 : // calculate the tilt error variance using an alternative numerical difference technique
1037 : // and log with value generated by NavEKF3_core::calcTiltErrorVariance()
1038 : void verifyTiltErrorVariance();
1039 : #endif
1040 :
1041 : // update timing statistics structure
1042 : void updateTimingStatistics(void);
1043 :
1044 : // Update the state index limit based on which states are active
1045 : void updateStateIndexLim(void);
1046 :
1047 : // correct GPS data for antenna position
1048 : void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
1049 :
1050 : // correct external navigation earth-frame position using sensor body-frame offset
1051 : void CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data);
1052 :
1053 : // correct external navigation earth-frame velocity using sensor body-frame offset
1054 : void CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const;
1055 :
1056 : // calculate velocity variances and innovations
1057 : // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
1058 : // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
1059 : // variances argument is updated with variances for each axis
1060 : void CalculateVelInnovationsAndVariances(const Vector3F &velocity, ftype noise, ftype accel_scale, Vector3F &innovations, Vector3F &variances) const;
1061 :
1062 : // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
1063 : // that uses IMU, GPS horizontal velocity and optionally true airspeed data.
1064 : void runYawEstimatorPrediction(void);
1065 :
1066 : // Run the GPS velocity correction step for the GSF yaw estimator and use the
1067 : // yaw estimate to reset the main EKF yaw if requested
1068 : void runYawEstimatorCorrection(void);
1069 :
1070 : // reset the quaternion states using the supplied yaw angle, maintaining the previous roll and pitch
1071 : // also reset the body to nav frame rotation matrix
1072 : // reset the quaternion state covariances using the supplied yaw variance
1073 : // yaw : new yaw angle (rad)
1074 : // yaw_variance : variance of new yaw angle (rad^2)
1075 : // order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle
1076 : void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
1077 :
1078 : // attempt to reset the yaw to the EKF-GSF value
1079 : // emergency_reset should be true if this reset is triggered by the loss of the yaw estimate
1080 : // returns false if unsuccessful
1081 : bool EKFGSF_resetMainFilterYaw(bool emergency_reset);
1082 :
1083 : // returns true on success and populates yaw (in radians) and yawVariance (rad^2)
1084 : bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;
1085 :
1086 : // Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
1087 : void FuseDragForces();
1088 : void SelectDragFusion();
1089 : void SampleDragData(const imu_elements &imu);
1090 :
1091 : bool getGPSLLH(Location &loc) const;
1092 :
1093 : // Variables
1094 : bool statesInitialised; // boolean true when filter states have been initialised
1095 : bool magHealth; // boolean true if magnetometer has passed innovation consistency check
1096 : bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out
1097 : bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
1098 : bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
1099 : bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
1100 : bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
1101 : bool dragTimeout; // boolean true if drag measurements have failed for too long and have timed out
1102 : bool badIMUdata; // boolean true if the bad IMU data is detected
1103 : bool velAiding; // boolean true if the velocity drift is constrained by observations
1104 : bool waitingForGpsChecks; // boolean true if the EKF should write GPS data to the buffer until quality checks have passed
1105 : uint32_t badIMUdata_ms; // time stamp bad IMU data was last detected
1106 : uint32_t goodIMUdata_ms; // time stamp good IMU data was last detected
1107 : uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
1108 :
1109 : ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
1110 : Matrix24 P; // covariance matrix
1111 : EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
1112 : EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer
1113 : EKF_obs_buffer_t<mag_elements> storedMag; // Magnetometer data buffer
1114 : EKF_obs_buffer_t<baro_elements> storedBaro; // Baro data buffer
1115 : EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer
1116 : #if EK3_FEATURE_RANGEFINDER_MEASUREMENTS
1117 : EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
1118 : #endif
1119 : EKF_IMU_buffer_t<output_elements> storedOutput;// output state buffer
1120 : Matrix3F prevTnb; // previous nav to body transformation used for INS earth rotation compensation
1121 : ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
1122 : ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
1123 : Vector3F earthRateNED; // earths angular rate vector in NED (rad/s)
1124 : ftype dtIMUavg; // expected time between IMU measurements (sec)
1125 : ftype dtEkfAvg; // expected time between EKF updates (sec)
1126 : ftype dt; // time lapsed since the last covariance prediction (sec)
1127 : ftype hgtRate; // state for rate of change of height filter
1128 : bool onGround; // true when the flight vehicle is definitely on the ground
1129 : bool prevOnGround; // value of onGround from previous frame - used to detect transition
1130 : bool inFlight; // true when the vehicle is definitely flying
1131 : bool prevInFlight; // value inFlight from previous frame - used to detect transition
1132 : bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity
1133 : bool fusingStationaryZeroVel; // true when fusing synthetic zero velocity while stationary on ground
1134 : Vector6 innovVelPos; // innovation output for a group of measurements
1135 : Vector6 varInnovVelPos; // innovation variance output for a group of measurements
1136 : Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s)
1137 : bool fuseVelData; // this boolean causes the velNE measurements to be fused
1138 : bool fuseVelVertData; // this boolean causes the velD measurement to be fused
1139 : bool fusePosData; // this boolean causes the posNE measurements to be fused
1140 : bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
1141 : Vector3F innovMag; // innovation output from fusion of X,Y,Z compass measurements
1142 : Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
1143 : ftype innovVtas; // innovation output from fusion of airspeed measurements
1144 : ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
1145 : ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
1146 : ftype defaultAirSpeedVariance; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
1147 : bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
1148 : MagCal effectiveMagCal; // the actual mag calibration being used as the default
1149 : uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
1150 : uint32_t prevBetaDragStep_ms; // time stamp of last synthetic sideslip fusion step
1151 : ftype innovBeta; // synthetic sideslip innovation (rad)
1152 : uint32_t lastMagUpdate_us; // last time compass was updated in usec
1153 : uint32_t lastMagRead_ms; // last time compass data was successfully read
1154 : Vector3F velDotNED; // rate of change of velocity in NED frame
1155 : Vector3F velDotNEDfilt; // low pass filtered velDotNED
1156 : uint32_t imuSampleTime_ms; // time that the last IMU value was taken
1157 : bool tasDataToFuse; // true when new airspeed data is waiting to be fused
1158 : uint32_t lastBaroReceived_ms; // time last time we received baro height data
1159 : uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
1160 : uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
1161 : uint32_t lastGpsPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
1162 : uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
1163 : uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
1164 : uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec)
1165 : uint32_t lastTimeGpsReceived_ms;// last time we received GPS data
1166 : uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements
1167 : uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
1168 : bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
1169 : uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
1170 : uint32_t ekfStartTime_ms; // time the EKF was started (msec)
1171 : Vector2F lastKnownPositionNE; // last known position
1172 : float lastKnownPositionD; // last known height
1173 : uint32_t lastLaunchAccelTime_ms;
1174 : ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
1175 : ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
1176 : ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
1177 : Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
1178 : ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
1179 : bool inhibitWindStates; // true when wind states and covariances should not be used
1180 : ftype lastAirspeedEstimate; // last true airspeed estimate (m/s)
1181 : bool lastAspdEstIsValid; // true when the last true airspeed estimate is valid (m/s)
1182 : bool windStateIsObservable; // true when wind states are observable from measurements.
1183 : bool treatWindStatesAsTruth; // true when wind states should be used as a truth reference
1184 : bool windStatesAligned; // true when wind states have been aligned
1185 : bool inhibitMagStates; // true when magnetic field states are inactive
1186 : bool lastInhibitMagStates; // previous inhibitMagStates
1187 : bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction
1188 : bool needEarthBodyVarReset; // we need to reset mag earth variances at next CovariancePrediction
1189 : bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive
1190 : bool gpsIsInUse; // bool true when GPS data is being used to correct states estimates
1191 : Location EKF_origin; // LLH origin of the NED axis system, internal only
1192 : Location &public_origin; // LLH origin of the NED axis system, public functions
1193 : bool validOrigin; // true when the EKF origin is valid
1194 : ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
1195 : ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
1196 : ftype gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
1197 : uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
1198 : uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
1199 : uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
1200 : ftype posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
1201 : bool useGpsVertVel; // true if GPS vertical velocity should be used
1202 : uint16_t yawResetCount; // number of yaw resets performed by this core
1203 : bool tiltAlignComplete; // true when tilt alignment is complete
1204 : bool yawAlignComplete; // true when yaw alignment is complete
1205 : uint8_t yawAlignGpsValidCount; // number of continuous good GPS velocity samples used for in flight yaw alignment
1206 : bool magStateInitComplete; // true when the magnetic field states have been initialised
1207 : uint8_t stateIndexLim; // Max state index used during matrix and array operations
1208 : imu_elements imuDataDelayed; // IMU data at the fusion time horizon
1209 : imu_elements imuDataNew; // IMU data at the current time horizon
1210 : imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
1211 : QuaternionF imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
1212 : baro_elements baroDataNew; // Baro data at the current time horizon
1213 : baro_elements baroDataDelayed; // Baro data at the fusion time horizon
1214 : range_elements rangeDataNew; // Range finder data at the current time horizon
1215 : range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
1216 : tas_elements tasDataNew; // TAS data at the current time horizon
1217 : tas_elements tasDataDelayed; // TAS data at the fusion time horizon
1218 : mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
1219 : gps_elements gpsDataNew; // GPS data at the current time horizon
1220 : gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
1221 : uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
1222 : output_elements outputDataNew; // output state data at the current time step
1223 : output_elements outputDataDelayed; // output state data at the current time step
1224 : Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
1225 : Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m)
1226 : Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
1227 : ftype badImuVelErrIntegral; // integral of output predictor D velocity tracking error when bad IMU data is detected (m)
1228 : ftype innovYaw; // compass yaw angle innovation (rad)
1229 : uint32_t timeTasReceived_ms; // time last TAS data was received (msec)
1230 : bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system
1231 : uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
1232 : bool consistentMagData; // true when the magnetometers are passing consistency checks
1233 : bool motorsArmed; // true when the motors have been armed
1234 : bool prevMotorsArmed; // value of motorsArmed from previous frame
1235 : bool posVelFusionDelayed; // true when the position and velocity fusion has been delayed
1236 : #if EK3_FEATURE_OPTFLOW_FUSION
1237 : bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
1238 : #endif
1239 : bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
1240 : bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
1241 : bool airDataFusionWindOnly; // true when sideslip and airspeed fusion is only allowed to modify the wind states
1242 : Vector3F lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
1243 : bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
1244 : Vector2F posResetNE; // Change in North/East position due to last in-flight reset in metres
1245 : ftype posResetD; // Change in Down position due to last in-flight reset in metres
1246 : uint16_t posNEResetCount; // number of NE position resets performed by this core
1247 : uint16_t posDResetCount; // number of D position resets performed by this core
1248 : ftype yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
1249 : QuaternionF prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
1250 : ftype hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
1251 : uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
1252 : bool runUpdates; // boolean true when the EKF updates can be run
1253 : uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
1254 : uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
1255 : ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
1256 : Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
1257 : Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
1258 : bool magFieldLearned; // true when the magnetic field has been learned
1259 : uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete
1260 : Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
1261 : Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
1262 : bool delAngBiasLearned; // true when the gyro bias has been learned
1263 : nav_filter_status filterStatus; // contains the status of various filter outputs
1264 : ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
1265 : double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
1266 : uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected
1267 : Vector3F outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
1268 : Vector3F velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
1269 : Vector3F posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
1270 : uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
1271 : uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
1272 : ftype tiltErrorVariance; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
1273 :
1274 : // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
1275 : struct {
1276 : ftype pos;
1277 : ftype vel;
1278 : ftype acc;
1279 : } vertCompFiltState;
1280 :
1281 : // variables used by the pre-initialisation GPS checks
1282 : Location gpsloc_prev; // LLH location of previous GPS measurement
1283 : uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks
1284 : ftype gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
1285 : ftype gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
1286 : ftype gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
1287 :
1288 : // variable used by the in-flight GPS quality check
1289 : bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
1290 : bool gpsVertAccPass; // true when reported GPS vertical accuracy passes in-flight checks
1291 : bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
1292 : ftype sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy
1293 : ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
1294 : uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
1295 : uint32_t lastGpsInnovPassTime_ms; // last time in msec the GPS innovations passed
1296 : uint32_t lastGpsInnovFailTime_ms; // last time in msec the GPS innovations failed
1297 : uint32_t lastGpsVertAccPassTime_ms; // last time in msec the GPS vertical accuracy test passed
1298 : uint32_t lastGpsVertAccFailTime_ms; // last time in msec the GPS vertical accuracy test failed
1299 : bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
1300 : bool gpsAccuracyGoodForAltitude; // true when the GPS accuracy is considered to be good enough to use it as an altitude source.
1301 : Vector3F gpsVelInnov; // gps velocity innovations
1302 : Vector3F gpsVelVarInnov; // gps velocity innovation variances
1303 : uint32_t gpsRetrieveTime_ms; // system time that GPS data was retrieved from the buffer (to detect timeouts)
1304 :
1305 : // variables added for optical flow fusion
1306 : EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
1307 : bool flowDataValid; // true while optical flow data is still fresh
1308 : Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
1309 : uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
1310 : uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
1311 : uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
1312 : uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
1313 : Vector2 flowVarInnov; // optical flow innovations variances (rad/sec)^2
1314 : Vector2 flowInnov; // optical flow LOS innovations (rad/sec)
1315 : uint32_t flowInnovTime_ms; // system time that optical flow innovations and variances were recorded (to detect timeouts)
1316 : #if EK3_FEATURE_OPTFLOW_FUSION
1317 : ftype Popt; // Optical flow terrain height state covariance (m^2)
1318 : #endif
1319 :
1320 : #if EK3_FEATURE_OPTFLOW_AGL_KF
1321 : // ---- 2-state AGL Kalman Filter ----
1322 : // Uses bias-corrected IMU delta-velocity for prediction and downward rangefinder
1323 : // as measurement, decoupled from the main filter's vertical position state.
1324 : // State: x = [aglKfH (m, +up), aglKfV (m/s, +up)]
1325 : ftype aglKfH; // AGL height estimate (m, positive up from ground)
1326 : ftype aglKfV; // AGL velocity estimate (m/s, positive = climbing)
1327 : ftype aglKfP[2][2]; // 2x2 covariance matrix (upper triangle, symmetric)
1328 : bool aglKfValid; // true when RF has been fused within the last 5 s
1329 : uint32_t lastAglRngFuseTime_ms; // timestamp of last successful RF fusion into AGL KF
1330 : #endif
1331 : ftype terrainState; // terrain position state (m)
1332 : ftype prevPosN; // north position at last measurement
1333 : ftype prevPosE; // east position at last measurement
1334 : ftype varInnovRng; // range finder observation innovation variance (m^2)
1335 : ftype innovRng; // range finder observation innovation (m)
1336 : struct {
1337 : uint32_t timestamp_ms; // system timestamp of last correct optical flow sample (used for calibration)
1338 : Vector2f flowRate; // latest corrected optical flow flow rate (used for calibration)
1339 : Vector2f bodyRate; // latest corrected optical flow body rate (used for calibration)
1340 : Vector2f losPred; // EKF estimated component of flowRate that comes from vehicle movement (not rotation)
1341 : } flowCalSample;
1342 :
1343 : #if EK3_FEATURE_OPTFLOW_SRTM
1344 : // terrain altitude from SRTM (only used for optical flow when rangefinder is out-of-range)
1345 : ftype terrain_srtm_alt; // terrain (from SRTM) altitude above the EKF origin (m)
1346 : uint32_t terrain_srtm_alt_ms; // system time when the SRTM terrain altitude was last updated (msec)
1347 : bool terrain_srtm_alt_valid; // true when the SRTM terrain altitude is valid (e.g. has been received within 5 seconds)
1348 : #endif
1349 :
1350 : ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m)
1351 : bool inhibitGndState; // true when the terrain position state is to remain constant
1352 : uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
1353 : Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
1354 : Vector2F auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
1355 : ftype R_LOS; // variance of optical flow rate measurements (rad/sec)^2
1356 : ftype auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
1357 : Vector2F flowGyroBias; // bias error of optical flow sensor gyro output
1358 : bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon.
1359 : bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
1360 : bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
1361 : bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon
1362 : enum AidingMode {
1363 : AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute.
1364 : 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.
1365 : AID_RELATIVE=2, // only optical flow aiding is being used so position estimates will be relative
1366 : };
1367 : AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
1368 : AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
1369 : bool gndOffsetValid; // true when the ground offset state can still be considered valid
1370 : Vector3F delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
1371 : ftype delTimeOF; // time that delAngBodyOF is summed across
1372 : bool flowFusionActive; // true when optical flow fusion is active
1373 :
1374 : Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m)
1375 :
1376 : // Range finder
1377 : ftype baroHgtOffset; // offset applied when when switching to use of Baro height
1378 : ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
1379 : uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
1380 : bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference
1381 : #if EK3_FEATURE_RANGEFINDER_MEASUREMENTS
1382 : ftype storedRngMeas[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3]; // Ringbuffer of stored range measurements for dual range sensors
1383 : uint32_t storedRngMeasTime_ms[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3]; // Ringbuffers of stored range measurement times for dual range sensors
1384 : uint8_t rngMeasIndex[DOWNWARD_RANGEFINDER_MAX_INSTANCES]; // Current range measurement ringbuffer index for dual range sensors
1385 : #endif
1386 :
1387 : // body frame odometry fusion
1388 : #if EK3_FEATURE_BODY_ODOM
1389 : EKF_obs_buffer_t<vel_odm_elements> storedBodyOdm; // body velocity data buffer
1390 : vel_odm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon
1391 : vel_odm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon
1392 : #endif
1393 : uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec)
1394 : Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements
1395 : Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (m/sec)^2
1396 : Vector3 innovBodyVel; // Body velocity XYZ innovations (m/sec)
1397 : uint32_t prevBodyVelFuseTime_ms; // previous time all body velocity measurement components passed their innovation consistency checks (msec)
1398 : uint32_t bodyOdmMeasTime_ms; // time body velocity measurements were accepted for input to the data buffer (msec)
1399 : bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed
1400 : bool bodyVelFusionActive; // true when body frame velocity fusion is active
1401 :
1402 : #if EK3_FEATURE_BODY_ODOM
1403 : // wheel sensor fusion
1404 : EKF_obs_buffer_t<wheel_odm_elements> storedWheelOdm; // body velocity data buffer
1405 : wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon
1406 : #endif
1407 :
1408 : // GPS yaw sensor fusion
1409 : uint32_t yawMeasTime_ms; // system time GPS yaw angle was last input to the data buffer
1410 : EKF_obs_buffer_t<yaw_elements> storedYawAng; // GPS yaw angle buffer
1411 : yaw_elements yawAngDataNew; // GPS yaw angle at the current time horizon
1412 : yaw_elements yawAngDataDelayed; // GPS yaw angle at the fusion time horizon
1413 : yaw_elements yawAngDataStatic; // yaw angle (regardless of yaw source) when the vehicle was last on ground and not moving
1414 :
1415 : // Range Beacon Sensor Fusion
1416 : #if EK3_FEATURE_BEACON_FUSION
1417 : class BeaconFusion {
1418 : public:
1419 3989 : BeaconFusion(AP_DAL &_dal) :
1420 3989 : dal{_dal}
1421 3989 : {}
1422 :
1423 : void InitialiseVariables();
1424 :
1425 : EKF_obs_buffer_t<rng_bcn_elements> storedRange; // Beacon range buffer
1426 : rng_bcn_elements dataDelayed; // Range beacon data at the fusion time horizon
1427 : uint32_t lastPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
1428 : ftype testRatio; // Innovation test ratio for range beacon measurements
1429 : bool health; // boolean true if range beacon measurements have passed innovation consistency check
1430 : ftype varInnov; // range beacon observation innovation variance (m^2)
1431 : ftype innov; // range beacon observation innovation (m)
1432 : uint32_t lastTime_ms[4]; // last time we received a range beacon measurement (msec)
1433 : bool dataToFuse; // true when there is new range beacon data to fuse
1434 : Vector3F vehiclePosNED; // NED position estimate from the beacon system (NED)
1435 : ftype vehiclePosErr; // estimated position error from the beacon system (m)
1436 : uint32_t last3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
1437 : bool goodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
1438 : uint8_t lastChecked; // index of the last range beacon checked for data
1439 : Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
1440 : ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
1441 : bool alignmentStarted; // True when the initial position alignment using range measurements has started
1442 : bool alignmentCompleted; // True when the initial position alignment using range measurements has finished
1443 : uint8_t lastIndex; // Range beacon index last read - used during initialisation of the 3-state filter
1444 : Vector3F posSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
1445 : uint8_t numMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
1446 : ftype sum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
1447 : uint8_t N; // Number of range beacons in use
1448 : ftype maxPosD; // maximum position of all beacons in the down direction (m)
1449 : ftype minPosD; // minimum position of all beacons in the down direction (m)
1450 : bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used
1451 :
1452 : ftype posDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
1453 : ftype posOffsetMaxVar; // Variance of the PosDownOffsetMax state (m)
1454 : ftype maxOffsetStateChangeFilt; // Filtered magnitude of the change in PosOffsetHigh
1455 :
1456 : ftype posDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
1457 : ftype posOffsetMinVar; // Variance of the PosDownOffsetMin state (m)
1458 : ftype minOffsetStateChangeFilt; // Filtered magnitude of the change in PosOffsetLow
1459 :
1460 : Vector3F posOffsetNED; // NED position of the beacon origin in earth frame (m)
1461 : bool originEstInit; // True when the beacon origin has been initialised
1462 :
1463 : // Range Beacon Fusion Debug Reporting
1464 : uint8_t fuseDataReportIndex;// index of range beacon fusion data last reported
1465 : struct FusionReport {
1466 : ftype rng; // measured range to beacon (m)
1467 : ftype innov; // range innovation (m)
1468 : ftype innovVar; // innovation variance (m^2)
1469 : ftype testRatio; // innovation consistency test ratio
1470 : Vector3F beaconPosNED; // beacon NED position
1471 : } *fusionReport;
1472 : uint8_t numFusionReports;
1473 :
1474 : AP_DAL &dal;
1475 : } rngBcn{dal};
1476 : #endif // if EK3_FEATURE_BEACON_FUSION
1477 :
1478 : #if EK3_FEATURE_DRAG_FUSION
1479 : // drag fusion for multicopter wind estimation
1480 : EKF_obs_buffer_t<drag_elements> storedDrag;
1481 : drag_elements dragSampleDelayed;
1482 : drag_elements dragDownSampled; // down sampled from filter prediction rate to observation rate
1483 : uint8_t dragSampleCount; // number of drag specific force samples accumulated at the filter prediction rate
1484 : ftype dragSampleTimeDelta; // time integral across all samples used to form _drag_down_sampled (sec)
1485 : Vector2F innovDrag; // multirotor drag measurement innovation (m/sec**2)
1486 : Vector2F innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2)
1487 : Vector2F dragTestRatio; // drag innovation consistency check ratio
1488 : #endif
1489 : uint32_t lastDragPassTime_ms; // system time that drag samples were last successfully fused
1490 : bool dragFusionEnabled;
1491 :
1492 : // height source selection logic
1493 : AP_NavEKF_Source::SourceZ activeHgtSource; // active height source
1494 : AP_NavEKF_Source::SourceZ prevHgtSource; // previous height source used to detect changes in source
1495 :
1496 : // Movement detector
1497 : bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
1498 : ftype rngAtStartOfFlight; // range finder measurement at start of flight
1499 : uint32_t timeAtArming_ms; // time in msec that the vehicle armed
1500 :
1501 : // baro ground effect
1502 : ftype meaHgtAtTakeOff; // height measured at commencement of takeoff
1503 :
1504 : // control of post takeoff magnetic field and heading resets
1505 : bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
1506 : uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
1507 : bool finalInflightMagInit; // true when the final post takeoff initialisation of magnetic field states been performed
1508 : bool magStateResetRequest; // true if magnetic field states need to be reset using the magnetomter measurements
1509 : bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
1510 : bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
1511 : ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
1512 : ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
1513 : QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset
1514 : MagFuseSel magFusionSel; // magnetometer fusion selection
1515 :
1516 : // Used by on ground movement check required when operating on ground without a yaw reference
1517 : ftype gyro_diff; // filtered gyro difference (rad/s)
1518 : ftype accel_diff; // filtered acceerometer difference (m/s/s)
1519 : Vector3F gyro_prev; // gyro vector from previous time step (rad/s)
1520 : Vector3F accel_prev; // accelerometer vector from previous time step (m/s/s)
1521 : bool onGroundNotMoving; // true when on the ground and not moving
1522 : uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
1523 :
1524 : // variables used to inhibit accel bias learning
1525 : bool inhibitDelVelBiasStates; // true when all IMU delta velocity bias states are de-activated
1526 : bool dvelBiasAxisInhibit[3] {}; // true when IMU delta velocity bias states for a specific axis is de-activated
1527 : Vector3F dvelBiasAxisVarPrev; // saved delta velocity XYZ bias variances (m/sec)**2
1528 :
1529 : #if EK3_FEATURE_EXTERNAL_NAV
1530 : // external navigation fusion
1531 : EKF_obs_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
1532 : ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
1533 : uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec)
1534 : uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
1535 : bool extNavDataToFuse; // true when there is new external nav data to fuse
1536 : bool extNavUsedForPos; // true when the external nav data is being used as a position reference.
1537 : EKF_obs_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer
1538 : ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
1539 : uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
1540 : bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
1541 : Vector3F extNavVelInnov; // external nav velocity innovations
1542 : Vector3F extNavVelVarInnov; // external nav velocity innovation variances
1543 : uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
1544 : EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
1545 : yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
1546 : uint32_t last_extnav_yaw_fusion_ms; // system time that external nav yaw was last fused
1547 : #endif // EK3_FEATURE_EXTERNAL_NAV
1548 : bool useExtNavVel; // true if external nav velocity should be used
1549 :
1550 : // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
1551 : struct {
1552 : bool bad_xmag:1;
1553 : bool bad_ymag:1;
1554 : bool bad_zmag:1;
1555 : bool bad_airspeed:1;
1556 : bool bad_sideslip:1;
1557 : bool bad_nvel:1;
1558 : bool bad_evel:1;
1559 : bool bad_dvel:1;
1560 : bool bad_npos:1;
1561 : bool bad_epos:1;
1562 : bool bad_dpos:1;
1563 : bool bad_yaw:1;
1564 : bool bad_decl:1;
1565 : bool bad_xflow:1;
1566 : bool bad_yflow:1;
1567 : bool bad_rngbcn:1;
1568 : bool bad_xvel:1;
1569 : bool bad_yvel:1;
1570 : bool bad_zvel:1;
1571 : } faultStatus;
1572 :
1573 : // flags indicating which GPS quality checks are failing
1574 : union {
1575 : struct {
1576 : bool bad_sAcc:1;
1577 : bool bad_hAcc:1;
1578 : bool bad_vAcc:1;
1579 : bool bad_yaw:1;
1580 : bool bad_sats:1;
1581 : bool bad_VZ:1;
1582 : bool bad_horiz_drift:1;
1583 : bool bad_hdop:1;
1584 : bool bad_vert_vel:1;
1585 : bool bad_fix:1;
1586 : bool bad_horiz_vel:1;
1587 : };
1588 : uint16_t value;
1589 : } gpsCheckStatus;
1590 :
1591 : // string representing last reason for prearm failure
1592 : char prearm_fail_string[40];
1593 :
1594 : // earth field from WMM tables
1595 : bool have_table_earth_field; // true when we have initialised table_earth_field_ga
1596 : Vector3F table_earth_field_ga; // earth field from WMM tables
1597 : ftype table_declination; // declination in radians from the tables
1598 :
1599 : // 1Hz update
1600 : uint32_t last_oneHz_ms;
1601 : void oneHzUpdate(void);
1602 :
1603 : // move EKF origin at 1Hz
1604 : void moveEKFOrigin(void);
1605 :
1606 : // handle earth field updates
1607 : void getEarthFieldTable(const Location &loc);
1608 : void checkUpdateEarthField(void);
1609 :
1610 : // timing statistics
1611 : struct ekf_timing timing;
1612 :
1613 : // when was attitude filter status last non-zero?
1614 : uint32_t last_filter_ok_ms;
1615 :
1616 : // should we assume zero sideslip?
1617 : bool assume_zero_sideslip(void) const;
1618 :
1619 : // vehicle specific initial gyro bias uncertainty
1620 : ftype InitialGyroBiasUncertainty(void) const;
1621 :
1622 : // get the gyro bias limit for this core's IMU
1623 : ftype getGyroBiasLimit(void) const;
1624 :
1625 : /*
1626 : learn magnetometer biases from GPS yaw. Return true if the
1627 : resulting mag vector is close enough to the one predicted by GPS
1628 : yaw to use it for fallback
1629 : */
1630 : bool learnMagBiasFromGPS(void);
1631 :
1632 : uint32_t last_gps_yaw_ms; // last time the EKF attempted to use the GPS yaw
1633 : uint32_t last_gps_yaw_fuse_ms; // last time the EKF successfully fused the GPS yaw
1634 : bool gps_yaw_mag_fallback_ok;
1635 : bool gps_yaw_mag_fallback_active;
1636 : uint8_t gps_yaw_fallback_good_counter;
1637 :
1638 : /*
1639 : Update the on ground and not moving check.
1640 : Should be called once per IMU update.
1641 : Only updates when on ground and when operating with an external yaw sensor
1642 : */
1643 : void updateMovementCheck(void);
1644 :
1645 : // The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
1646 : uint32_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec)
1647 : uint32_t EKFGSF_yaw_reset_request_ms; // timestamp of last emergency yaw reset request (uSec)
1648 : uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed
1649 : bool EKFGSF_run_filterbank; // true when the filter bank is active
1650 : uint8_t EKFGSF_yaw_valid_count; // number of updates since the last invalid yaw estimate
1651 :
1652 : // logging timestamps
1653 : uint32_t lastLogTime_ms;
1654 : uint32_t lastUpdateTime_ms;
1655 : uint32_t lastEkfStateVarLogTime_ms;
1656 : uint32_t lastTimingLogTime_ms;
1657 :
1658 : // bits in EK3_AFFINITY
1659 : enum ekf_affinity {
1660 : EKF_AFFINITY_GPS = (1U<<0),
1661 : EKF_AFFINITY_BARO = (1U<<1),
1662 : EKF_AFFINITY_MAG = (1U<<2),
1663 : EKF_AFFINITY_ARSP = (1U<<3),
1664 : };
1665 :
1666 : // update selected_sensors for this core
1667 : void update_sensor_selection(void);
1668 : void update_gps_selection(void);
1669 : void update_mag_selection(void);
1670 : void update_baro_selection(void);
1671 : void update_airspeed_selection(void);
1672 :
1673 : // selected and preferred sensor instances. We separate selected
1674 : // from preferred so that calcGpsGoodToAlign() can ensure the
1675 : // preferred sensor is ready. Note that magSelectIndex is used for
1676 : // compass selection
1677 : uint8_t selected_gps;
1678 : uint8_t preferred_gps;
1679 : uint8_t selected_baro;
1680 : uint8_t selected_airspeed;
1681 :
1682 : // source reset handling
1683 : AP_NavEKF_Source::SourceXY posxy_source_last; // horizontal position source on previous iteration (used to detect a changes)
1684 : bool posxy_source_reset; // true when the horizontal position source has changed but the position has not yet been reset
1685 : AP_NavEKF_Source::SourceYaw yaw_source_last; // yaw source on previous iteration (used to detect a change)
1686 : bool yaw_source_reset; // true when the yaw source has changed but the yaw has not yet been reset
1687 :
1688 : // logging functions shared by cores:
1689 : void Log_Write_XKF1(uint64_t time_us) const;
1690 : void Log_Write_XKF2(uint64_t time_us) const;
1691 : void Log_Write_XKF3(uint64_t time_us) const;
1692 : void Log_Write_XKF4(uint64_t time_us) const;
1693 : void Log_Write_XKF5(uint64_t time_us) const;
1694 : void Log_Write_XKFS(uint64_t time_us) const;
1695 : void Log_Write_Quaternion(uint64_t time_us) const;
1696 : void Log_Write_Beacon(uint64_t time_us);
1697 : void Log_Write_BodyOdom(uint64_t time_us);
1698 : void Log_Write_State_Variances(uint64_t time_us);
1699 : void Log_Write_Timing(uint64_t time_us);
1700 : void Log_Write_GSF(uint64_t time_us);
1701 : };
|
