Line data Source code
1 : /*
2 : 24 state EKF based on the derivation in https://github.com/priseborough/
3 : InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/
4 : GenerateNavFilterEquations.m
5 :
6 : Converted from Matlab to C++ by Paul Riseborough
7 :
8 : This program is free software: you can redistribute it and/or modify
9 : it under the terms of the GNU General Public License as published by
10 : the Free Software Foundation, either version 3 of the License, or
11 : (at your option) any later version.
12 :
13 : This program is distributed in the hope that it will be useful,
14 : but WITHOUT ANY WARRANTY; without even the implied warranty of
15 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 : GNU General Public License for more details.
17 :
18 : You should have received a copy of the GNU General Public License
19 : along with this program. If not, see <http://www.gnu.org/licenses/>.
20 : */
21 : #pragma once
22 :
23 : #if !defined(HAL_DEBUG_BUILD) || !HAL_DEBUG_BUILD
24 : #pragma GCC optimize("O2")
25 : #endif
26 :
27 : #include <AP_InertialSensor/AP_InertialSensor.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/EKF_Buffer.h>
33 : #include <AP_RangeFinder/AP_RangeFinder.h>
34 : #include <AP_Beacon/AP_Beacon_config.h>
35 :
36 : #include "AP_NavEKF/EKFGSF_yaw.h"
37 :
38 : // GPS pre-flight check bit locations
39 : #define MASK_GPS_NSATS (1<<0)
40 : #define MASK_GPS_HDOP (1<<1)
41 : #define MASK_GPS_SPD_ERR (1<<2)
42 : #define MASK_GPS_POS_ERR (1<<3)
43 : #define MASK_GPS_YAW_ERR (1<<4)
44 : #define MASK_GPS_POS_DRIFT (1<<5)
45 : #define MASK_GPS_VERT_SPD (1<<6)
46 : #define MASK_GPS_HORIZ_SPD (1<<7)
47 :
48 : // active height source
49 : #define HGT_SOURCE_BARO 0
50 : #define HGT_SOURCE_RNG 1
51 : #define HGT_SOURCE_GPS 2
52 : #define HGT_SOURCE_BCN 3
53 : #define HGT_SOURCE_EXTNAV 4
54 :
55 : // target EKF update time step
56 : #define EKF_TARGET_DT 0.01f
57 :
58 : // mag fusion final reset altitude
59 : #define EKF2_MAG_FINAL_RESET_ALT 2.5f
60 :
61 : // maximum number of yaw resets due to detected magnetic anomaly allowed per flight
62 : #define MAG_ANOMALY_RESET_MAX 2
63 :
64 : // number of seconds a request to reset the yaw to the GSF estimate is active before it times out
65 : #define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
66 :
67 : // limit on horizontal position states
68 : #if HAL_WITH_EKF_DOUBLE
69 : #define EK2_POSXY_STATE_LIMIT 50.0e6
70 : #else
71 : #define EK2_POSXY_STATE_LIMIT 1.0e6
72 : #endif
73 :
74 : // maximum number of downward facing rangefinder instances available
75 : #if AP_RANGEFINDER_ENABLED
76 : #if RANGEFINDER_MAX_INSTANCES > 1
77 : #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 2
78 : #else
79 : #define DOWNWARD_RANGEFINDER_MAX_INSTANCES 1
80 : #endif
81 : #endif
82 :
83 : class AP_AHRS;
84 :
85 : class NavEKF2_core : public NavEKF_core_common
86 : {
87 : public:
88 : // Constructor
89 : NavEKF2_core(class NavEKF2 *_frontend);
90 :
91 : // setup this core backend
92 : bool setup_core(uint8_t _imu_index, uint8_t _core_index);
93 :
94 : // Initialise the states from accelerometer and magnetometer data (if present)
95 : // This method can only be used when the vehicle is static
96 : bool InitialiseFilterBootstrap(void);
97 :
98 : // Update Filter States - this should be called whenever new IMU data is available
99 : // The predict flag is set true when a new prediction cycle can be started
100 : void UpdateFilter(bool predict);
101 :
102 : // Check basic filter health metrics and return a consolidated health status
103 : bool healthy(void) const;
104 :
105 : // Return a consolidated error score where higher numbers are less healthy
106 : // Intended to be used by the front-end to determine which is the primary EKF
107 : ftype errorScore(void) const;
108 :
109 : // Write the last calculated NE position relative to the reference point (m).
110 : // If a calculated solution is not available, use the best available data and return false
111 : // If false returned, do not use for flight control
112 : bool getPosNE(Vector2p &posNE) const;
113 :
114 : // Write the last calculated D position relative to the reference point (m).
115 : // If a calculated solution is not available, use the best available data and return false
116 : // If false returned, do not use for flight control
117 : bool getPosD(postype_t &posD) const;
118 :
119 : // return NED velocity in m/s
120 : void getVelNED(Vector3f &vel) const;
121 :
122 : // return estimate of true airspeed vector in body frame in m/s
123 : // returns false if estimate is unavailable
124 : bool getAirSpdVec(Vector3f &vel) const;
125 :
126 : // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
127 : // 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
128 : // but will always be kinematically consistent with the z component of the EKF position state
129 : float getPosDownDerivative(void) const;
130 :
131 : // return body axis gyro bias estimates in rad/sec
132 : void getGyroBias(Vector3f &gyroBias) const;
133 :
134 : // return body axis gyro scale factor error as a percentage
135 : void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;
136 :
137 : // reset body axis gyro bias estimates
138 : void resetGyroBias(void);
139 :
140 : // Resets the baro so that it reads zero at the current height
141 : // Resets the EKF height to zero
142 : // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
143 : // Returns true if the height datum reset has been performed
144 : // If using a range finder for height no reset is performed and it returns false
145 : bool resetHeightDatum(void);
146 :
147 : // return the horizontal speed limit in m/s set by optical flow sensor limits
148 : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
149 : void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
150 :
151 : // return the Z-accel bias estimate in m/s^2
152 : void getAccelZBias(float &zbias) const;
153 :
154 : // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
155 : void getWind(Vector3f &wind) const;
156 :
157 : // return earth magnetic field estimates in measurement units / 1000
158 : void getMagNED(Vector3f &magNED) const;
159 :
160 : // return body magnetic field estimates in measurement units / 1000
161 : void getMagXYZ(Vector3f &magXYZ) const;
162 :
163 : // Return estimated magnetometer offsets
164 : // Return true if magnetometer offsets are valid
165 : bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
166 :
167 : // Return the last calculated latitude, longitude and height in WGS-84
168 : // If a calculated location isn't available, return a raw GPS measurement
169 : // The status will return true if a calculation or raw measurement is available
170 : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
171 : bool getLLH(Location &loc) const;
172 :
173 : // return the latitude and longitude and height used to set the NED origin
174 : // All NED positions calculated by the filter are relative to this location
175 : // Returns false if the origin has not been set
176 : bool getOriginLLH(Location &loc) const;
177 :
178 : // set the latitude and longitude and height used to set the NED origin
179 : // All NED positions calculated by the filter will be relative to this location
180 : // returns false if absolute aiding and GPS is being used or if the origin is already set
181 : bool setOriginLLH(const Location &loc);
182 :
183 : // return estimated height above ground level
184 : // return false if ground height is not being estimated.
185 : bool getHAGL(float &HAGL) const;
186 :
187 : // return the Euler roll, pitch and yaw angle in radians
188 : void getEulerAngles(Vector3f &eulers) const;
189 :
190 : // return the transformation matrix from XYZ (body) to NED axes
191 : void getRotationBodyToNED(Matrix3f &mat) const;
192 :
193 : // return the quaternions defining the rotation from NED to XYZ (body) axes
194 : void getQuaternion(Quaternion &quat) const;
195 :
196 : // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
197 : bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
198 :
199 : // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
200 : bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
201 :
202 : // should we use the compass? This is public so it can be used for
203 : // reporting via ahrs.use_compass()
204 : bool use_compass(void) const;
205 :
206 : // write the raw optical flow measurements
207 : // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
208 : // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
209 : // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
210 : // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
211 : // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
212 : // posOffset is the XYZ flow sensor position in the body frame in m
213 : // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
214 : void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
215 :
216 : /*
217 : Returns the following data for debugging range beacon fusion
218 : ID : beacon identifier
219 : rng : measured range to beacon (m)
220 : innov : range innovation (m)
221 : innovVar : innovation variance (m^2)
222 : testRatio : innovation consistency test ratio
223 : beaconPosNED : beacon NED position (m)
224 : */
225 : bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
226 :
227 : // Set to true if the terrain underneath is stable enough to be used as a height reference
228 : // in combination with a range finder. Set to false if the terrain underneath the vehicle
229 : // cannot be used as a height reference. Use to prevent range finder operation otherwise
230 : // enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
231 : void setTerrainHgtStable(bool val);
232 :
233 : /*
234 : return the filter fault status as a bitmasked integer
235 : 0 = quaternions are NaN
236 : 1 = velocities are NaN
237 : 2 = badly conditioned X magnetometer fusion
238 : 3 = badly conditioned Y magnetometer fusion
239 : 4 = badly conditioned Z magnetometer fusion
240 : 5 = badly conditioned airspeed fusion
241 : 6 = badly conditioned synthetic sideslip fusion
242 : 7 = filter is not initialised
243 : */
244 : void getFilterFaults(uint16_t &faults) const;
245 :
246 : /*
247 : return filter gps quality check status
248 : */
249 : void getFilterGpsStatus(nav_gps_status &status) const;
250 :
251 : /*
252 : Return a filter function status that indicates:
253 : Which outputs are valid
254 : If the filter has detected takeoff
255 : If the filter has activated the mode that mitigates against ground effect static pressure errors
256 : If GPS data is being used
257 : */
258 : void getFilterStatus(nav_filter_status &status) const;
259 :
260 : // return a terrain altitude variance
261 : bool getTerrainAltVariance(float &terrain_alt_variance) const;
262 :
263 : // provides the height limit to be observed by the control loops
264 : // returns false if no height limiting is required
265 : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
266 : bool getHeightControlLimit(float &height) const;
267 :
268 : // return the number of yaw resets performed by this core
269 2113079 : uint16_t getYawResetCount(void) const { return yawResetCount; }
270 :
271 : // return the number of NE position resets performed by this core
272 2113079 : uint16_t getPosNorthEastResetCount(void) const { return posNEResetCount; }
273 :
274 : // return the number of D position resets performed by this core
275 2113079 : uint16_t getPosDownResetCount(void) const { return posDResetCount; }
276 :
277 : // report any reason for why the backend is refusing to initialise
278 : const char *prearm_failure_reason(void) const;
279 :
280 : // report the number of frames lapsed since the last state prediction
281 : // this is used by other instances to level load
282 : uint8_t getFramesSincePredict(void) const;
283 :
284 : // get the IMU index. For now we return the gyro index, as that is most
285 : // critical for use by other subsystems.
286 4420898 : uint8_t getIMUIndex(void) const { return gyro_index_active; }
287 :
288 : /*
289 : * Write position and quaternion data from an external navigation system
290 : *
291 : * pos : position in the RH navigation frame. Frame is assumed to be NED (m)
292 : * quat : quaternion desribing the rotation from navigation frame to body frame
293 : * posErr : 1-sigma spherical position error (m)
294 : * angErr : 1-sigma spherical angle error (rad)
295 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
296 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
297 : * resetTime_ms : system time of the last position reset request (mSec)
298 : *
299 : * Sensor offsets are pulled directly from the AP_VisualOdom library
300 : *
301 : */
302 : void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
303 :
304 : /*
305 : * Write velocity data from an external navigation system
306 : * vel : velocity in NED (m)
307 : * err : velocity error (m/s)
308 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
309 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
310 : */
311 : void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
312 :
313 : // return true when external nav data is also being used as a yaw observation
314 : bool isExtNavUsedForYaw(void) const;
315 :
316 : // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
317 : void writeDefaultAirSpeed(float airspeed);
318 :
319 : // request a reset the yaw to the EKF-GSF value
320 : void EKFGSF_requestYawReset();
321 :
322 : // return true if we are tilt aligned
323 66714 : bool have_aligned_tilt(void) const {
324 66714 : return tiltAlignComplete;
325 : }
326 :
327 : // return true if we are yaw aligned
328 66714 : bool have_aligned_yaw(void) const {
329 66714 : return yawAlignComplete;
330 : }
331 :
332 : void Log_Write(uint64_t time_us);
333 :
334 : // get a yaw estimator instance
335 0 : const EKFGSF_yaw *get_yawEstimator(void) const { return yawEstimator; }
336 :
337 : private:
338 : EKFGSF_yaw *yawEstimator;
339 : class AP_DAL &dal;
340 :
341 : // Reference to the global EKF frontend for parameters
342 : class NavEKF2 *frontend;
343 : uint8_t imu_index; // preferred IMU index
344 : uint8_t gyro_index_active; // active gyro index (in case preferred fails)
345 : uint8_t accel_index_active; // active accel index (in case preferred fails)
346 : uint8_t core_index;
347 : uint8_t imu_buffer_length;
348 :
349 : #if MATH_CHECK_INDEXES
350 : typedef VectorN<ftype,2> Vector2;
351 : typedef VectorN<ftype,3> Vector3;
352 : typedef VectorN<ftype,4> Vector4;
353 : typedef VectorN<ftype,5> Vector5;
354 : typedef VectorN<ftype,6> Vector6;
355 : typedef VectorN<ftype,7> Vector7;
356 : typedef VectorN<ftype,8> Vector8;
357 : typedef VectorN<ftype,9> Vector9;
358 : typedef VectorN<ftype,10> Vector10;
359 : typedef VectorN<ftype,11> Vector11;
360 : typedef VectorN<ftype,13> Vector13;
361 : typedef VectorN<ftype,14> Vector14;
362 : typedef VectorN<ftype,15> Vector15;
363 : typedef VectorN<ftype,22> Vector22;
364 : typedef VectorN<ftype,23> Vector23;
365 : typedef VectorN<ftype,24> Vector24;
366 : typedef VectorN<ftype,25> Vector25;
367 : typedef VectorN<ftype,31> Vector31;
368 : typedef VectorN<VectorN<ftype,3>,3> Matrix3;
369 : typedef VectorN<VectorN<ftype,24>,24> Matrix24;
370 : typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
371 : typedef VectorN<uint32_t,50> Vector_u32_50;
372 : #else
373 : typedef ftype Vector2[2];
374 : typedef ftype Vector3[3];
375 : typedef ftype Vector4[4];
376 : typedef ftype Vector5[5];
377 : typedef ftype Vector6[6];
378 : typedef ftype Vector7[7];
379 : typedef ftype Vector8[8];
380 : typedef ftype Vector9[9];
381 : typedef ftype Vector10[10];
382 : typedef ftype Vector11[11];
383 : typedef ftype Vector13[13];
384 : typedef ftype Vector14[14];
385 : typedef ftype Vector15[15];
386 : typedef ftype Vector22[22];
387 : typedef ftype Vector23[23];
388 : typedef ftype Vector24[24];
389 : typedef ftype Vector25[25];
390 : typedef ftype Matrix3[3][3];
391 : typedef ftype Matrix24[24][24];
392 : typedef ftype Matrix34_50[34][50];
393 : typedef uint32_t Vector_u32_50[50];
394 : #endif
395 :
396 : // the states are available in two forms, either as a Vector31, or
397 : // broken down as individual elements. Both are equivalent (same
398 : // memory)
399 : struct state_elements {
400 : Vector3F angErr; // 0..2
401 : Vector3F velocity; // 3..5
402 : Vector3F position; // 6..8
403 : Vector3F gyro_bias; // 9..11
404 : Vector3F gyro_scale; // 12..14
405 : ftype accel_zbias; // 15
406 : Vector3F earth_magfield; // 16..18
407 : Vector3F body_magfield; // 19..21
408 : Vector2F wind_vel; // 22..23
409 : QuaternionF quat; // 24..27
410 : };
411 :
412 : union {
413 : Vector28 statesArray;
414 : struct state_elements stateStruct;
415 : };
416 :
417 : struct output_elements {
418 : QuaternionF quat; // 0..3
419 : Vector3F velocity; // 4..6
420 : Vector3F position; // 7..9
421 : };
422 :
423 : struct imu_elements {
424 : Vector3F delAng; // 0..2
425 : Vector3F delVel; // 3..5
426 : ftype delAngDT; // 6
427 : ftype delVelDT; // 7
428 : uint32_t time_ms; // 8
429 : uint8_t gyro_index;
430 : uint8_t accel_index;
431 : };
432 :
433 : struct gps_elements : EKF_obs_element_t {
434 : Vector2F pos;
435 : ftype hgt;
436 : Vector3F vel;
437 : uint8_t sensor_idx;
438 : };
439 :
440 : struct mag_elements : EKF_obs_element_t {
441 : Vector3F mag;
442 : };
443 :
444 : struct baro_elements : EKF_obs_element_t {
445 : ftype hgt;
446 : };
447 :
448 : struct range_elements : EKF_obs_element_t {
449 : ftype rng;
450 : uint8_t sensor_idx;
451 : };
452 :
453 : struct rng_bcn_elements : EKF_obs_element_t {
454 : ftype rng; // range measurement to each beacon (m)
455 : Vector3F beacon_posNED; // NED position of the beacon (m)
456 : ftype rngErr; // range measurement error 1-std (m)
457 : uint8_t beacon_ID; // beacon identification number
458 : };
459 :
460 : struct tas_elements : EKF_obs_element_t {
461 : ftype tas;
462 : };
463 :
464 : struct of_elements : EKF_obs_element_t {
465 : Vector2F flowRadXY;
466 : Vector2F flowRadXYcomp;
467 : Vector3F bodyRadXYZ;
468 : Vector3F body_offset;
469 : float heightOverride;
470 : };
471 :
472 : struct ext_nav_elements : EKF_obs_element_t {
473 : Vector3F pos; // XYZ position measured in a RH navigation frame (m)
474 : QuaternionF quat; // quaternion describing the rotation from navigation to body frame
475 : ftype posErr; // spherical poition measurement error 1-std (m)
476 : ftype angErr; // spherical angular measurement error 1-std (rad)
477 : bool posReset; // true when the position measurement has been reset
478 : };
479 :
480 : // bias estimates for the IMUs that are enabled but not being used
481 : // by this core.
482 : struct {
483 : Vector3F gyro_bias;
484 : Vector3F gyro_scale;
485 : ftype accel_zbias;
486 : } inactiveBias[INS_MAX_INSTANCES];
487 :
488 : struct ext_nav_vel_elements : EKF_obs_element_t {
489 : Vector3F vel; // velocity in NED (m)
490 : ftype err; // velocity measurement error (m/s)
491 : };
492 :
493 : // update the navigation filter status
494 : void updateFilterStatus(void);
495 :
496 : // update the quaternion, velocity and position states using IMU measurements
497 : void UpdateStrapdownEquationsNED();
498 :
499 : // calculate the predicted state covariance matrix
500 : void CovariancePrediction();
501 :
502 : // force symmetry on the state covariance matrix
503 : void ForceSymmetry();
504 :
505 : // copy covariances across from covariance prediction calculation and fix numerical errors
506 : void CopyCovariances();
507 :
508 : // constrain variances (diagonal terms) in the state covariance matrix
509 : void ConstrainVariances();
510 :
511 : // constrain states
512 : void ConstrainStates();
513 :
514 : // constrain earth field using WMM tables
515 : void MagTableConstrain(void);
516 :
517 : // fuse selected position, velocity and height measurements
518 : void FuseVelPosNED();
519 :
520 : // fuse range beacon measurements
521 : void FuseRngBcn();
522 :
523 : // use range beacon measurements to calculate a static position
524 : void FuseRngBcnStatic();
525 :
526 : // calculate the offset from EKF vertical position datum to the range beacon system datum
527 : void CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning);
528 :
529 : // fuse magnetometer measurements
530 : void FuseMagnetometer();
531 :
532 : // fuse true airspeed measurements
533 : void FuseAirspeed();
534 :
535 : // fuse synthetic sideslip measurement of zero
536 : void FuseSideslip();
537 :
538 : // zero specified range of rows in the state covariance matrix
539 : void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last);
540 :
541 : // zero specified range of columns in the state covariance matrix
542 : void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);
543 :
544 : // Reset the stored output history to current data
545 : void StoreOutputReset(void);
546 :
547 : // Reset the stored output quaternion history to current EKF state
548 : void StoreQuatReset(void);
549 :
550 : // Rotate the stored output quaternion history through a quaternion rotation
551 : void StoreQuatRotate(const QuaternionF &deltaQuat);
552 :
553 : // calculate the NED earth spin vector in rad/sec
554 : void calcEarthRateNED(Vector3F &omega, int32_t latitude) const;
555 :
556 : // initialise the covariance matrix
557 : void CovarianceInit();
558 :
559 : // helper functions for readIMUData
560 : bool readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt);
561 : bool readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt);
562 :
563 : // helper functions for correcting IMU data
564 : void correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index);
565 : void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index);
566 :
567 : // update IMU delta angle and delta velocity measurements
568 : void readIMUData();
569 :
570 : // update estimate of inactive bias states
571 : void learnInactiveBiases();
572 :
573 : // check for new valid GPS data and update stored measurement if available
574 : void readGpsData();
575 :
576 : // check for new altitude measurement data and update stored measurement if available
577 : void readBaroData();
578 :
579 : // check for new magnetometer data and update store measurements if available
580 : void readMagData();
581 :
582 : // try changing compasses on compass failure or timeout
583 : void tryChangeCompass(void);
584 :
585 : // check for new airspeed data and update stored measurements if available
586 : void readAirSpdData();
587 :
588 : // check for new range beacon data and update stored measurements if available
589 : void readRngBcnData();
590 :
591 : // determine when to perform fusion of GPS position and velocity measurements
592 : void SelectVelPosFusion();
593 :
594 : // determine when to perform fusion of range measurements take relative to a beacon at a known NED position
595 : void SelectRngBcnFusion();
596 :
597 : // determine when to perform fusion of magnetometer measurements
598 : void SelectMagFusion();
599 :
600 : // determine when to perform fusion of true airspeed measurements
601 : void SelectTasFusion();
602 :
603 : // determine when to perform fusion of synthetic sideslp measurements
604 : void SelectBetaFusion();
605 :
606 : // force alignment of the yaw angle using GPS velocity data
607 : void realignYawGPS();
608 :
609 : // initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
610 : // and return attitude quaternion
611 : QuaternionF calcQuatAndFieldStates(ftype roll, ftype pitch);
612 :
613 : // zero stored variables
614 : void InitialiseVariables();
615 :
616 : void InitialiseVariablesMag();
617 :
618 : // reset the horizontal position states uing the last GPS measurement
619 : void ResetPosition(void);
620 :
621 : // reset the stateStruct's NE position to the specified position
622 : void ResetPositionNE(ftype posN, ftype posE);
623 :
624 : // reset velocity states using the last GPS measurement
625 : void ResetVelocity(void);
626 :
627 : // reset the vertical position state using the last height measurement
628 : void ResetHeight(void);
629 :
630 : // reset the stateStruct's D position
631 : void ResetPositionD(ftype posD);
632 :
633 : // return true if we should use the airspeed sensor
634 : bool useAirspeed(void) const;
635 :
636 : // return true if the vehicle code has requested the filter to be ready for flight
637 : bool readyToUseGPS(void) const;
638 :
639 : // return true if the filter to be ready to use the beacon range measurements
640 : bool readyToUseRangeBeacon(void) const;
641 :
642 : // return true if the filter to be ready to use external nav data
643 : bool readyToUseExtNav(void) const;
644 :
645 : // Check for filter divergence
646 : void checkDivergence(void);
647 :
648 : // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
649 : void calcIMU_Weighting(ftype K1, ftype K2);
650 :
651 : // return true if optical flow data is available
652 : bool optFlowDataPresent(void) const;
653 :
654 : // return true if we should use the range finder sensor
655 : bool useRngFinder(void) const;
656 :
657 : // determine when to perform fusion of optical flow measurements
658 : void SelectFlowFusion();
659 :
660 : // Estimate terrain offset using a single state EKF
661 : void EstimateTerrainOffset();
662 :
663 : // fuse optical flow measurements into the main filter
664 : void FuseOptFlow();
665 :
666 : // Control filter mode changes
667 : void controlFilterModes();
668 :
669 : // Determine if we are flying or on the ground
670 : void detectFlight();
671 :
672 : // Set inertial navigaton aiding mode
673 : void setAidingMode();
674 :
675 : // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
676 : // avoid unnecessary operations
677 : void setWindMagStateLearningMode();
678 :
679 : // Check the alignmnent status of the tilt attitude
680 : // Used during initial bootstrap alignment of the filter
681 : void checkAttitudeAlignmentStatus();
682 :
683 : // Control reset of yaw and magnetic field states
684 : void controlMagYawReset();
685 :
686 : // set the latitude and longitude and height used to set the NED origin
687 : // All NED positions calculated by the filter will be relative to this location
688 : // returns false if the origin has already been set
689 : bool setOrigin(const Location &loc);
690 :
691 : // Assess GPS data quality and set gpsGoodToAlign if good enough to align the EKF
692 : void calcGpsGoodToAlign(void);
693 :
694 : // return true and set the class variable true if the delta angle bias has been learned
695 : bool checkGyroCalStatus(void);
696 :
697 : // update inflight calculaton that determines if GPS data is good enough for reliable navigation
698 : void calcGpsGoodForFlight(void);
699 :
700 : #if AP_RANGEFINDER_ENABLED
701 : // Read the range finder and take new measurements if available
702 : // Apply a median filter to range finder data
703 : void readRangeFinder();
704 : #endif
705 :
706 : // check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
707 : void detectOptFlowTakeoff(void);
708 :
709 : // align the NE earth magnetic field states with the published declination
710 : void alignMagStateDeclination();
711 :
712 : // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
713 : void fuseEulerYaw();
714 :
715 : // Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
716 : // Input is 1-sigma uncertainty in published declination
717 : void FuseDeclination(ftype declErr);
718 :
719 : // return magnetic declination in radians
720 : ftype MagDeclination(void) const;
721 :
722 : // Propagate PVA solution forward from the fusion time horizon to the current time horizon
723 : // using a simple observer
724 : void calcOutputStates();
725 :
726 : // calculate a filtered offset between baro height measurement and EKF height estimate
727 : void calcFiltBaroOffset();
728 :
729 : // correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
730 : void correctEkfOriginHeight();
731 :
732 : // Select height data to be fused from the available baro, range finder and GPS sources
733 : void selectHeightForFusion();
734 :
735 : // zero attitude state covariances, but preserve variances
736 : void zeroAttCovOnly();
737 :
738 : // record a yaw reset event
739 : void recordYawReset();
740 :
741 : // record a magnetic field state reset event
742 : void recordMagReset();
743 :
744 : // effective value of MAG_CAL
745 : uint8_t effective_magCal(void) const;
746 :
747 : // update timing statistics structure
748 : void updateTimingStatistics(void);
749 :
750 : // correct gps data for antenna position
751 : void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
752 :
753 : // correct external navigation earth-frame position using sensor body-frame offset
754 : void CorrectExtNavForSensorOffset(Vector3F &ext_position) const;
755 :
756 : // correct external navigation earth-frame velocity using sensor body-frame offset
757 : void CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const;
758 :
759 : // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
760 : // that uses IMU, GPS horizontal velocity and optionally true airspeed data.
761 : void runYawEstimatorPrediction(void);
762 :
763 : // Run the GPS velocity correction step for the GSF yaw estimator and use the
764 : // yaw estimate to reset the main EKF yaw if requested
765 : void runYawEstimatorCorrection(void);
766 :
767 : // reset the quaternion states using the supplied yaw angle, maintaining the previous roll and pitch
768 : // also reset the body to nav frame rotation matrix
769 : // reset the quaternion state covariances using the supplied yaw variance
770 : // yaw : new yaw angle (rad)
771 : // yaw_variance : variance of new yaw angle (rad^2)
772 : // isDeltaYaw : true when the yaw should be added to the existing yaw angle
773 : void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, bool isDeltaYaw);
774 :
775 : // attempt to reset the yaw to the EKF-GSF value
776 : // returns false if unsuccessful
777 : bool EKFGSF_resetMainFilterYaw();
778 :
779 : // Length of FIFO buffers used for non-IMU sensor data.
780 : // Must be larger than the time period defined by IMU_BUFFER_LENGTH
781 : static const uint32_t OBS_BUFFER_LENGTH = 5;
782 : static const uint32_t FLOW_BUFFER_LENGTH = 15;
783 : static const uint32_t EXTNAV_BUFFER_LENGTH = 15;
784 :
785 : static Matrix24 KH; // intermediate result used for covariance updates
786 : static Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
787 :
788 : // Variables
789 : bool statesInitialised; // boolean true when filter states have been initialised
790 : bool magHealth; // boolean true if magnetometer has passed innovation consistency check
791 : bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out
792 : bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
793 : bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
794 : bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
795 : bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
796 : bool badIMUdata; // boolean true if the bad IMU data is detected
797 :
798 : ftype gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
799 : Matrix24 P; // covariance matrix
800 : EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
801 : EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer
802 : EKF_obs_buffer_t<mag_elements> storedMag; // Magnetometer data buffer
803 : EKF_obs_buffer_t<baro_elements> storedBaro; // Baro data buffer
804 : EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer
805 : EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
806 : EKF_IMU_buffer_t<output_elements> storedOutput;// output state buffer
807 : Matrix3F prevTnb; // previous nav to body transformation used for INS earth rotation compensation
808 : ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
809 : ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
810 : Vector3F earthRateNED; // earths angular rate vector in NED (rad/s)
811 : ftype dtIMUavg; // expected time between IMU measurements (sec)
812 : ftype dtEkfAvg; // expected time between EKF updates (sec)
813 : ftype dt; // time lapsed since the last covariance prediction (sec)
814 : ftype hgtRate; // state for rate of change of height filter
815 : bool onGround; // true when the flight vehicle is definitely on the ground
816 : bool prevOnGround; // value of onGround from previous frame - used to detect transition
817 : bool inFlight; // true when the vehicle is definitely flying
818 : bool prevInFlight; // value inFlight from previous frame - used to detect transition
819 : bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity
820 : uint32_t airborneDetectTime_ms; // last time flight movement was detected
821 : Vector6 innovVelPos; // innovation output for a group of measurements
822 : Vector6 varInnovVelPos; // innovation variance output for a group of measurements
823 : Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s)
824 : bool fuseVelData; // this boolean causes the velNED measurements to be fused
825 : bool fusePosData; // this boolean causes the posNE measurements to be fused
826 : bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
827 : Vector3F innovMag; // innovation output from fusion of X,Y,Z compass measurements
828 : Vector3F varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
829 : ftype innovVtas; // innovation output from fusion of airspeed measurements
830 : ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
831 : bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step
832 : uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
833 : uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
834 : uint32_t lastMagUpdate_us; // last time compass was updated in usec
835 : uint32_t lastMagRead_ms; // last time compass data was successfully read
836 : Vector3F velDotNED; // rate of change of velocity in NED frame
837 : Vector3F velDotNEDfilt; // low pass filtered velDotNED
838 : uint32_t imuSampleTime_ms; // time that the last IMU value was taken
839 : bool tasDataToFuse; // true when new airspeed data is waiting to be fused
840 : uint32_t lastBaroReceived_ms; // time last time we received baro height data
841 : uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
842 : uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
843 : uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
844 : uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
845 : uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
846 : uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec)
847 : uint32_t lastTimeGpsReceived_ms;// last time we received GPS data
848 : uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements
849 : uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
850 : bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
851 : uint32_t lastYawTime_ms; // time stamp when yaw observation was last fused (msec)
852 : uint32_t ekfStartTime_ms; // time the EKF was started (msec)
853 : Vector2F lastKnownPositionNE; // last known position
854 : ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
855 : ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
856 : ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
857 : Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
858 : ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
859 : ftype defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
860 : bool inhibitWindStates; // true when wind states and covariances are to remain constant
861 : bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
862 : bool lastInhibitMagStates; // previous inhibitMagStates
863 : bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction
864 : bool gpsNotAvailable; // bool true when valid GPS data is not available
865 : uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
866 : Location EKF_origin; // LLH origin of the NED axis system
867 : bool validOrigin; // true when the EKF origin is valid
868 : ftype gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
869 : ftype gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
870 : ftype gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
871 : uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
872 : uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
873 : uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
874 : ftype posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
875 : bool useGpsVertVel; // true if GPS vertical velocity should be used
876 : uint16_t yawResetCount; // number of yaw resets performed by this core
877 : Vector3F tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion
878 : ftype tiltErrFilt; // Filtered tilt error metric
879 : bool tiltAlignComplete; // true when tilt alignment is complete
880 : bool yawAlignComplete; // true when yaw alignment is complete
881 : bool magStateInitComplete; // true when the magnetic field sttes have been initialised
882 : uint8_t stateIndexLim; // Max state index used during matrix and array operations
883 : imu_elements imuDataDelayed; // IMU data at the fusion time horizon
884 : imu_elements imuDataNew; // IMU data at the current time horizon
885 : imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
886 : QuaternionF imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
887 : baro_elements baroDataNew; // Baro data at the current time horizon
888 : baro_elements baroDataDelayed; // Baro data at the fusion time horizon
889 : range_elements rangeDataNew; // Range finder data at the current time horizon
890 : range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
891 : tas_elements tasDataNew; // TAS data at the current time horizon
892 : tas_elements tasDataDelayed; // TAS data at the fusion time horizon
893 : mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
894 : gps_elements gpsDataNew; // GPS data at the current time horizon
895 : gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
896 : output_elements outputDataNew; // output state data at the current time step
897 : output_elements outputDataDelayed; // output state data at the current time step
898 : Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
899 : Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m)
900 : Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec)
901 : ftype innovYaw; // compass yaw angle innovation (rad)
902 : uint32_t timeTasReceived_ms; // time last TAS data was received (msec)
903 : bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system
904 : uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
905 : bool consistentMagData; // true when the magnetometers are passing consistency checks
906 : bool motorsArmed; // true when the motors have been armed
907 : bool prevMotorsArmed; // value of motorsArmed from previous frame
908 : bool posVelFusionDelayed; // true when the position and velocity fusion has been delayed
909 : bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
910 : bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
911 : bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
912 : Vector3F lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
913 : bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
914 : Vector2F posResetNE; // Change in North/East position due to last in-flight reset in metres
915 : ftype posResetD; // Change in Down position due to last in-flight reset in metres
916 : uint16_t posNEResetCount; // number of NE position resets performed by this core
917 : uint16_t posDResetCount; // number of D position resets performed by this core
918 : ftype yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
919 : QuaternionF prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
920 : ftype hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
921 : uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
922 : bool runUpdates; // boolean true when the EKF updates can be run
923 : uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
924 : bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
925 : uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
926 : ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
927 : Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
928 : Vector3F delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
929 : bool magFieldLearned; // true when the magnetic field has been learned
930 : uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete
931 : Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
932 : Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)
933 : bool delAngBiasLearned; // true when the gyro bias has been learned
934 : nav_filter_status filterStatus; // contains the status of various filter outputs
935 : ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
936 : double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
937 : uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected
938 : Vector3F outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
939 : Vector3F velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
940 : Vector3F posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
941 :
942 : // variables used to calculate a vertical velocity that is kinematically consistent with the verical position
943 : struct {
944 : ftype pos;
945 : ftype vel;
946 : ftype acc;
947 : } vertCompFiltState;
948 :
949 : // variables used by the pre-initialisation GPS checks
950 : Location gpsloc_prev; // LLH location of previous GPS measurement
951 : uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks
952 : ftype gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
953 : ftype gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks
954 : ftype gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
955 :
956 : // variable used by the in-flight GPS quality check
957 : bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
958 : bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
959 : ftype sAccFilterState1; // state variable for LPF applid to reported GPS speed accuracy
960 : ftype sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
961 : uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
962 : uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed
963 : uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed
964 : bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
965 :
966 : // variables added for optical flow fusion
967 : EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
968 : of_elements ofDataNew; // OF data at the current time horizon
969 : of_elements ofDataDelayed; // OF data at the fusion time horizon
970 : bool flowDataToFuse; // true when optical flow data is ready for fusion
971 : bool flowDataValid; // true while optical flow data is still fresh
972 : Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
973 : uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
974 : uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
975 : uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
976 : uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
977 : Matrix3F Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
978 : Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
979 : Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
980 : ftype Popt; // Optical flow terrain height state covariance (m^2)
981 : ftype terrainState; // terrain position state (m)
982 : ftype prevPosN; // north position at last measurement
983 : ftype prevPosE; // east position at last measurement
984 : ftype varInnovRng; // range finder observation innovation variance (m^2)
985 : ftype innovRng; // range finder observation innovation (m)
986 : ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m)
987 : bool inhibitGndState; // true when the terrain position state is to remain constant
988 : uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks
989 : Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
990 : Vector2F auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
991 : ftype R_LOS; // variance of optical flow rate measurements (rad/sec)^2
992 : ftype auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
993 : Vector2F flowGyroBias; // bias error of optical flow sensor gyro output
994 : bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon.
995 : bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
996 : bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
997 : bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon
998 : enum AidingMode {
999 : 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.
1000 : 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.
1001 : AID_RELATIVE=2, // only optical flow aiding is being used so position estimates will be relative
1002 : };
1003 : AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
1004 : AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
1005 : bool gndOffsetValid; // true when the ground offset state can still be considered valid
1006 : Vector3F delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
1007 : ftype delTimeOF; // time that delAngBodyOF is summed across
1008 : Vector3F accelPosOffset; // position of IMU accelerometer unit in body frame (m)
1009 :
1010 :
1011 : // Range finder
1012 : ftype baroHgtOffset; // offset applied when when switching to use of Baro height
1013 : ftype rngOnGnd; // Expected range finder reading in metres when vehicle is on ground
1014 : uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement
1015 : bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference
1016 : #if AP_RANGEFINDER_ENABLED
1017 : ftype storedRngMeas[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3]; // Ringbuffer of stored range measurements for dual range sensors
1018 : uint32_t storedRngMeasTime_ms[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3]; // Ringbuffers of stored range measurement times for dual range sensors
1019 : uint8_t rngMeasIndex[DOWNWARD_RANGEFINDER_MAX_INSTANCES]; // Current range measurement ringbuffer index for dual range sensors
1020 : #endif
1021 :
1022 : // Range Beacon Sensor Fusion
1023 : EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
1024 : rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon
1025 : rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
1026 : uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec)
1027 : ftype rngBcnTestRatio; // Innovation test ratio for range beacon measurements
1028 : bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
1029 : bool rngBcnTimeout; // boolean true if range beacon measurements have faled innovation consistency checks for too long
1030 : ftype varInnovRngBcn; // range beacon observation innovation variance (m^2)
1031 : ftype innovRngBcn; // range beacon observation innovation (m)
1032 : uint32_t lastTimeRngBcn_ms[10]; // last time we received a range beacon measurement (msec)
1033 : #if AP_BEACON_ENABLED
1034 : bool rngBcnDataToFuse; // true when there is new range beacon data to fuse
1035 : #else
1036 : const bool rngBcnDataToFuse = false; // true when there is new range beacon data to fuse
1037 : #endif
1038 : Vector3F beaconVehiclePosNED; // NED position estimate from the beacon system (NED)
1039 : ftype beaconVehiclePosErr; // estimated position error from the beacon system (m)
1040 : uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec)
1041 : bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter
1042 : uint8_t lastRngBcnChecked; // index of the last range beacon checked for data
1043 : Vector3F receiverPos; // receiver NED position (m) - alignment 3 state filter
1044 : ftype receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter (
1045 : bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started
1046 : bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished
1047 : uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter
1048 : Vector3F rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
1049 : uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter
1050 : ftype rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter
1051 : uint8_t N_beacons; // Number of range beacons in use
1052 : ftype maxBcnPosD; // maximum position of all beacons in the down direction (m)
1053 : ftype minBcnPosD; // minimum position of all beacons in the down direction (m)
1054 : ftype bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
1055 :
1056 : ftype bcnPosOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
1057 : ftype bcnPosOffsetMaxVar; // Variance of the bcnPosOffsetHigh state (m)
1058 : ftype OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh
1059 :
1060 : ftype bcnPosOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
1061 : ftype bcnPosOffsetMinVar; // Variance of the bcnPosoffset state (m)
1062 : ftype OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow
1063 :
1064 : // Range Beacon Fusion Debug Reporting
1065 : uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported
1066 : struct rngBcnFusionReport_t {
1067 : ftype rng; // measured range to beacon (m)
1068 : ftype innov; // range innovation (m)
1069 : ftype innovVar; // innovation variance (m^2)
1070 : ftype testRatio; // innovation consistency test ratio
1071 : Vector3F beaconPosNED; // beacon NED position
1072 : } rngBcnFusionReport[10];
1073 :
1074 : // height source selection logic
1075 : uint8_t activeHgtSource; // integer defining active height source
1076 :
1077 : // Movement detector
1078 : bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
1079 : ftype rngAtStartOfFlight; // range finder measurement at start of flight
1080 : uint32_t timeAtArming_ms; // time in msec that the vehicle armed
1081 :
1082 : // baro ground effect
1083 : ftype meaHgtAtTakeOff; // height measured at commencement of takeoff
1084 :
1085 : // control of post takeoff magnetic field and heading resets
1086 : bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
1087 : bool finalInflightMagInit; // true when the final post takeoff initialisation of magnetic field states been performed
1088 : bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements
1089 : bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
1090 : bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
1091 : ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
1092 : ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
1093 : QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset
1094 : uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
1095 :
1096 : // external navigation fusion
1097 : EKF_obs_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
1098 : ext_nav_elements extNavDataNew; // External nav data at the current time horizon
1099 : ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
1100 : uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec)
1101 : uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
1102 : uint32_t lastExtNavPassTime_ms; // time stamp when external nav position measurement last passed innovation consistency check (msec)
1103 : bool extNavDataToFuse; // true when there is new external nav data to fuse
1104 : bool extNavUsedForYaw; // true when the external nav data is also being used as a yaw observation
1105 : bool extNavUsedForPos; // true when the external nav data is being used as a position reference.
1106 : bool extNavYawResetRequest; // true when a reset of vehicle yaw using the external nav data is requested
1107 :
1108 : EKF_obs_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer
1109 : ext_nav_vel_elements extNavVelNew; // external navigation velocity data at the current time horizon
1110 : ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon
1111 : uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
1112 : bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
1113 : bool useExtNavVel; // true external navigation velocity should be used
1114 :
1115 : // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
1116 : struct {
1117 : bool bad_xmag:1;
1118 : bool bad_ymag:1;
1119 : bool bad_zmag:1;
1120 : bool bad_airspeed:1;
1121 : bool bad_sideslip:1;
1122 : bool bad_nvel:1;
1123 : bool bad_evel:1;
1124 : bool bad_dvel:1;
1125 : bool bad_npos:1;
1126 : bool bad_epos:1;
1127 : bool bad_dpos:1;
1128 : bool bad_yaw:1;
1129 : bool bad_decl:1;
1130 : bool bad_xflow:1;
1131 : bool bad_yflow:1;
1132 : bool bad_rngbcn:1;
1133 : } faultStatus;
1134 :
1135 : // flags indicating which GPS quality checks are failing
1136 : struct {
1137 : bool bad_sAcc:1;
1138 : bool bad_hAcc:1;
1139 : bool bad_vAcc:1;
1140 : bool bad_yaw:1;
1141 : bool bad_sats:1;
1142 : bool bad_VZ:1;
1143 : bool bad_horiz_drift:1;
1144 : bool bad_hdop:1;
1145 : bool bad_vert_vel:1;
1146 : bool bad_fix:1;
1147 : bool bad_horiz_vel:1;
1148 : } gpsCheckStatus;
1149 :
1150 : // states held by magnetomter fusion across time steps
1151 : // magnetometer X,Y,Z measurements are fused across three time steps
1152 : // to level computational load as this is an expensive operation
1153 : struct {
1154 : ftype q0;
1155 : ftype q1;
1156 : ftype q2;
1157 : ftype q3;
1158 : ftype magN;
1159 : ftype magE;
1160 : ftype magD;
1161 : ftype magXbias;
1162 : ftype magYbias;
1163 : ftype magZbias;
1164 : Matrix3F DCM;
1165 : Vector3F MagPred;
1166 : ftype R_MAG;
1167 : Vector9 SH_MAG;
1168 : } mag_state;
1169 :
1170 : // string representing last reason for prearm failure
1171 : char prearm_fail_string[41];
1172 :
1173 : // earth field from WMM tables
1174 : bool have_table_earth_field; // true when we have initialised table_earth_field_ga
1175 : Vector3F table_earth_field_ga; // earth field from WMM tables
1176 : ftype table_declination; // declination in radians from the tables
1177 :
1178 : // timing statistics
1179 : struct ekf_timing timing;
1180 :
1181 : // when was attitude filter status last non-zero?
1182 : uint32_t last_filter_ok_ms;
1183 :
1184 : // should we assume zero sideslip?
1185 : bool assume_zero_sideslip(void) const;
1186 :
1187 : // vehicle specific initial gyro bias uncertainty
1188 : ftype InitialGyroBiasUncertainty(void) const;
1189 :
1190 : // The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
1191 : uint32_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec)
1192 : uint32_t EKFGSF_yaw_reset_request_ms; // timestamp of last emergency yaw reset request (uSec)
1193 : uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed
1194 : bool EKFGSF_run_filterbank; // true when the filter bank is active
1195 :
1196 : // logging timestamps
1197 : uint32_t lastTimingLogTime_ms;
1198 :
1199 : // logging functions shared by cores:
1200 : void Log_Write_NKF1(uint64_t time_us) const;
1201 : void Log_Write_NKF2(uint64_t time_us) const;
1202 : void Log_Write_NKF3(uint64_t time_us) const;
1203 : void Log_Write_NKF4(uint64_t time_us) const;
1204 : void Log_Write_NKF5(uint64_t time_us) const;
1205 : void Log_Write_Quaternion(uint64_t time_us) const;
1206 : void Log_Write_Beacon(uint64_t time_us);
1207 : void Log_Write_Timing(uint64_t time_us);
1208 : void Log_Write_GSF(uint64_t time_us) const;
1209 : };
|
