forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 2
/
AP_NavEKF2_core.h
1294 lines (1067 loc) · 67.4 KB
/
AP_NavEKF2_core.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
24 state EKF based on the derivation in https://github.com/priseborough/
InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/
GenerateNavFilterEquations.m
Converted from Matlab to C++ by Paul Riseborough
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#if !defined(HAL_DEBUG_BUILD) || !HAL_DEBUG_BUILD
#pragma GCC optimize("O2")
#endif
#define EK2_DISABLE_INTERRUPTS 0
#include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/vectorN.h>
#include <AP_NavEKF/AP_NavEKF_core_common.h>
#include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_NavEKF/EKFGSF_yaw.h"
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_HDOP (1<<1)
#define MASK_GPS_SPD_ERR (1<<2)
#define MASK_GPS_POS_ERR (1<<3)
#define MASK_GPS_YAW_ERR (1<<4)
#define MASK_GPS_POS_DRIFT (1<<5)
#define MASK_GPS_VERT_SPD (1<<6)
#define MASK_GPS_HORIZ_SPD (1<<7)
// active height source
#define HGT_SOURCE_BARO 0
#define HGT_SOURCE_RNG 1
#define HGT_SOURCE_GPS 2
#define HGT_SOURCE_BCN 3
#define HGT_SOURCE_EXTNAV 4
// target EKF update time step
#define EKF_TARGET_DT 0.01f
// mag fusion final reset altitude
#define EKF2_MAG_FINAL_RESET_ALT 2.5f
// maximum number of yaw resets due to detected magnetic anomaly allowed per flight
#define MAG_ANOMALY_RESET_MAX 2
// number of seconds a request to reset the yaw to the GSF estimate is active before it times out
#define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
class AP_AHRS;
class NavEKF2_core : public NavEKF_core_common
{
public:
// Constructor
NavEKF2_core(class NavEKF2 *_frontend);
// setup this core backend
bool setup_core(uint8_t _imu_index, uint8_t _core_index);
// Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static
bool InitialiseFilterBootstrap(void);
// Update Filter States - this should be called whenever new IMU data is available
// The predict flag is set true when a new prediction cycle can be started
void UpdateFilter(bool predict);
// Check basic filter health metrics and return a consolidated health status
bool healthy(void) const;
// Return a consolidated error score where higher numbers are less healthy
// Intended to be used by the front-end to determine which is the primary EKF
float errorScore(void) const;
// Write the last calculated NE position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool getPosNE(Vector2f &posNE) const;
// Write the last calculated D position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control
bool getPosD(float &posD) const;
// return NED velocity in m/s
void getVelNED(Vector3f &vel) const;
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
// 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
// but will always be kinematically consistent with the z component of the EKF position state
float getPosDownDerivative(void) const;
// This returns the specific forces in the NED frame
void getAccelNED(Vector3f &accelNED) const;
// return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const;
// return body axis gyro scale factor error as a percentage
void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;
// return tilt error convergence metric
void getTiltError(float &ang) const;
// reset body axis gyro bias estimates
void resetGyroBias(void);
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool resetHeightDatum(void);
// Commands the EKF to not use GPS.
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t setInhibitGPS(void);
// return the horizontal speed limit in m/s set by optical flow sensor limits
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return the Z-accel bias estimate in m/s^2
void getAccelZBias(float &zbias) const;
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void getWind(Vector3f &wind) const;
// return earth magnetic field estimates in measurement units / 1000
void getMagNED(Vector3f &magNED) const;
// return body magnetic field estimates in measurement units / 1000
void getMagXYZ(Vector3f &magXYZ) const;
// return the index for the active magnetometer
uint8_t getActiveMag() const;
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
// Return the last calculated latitude, longitude and height in WGS-84
// If a calculated location isn't available, return a raw GPS measurement
// The status will return true if a calculation or raw measurement is available
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
bool getLLH(struct Location &loc) const;
// return the latitude and longitude and height used to set the NED origin
// All NED positions calculated by the filter are relative to this location
// Returns false if the origin has not been set
bool getOriginLLH(struct Location &loc) const;
// set the latitude and longitude and height used to set the NED origin
// All NED positions calculated by the filter will be relative to this location
// The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
// Returns false if the filter has rejected the attempt to set the origin
bool setOriginLLH(const Location &loc);
// return estimated height above ground level
// return false if ground height is not being estimated.
bool getHAGL(float &HAGL) const;
// return the Euler roll, pitch and yaw angle in radians
void getEulerAngles(Vector3f &eulers) const;
// return the transformation matrix from XYZ (body) to NED axes
void getRotationBodyToNED(Matrix3f &mat) const;
// return the quaternions defining the rotation from NED to XYZ (body) axes
void getQuaternion(Quaternion &quat) const;
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool use_compass(void) const;
// write the raw optical flow measurements
// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
// rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
// rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// return data for debugging optical flow fusion
void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
/*
Returns the following data for debugging range beacon fusion
ID : beacon identifier
rng : measured range to beacon (m)
innov : range innovation (m)
innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
*/
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTakeoffExpected(bool val);
// called by vehicle code to specify that a touchdown is expected to happen
// causes the EKF to compensate for expected barometer errors due to ground effect
void setTouchdownExpected(bool val);
// Set to true if the terrain underneath is stable enough to be used as a height reference
// in combination with a range finder. Set to false if the terrain underneath the vehicle
// cannot be used as a height reference. Use to prevent range finder operation otherwise
// enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
void setTerrainHgtStable(bool val);
/*
return the filter fault status as a bitmasked integer
0 = quaternions are NaN
1 = velocities are NaN
2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion
4 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised
*/
void getFilterFaults(uint16_t &faults) const;
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
7 = unassigned
*/
void getFilterTimeouts(uint8_t &timeouts) const;
/*
return filter gps quality check status
*/
void getFilterGpsStatus(nav_gps_status &status) const;
/*
Return a filter function status that indicates:
Which outputs are valid
If the filter has detected takeoff
If the filter has activated the mode that mitigates against ground effect static pressure errors
If GPS data is being used
*/
void getFilterStatus(nav_filter_status &status) const;
// send an EKF_STATUS_REPORT message to GCS
void send_status_report(mavlink_channel_t chan) const;
// provides the height limit to be observed by the control loops
// returns false if no height limiting is required
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
bool getHeightControlLimit(float &height) const;
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAng) const;
// return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosNorthEastReset(Vector2f &pos) const;
// return the amount of D position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosDownReset(float &posD) const;
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const;
// report the number of frames lapsed since the last state prediction
// this is used by other instances to level load
uint8_t getFramesSincePredict(void) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(Vector3f &error) const;
// get the IMU index. For now we return the gyro index, as that is most
// critical for use by other subsystems.
uint8_t getIMUIndex(void) const { return gyro_index_active; }
// get timing statistics structure
void getTimingStatistics(struct ekf_timing &timing);
/*
* Write position and quaternion data from an external navigation system
*
* pos : position in the RH navigation frame. Frame is assumed to be NED (m)
* quat : quaternion desribing the rotation from navigation frame to body frame
* posErr : 1-sigma spherical position error (m)
* angErr : 1-sigma spherical angle error (rad)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
* resetTime_ms : system time of the last position reset request (mSec)
*
* Sensor offsets are pulled directly from the AP_VisualOdom library
*
*/
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
/*
* Write velocity data from an external navigation system
* vel : velocity in NED (m)
* err : velocity error (m/s)
* timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
* delay_ms : average delay of external nav system measurements relative to inertial measurements
*/
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
// return true when external nav data is also being used as a yaw observation
bool isExtNavUsedForYaw(void);
// get solution data for the EKF-GSF emergency yaw estimator
// return false if data not available
bool getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed);
// request a reset the yaw to the EKF-GSF value
void EKFGSF_requestYawReset();
private:
EKFGSF_yaw *yawEstimator;
// Reference to the global EKF frontend for parameters
class NavEKF2 *frontend;
uint8_t imu_index; // preferred IMU index
uint8_t gyro_index_active; // active gyro index (in case preferred fails)
uint8_t accel_index_active; // active accel index (in case preferred fails)
uint8_t core_index;
uint8_t imu_buffer_length;
#if MATH_CHECK_INDEXES
typedef VectorN<ftype,2> Vector2;
typedef VectorN<ftype,3> Vector3;
typedef VectorN<ftype,4> Vector4;
typedef VectorN<ftype,5> Vector5;
typedef VectorN<ftype,6> Vector6;
typedef VectorN<ftype,7> Vector7;
typedef VectorN<ftype,8> Vector8;
typedef VectorN<ftype,9> Vector9;
typedef VectorN<ftype,10> Vector10;
typedef VectorN<ftype,11> Vector11;
typedef VectorN<ftype,13> Vector13;
typedef VectorN<ftype,14> Vector14;
typedef VectorN<ftype,15> Vector15;
typedef VectorN<ftype,22> Vector22;
typedef VectorN<ftype,23> Vector23;
typedef VectorN<ftype,24> Vector24;
typedef VectorN<ftype,25> Vector25;
typedef VectorN<ftype,31> Vector31;
typedef VectorN<VectorN<ftype,3>,3> Matrix3;
typedef VectorN<VectorN<ftype,24>,24> Matrix24;
typedef VectorN<VectorN<ftype,34>,50> Matrix34_50;
typedef VectorN<uint32_t,50> Vector_u32_50;
#else
typedef ftype Vector2[2];
typedef ftype Vector3[3];
typedef ftype Vector4[4];
typedef ftype Vector5[5];
typedef ftype Vector6[6];
typedef ftype Vector7[7];
typedef ftype Vector8[8];
typedef ftype Vector9[9];
typedef ftype Vector10[10];
typedef ftype Vector11[11];
typedef ftype Vector13[13];
typedef ftype Vector14[14];
typedef ftype Vector15[15];
typedef ftype Vector22[22];
typedef ftype Vector23[23];
typedef ftype Vector24[24];
typedef ftype Vector25[25];
typedef ftype Matrix3[3][3];
typedef ftype Matrix24[24][24];
typedef ftype Matrix34_50[34][50];
typedef uint32_t Vector_u32_50[50];
#endif
const AP_AHRS *_ahrs;
// the states are available in two forms, either as a Vector31, or
// broken down as individual elements. Both are equivalent (same
// memory)
struct state_elements {
Vector3f angErr; // 0..2
Vector3f velocity; // 3..5
Vector3f position; // 6..8
Vector3f gyro_bias; // 9..11
Vector3f gyro_scale; // 12..14
float accel_zbias; // 15
Vector3f earth_magfield; // 16..18
Vector3f body_magfield; // 19..21
Vector2f wind_vel; // 22..23
Quaternion quat; // 24..27
};
union {
Vector28 statesArray;
struct state_elements stateStruct;
};
struct output_elements {
Quaternion quat; // 0..3
Vector3f velocity; // 4..6
Vector3f position; // 7..9
};
struct imu_elements {
Vector3f delAng; // 0..2
Vector3f delVel; // 3..5
float delAngDT; // 6
float delVelDT; // 7
uint32_t time_ms; // 8
uint8_t gyro_index;
uint8_t accel_index;
};
struct gps_elements {
Vector2f pos; // 0..1
float hgt; // 2
Vector3f vel; // 3..5
uint32_t time_ms; // 6
uint8_t sensor_idx; // 7..9
};
struct mag_elements {
Vector3f mag; // 0..2
uint32_t time_ms; // 3
};
struct baro_elements {
float hgt; // 0
uint32_t time_ms; // 1
};
struct range_elements {
float rng; // 0
uint32_t time_ms; // 1
uint8_t sensor_idx; // 2
};
struct rng_bcn_elements {
float rng; // range measurement to each beacon (m)
Vector3f beacon_posNED; // NED position of the beacon (m)
float rngErr; // range measurement error 1-std (m)
uint8_t beacon_ID; // beacon identification number
uint32_t time_ms; // measurement timestamp (msec)
};
struct tas_elements {
float tas; // 0
uint32_t time_ms; // 1
};
struct of_elements {
Vector2f flowRadXY; // 0..1
Vector2f flowRadXYcomp; // 2..3
uint32_t time_ms; // 4
Vector3f bodyRadXYZ; //8..10
const Vector3f *body_offset;// 5..7
};
struct ext_nav_elements {
Vector3f pos; // XYZ position measured in a RH navigation frame (m)
Quaternion quat; // quaternion describing the rotation from navigation to body frame
float posErr; // spherical poition measurement error 1-std (m)
float angErr; // spherical angular measurement error 1-std (rad)
uint32_t time_ms; // measurement timestamp (msec)
bool posReset; // true when the position measurement has been reset
};
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
Vector3f gyro_bias;
Vector3f gyro_scale;
float accel_zbias;
} inactiveBias[INS_MAX_INSTANCES];
struct ext_nav_vel_elements {
Vector3f vel; // velocity in NED (m)
float err; // velocity measurement error (m/s)
uint32_t time_ms; // measurement timestamp (msec)
};
// update the navigation filter status
void updateFilterStatus(void);
// update the quaternion, velocity and position states using IMU measurements
void UpdateStrapdownEquationsNED();
// calculate the predicted state covariance matrix
void CovariancePrediction();
// force symmetry on the state covariance matrix
void ForceSymmetry();
// copy covariances across from covariance prediction calculation and fix numerical errors
void CopyCovariances();
// constrain variances (diagonal terms) in the state covariance matrix
void ConstrainVariances();
// constrain states
void ConstrainStates();
// constrain earth field using WMM tables
void MagTableConstrain(void);
// fuse selected position, velocity and height measurements
void FuseVelPosNED();
// fuse range beacon measurements
void FuseRngBcn();
// use range beacon measurements to calculate a static position
void FuseRngBcnStatic();
// calculate the offset from EKF vertical position datum to the range beacon system datum
void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning);
// fuse magnetometer measurements
void FuseMagnetometer();
// fuse true airspeed measurements
void FuseAirspeed();
// fuse synthetic sideslip measurement of zero
void FuseSideslip();
// zero specified range of rows in the state covariance matrix
void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last);
// zero specified range of columns in the state covariance matrix
void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);
// Reset the stored output history to current data
void StoreOutputReset(void);
// Reset the stored output quaternion history to current EKF state
void StoreQuatReset(void);
// Rotate the stored output quaternion history through a quaternion rotation
void StoreQuatRotate(const Quaternion &deltaQuat);
// store altimeter data
void StoreBaro();
// recall altimeter data at the fusion time horizon
// return true if data found
bool RecallBaro();
// store range finder data
void StoreRange();
// recall range finder data at the fusion time horizon
// return true if data found
bool RecallRange();
// store magnetometer data
void StoreMag();
// recall magetometer data at the fusion time horizon
// return true if data found
bool RecallMag();
// store true airspeed data
void StoreTAS();
// recall true airspeed data at the fusion time horizon
// return true if data found
bool RecallTAS();
// store optical flow data
void StoreOF();
// recall optical flow data at the fusion time horizon
// return true if data found
bool RecallOF();
// calculate nav to body quaternions from body to nav rotation matrix
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
// calculate the NED earth spin vector in rad/sec
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
// initialise the covariance matrix
void CovarianceInit();
// helper functions for readIMUData
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt);
// helper functions for correcting IMU data
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);
void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index);
// update IMU delta angle and delta velocity measurements
void readIMUData();
// update estimate of inactive bias states
void learnInactiveBiases();
// check for new valid GPS data and update stored measurement if available
void readGpsData();
// check for new altitude measurement data and update stored measurement if available
void readBaroData();
// check for new magnetometer data and update store measurements if available
void readMagData();
// check for new airspeed data and update stored measurements if available
void readAirSpdData();
// check for new range beacon data and update stored measurements if available
void readRngBcnData();
// determine when to perform fusion of GPS position and velocity measurements
void SelectVelPosFusion();
// determine when to perform fusion of range measurements take relative to a beacon at a known NED position
void SelectRngBcnFusion();
// determine when to perform fusion of magnetometer measurements
void SelectMagFusion();
// determine when to perform fusion of true airspeed measurements
void SelectTasFusion();
// determine when to perform fusion of synthetic sideslp measurements
void SelectBetaFusion();
// force alignment of the yaw angle using GPS velocity data
void realignYawGPS();
// initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
// and return attitude quaternion
Quaternion calcQuatAndFieldStates(float roll, float pitch);
// zero stored variables
void InitialiseVariables();
void InitialiseVariablesMag();
// reset the horizontal position states uing the last GPS measurement
void ResetPosition(void);
// reset the stateStruct's NE position to the specified position
void ResetPositionNE(float posN, float posE);
// reset velocity states using the last GPS measurement
void ResetVelocity(void);
// reset the vertical position state using the last height measurement
void ResetHeight(void);
// reset the stateStruct's D position
void ResetPositionD(float posD);
// return true if we should use the airspeed sensor
bool useAirspeed(void) const;
// return true if the vehicle code has requested the filter to be ready for flight
bool readyToUseGPS(void) const;
// return true if the filter to be ready to use the beacon range measurements
bool readyToUseRangeBeacon(void) const;
// return true if the filter to be ready to use external nav data
bool readyToUseExtNav(void) const;
// Check for filter divergence
void checkDivergence(void);
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting(float K1, float K2);
// return true if optical flow data is available
bool optFlowDataPresent(void) const;
// return true if we should use the range finder sensor
bool useRngFinder(void) const;
// determine when to perform fusion of optical flow measurements
void SelectFlowFusion();
// Estimate terrain offset using a single state EKF
void EstimateTerrainOffset();
// fuse optical flow measurements into the main filter
void FuseOptFlow();
// Control filter mode changes
void controlFilterModes();
// Determine if we are flying or on the ground
void detectFlight();
// Set inertial navigaton aiding mode
void setAidingMode();
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void setWindMagStateLearningMode();
// Check the alignmnent status of the tilt attitude
// Used during initial bootstrap alignment of the filter
void checkAttitudeAlignmentStatus();
// Control reset of yaw and magnetic field states
void controlMagYawReset();
// Set the NED origin to be used until the next filter reset
void setOrigin(const Location &loc);
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
bool getTakeoffExpected();
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
bool getTouchdownExpected();
// Assess GPS data quality and set gpsGoodToAlign if good enough to align the EKF
void calcGpsGoodToAlign(void);
// return true and set the class variable true if the delta angle bias has been learned
bool checkGyroCalStatus(void);
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
void calcGpsGoodForFlight(void);
// Read the range finder and take new measurements if available
// Apply a median filter to range finder data
void readRangeFinder();
// check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
void detectOptFlowTakeoff(void);
// align the NE earth magnetic field states with the published declination
void alignMagStateDeclination();
// Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
void fuseEulerYaw();
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
// Input is 1-sigma uncertainty in published declination
void FuseDeclination(float declErr);
// return magnetic declination in radians
float MagDeclination(void) const;
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer
void calcOutputStates();
// calculate a filtered offset between baro height measurement and EKF height estimate
void calcFiltBaroOffset();
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void correctEkfOriginHeight();
// Select height data to be fused from the available baro, range finder and GPS sources
void selectHeightForFusion();
// zero attitude state covariances, but preserve variances
void zeroAttCovOnly();
// record a yaw reset event
void recordYawReset();
// record a magnetic field state reset event
void recordMagReset();
// effective value of MAG_CAL
uint8_t effective_magCal(void) const;
// update timing statistics structure
void updateTimingStatistics(void);
// correct gps data for antenna position
void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
// correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset(Vector3f &ext_position) const;
// correct external navigation earth-frame velocity using sensor body-frame offset
void CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
void runYawEstimatorPrediction(void);
// Run the GPS velocity correction step for the GSF yaw estimator and use the
// yaw estimate to reset the main EKF yaw if requested
void runYawEstimatorCorrection(void);
// reset the quaternion states using the supplied yaw angle, maintaining the previous roll and pitch
// also reset the body to nav frame rotation matrix
// reset the quaternion state covariances using the supplied yaw variance
// yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2)
// isDeltaYaw : true when the yaw should be added to the existing yaw angle
void resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw);
// attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful
bool EKFGSF_resetMainFilterYaw();
// Length of FIFO buffers used for non-IMU sensor data.
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
static const uint32_t OBS_BUFFER_LENGTH = 5;
static const uint32_t FLOW_BUFFER_LENGTH = 15;
static const uint32_t EXTNAV_BUFFER_LENGTH = 15;
// Variables
bool statesInitialised; // boolean true when filter states have been initialised
bool velHealth; // boolean true if velocity measurements have passed innovation consistency check
bool posHealth; // boolean true if position measurements have passed innovation consistency check
bool hgtHealth; // boolean true if height measurements have passed innovation consistency check
bool magHealth; // boolean true if magnetometer has passed innovation consistency check
bool tasHealth; // boolean true if true airspeed has passed innovation consistency check
bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data
bool badIMUdata; // boolean true if the bad IMU data is detected
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P; // covariance matrix
imu_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer
obs_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer
obs_ring_buffer_t<mag_elements> storedMag; // Magnetometer data buffer
obs_ring_buffer_t<baro_elements> storedBaro; // Baro data buffer
obs_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer
obs_ring_buffer_t<range_elements> storedRange; // Range finder data buffer
imu_ring_buffer_t<output_elements> storedOutput;// output state buffer
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
ftype dtIMUavg; // expected time between IMU measurements (sec)
ftype dtEkfAvg; // expected time between EKF updates (sec)
ftype dt; // time lapsed since the last covariance prediction (sec)
ftype hgtRate; // state for rate of change of height filter
bool onGround; // true when the flight vehicle is definitely on the ground
bool prevOnGround; // value of onGround from previous frame - used to detect transition
bool inFlight; // true when the vehicle is definitely flying
bool prevInFlight; // value inFlight from previous frame - used to detect transition
bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity
uint32_t airborneDetectTime_ms; // last time flight movement was detected
Vector6 innovVelPos; // innovation output for a group of measurements
Vector6 varInnovVelPos; // innovation variance output for a group of measurements
Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s)
bool fuseVelData; // this boolean causes the velNED measurements to be fused
bool fusePosData; // this boolean causes the posNE measurements to be fused
bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
uint32_t lastMagUpdate_us; // last time compass was updated in usec
Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED
uint32_t imuSampleTime_ms; // time that the last IMU value was taken
bool tasDataToFuse; // true when new airspeed data is waiting to be fused
uint32_t lastBaroReceived_ms; // time last time we received baro height data
uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastTimeGpsReceived_ms;// last time we received GPS data
uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements
uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector2f lastKnownPositionNE; // last known position
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold
float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold
Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool gpsNotAvailable; // bool true when valid GPS data is not available
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
struct Location EKF_origin; // LLH origin of the NED axis system
bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
uint32_t lastGpsVelPass_ms; // time of last GPS vertical velocity consistency check pass
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
bool useGpsVertVel; // true if GPS vertical velocity should be used
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
Vector3f tiltErrVec; // Vector of most recent attitude error correction from Vel,Pos fusion
float tiltErrFilt; // Filtered tilt error metric
bool tiltAlignComplete; // true when tilt alignment is complete
bool yawAlignComplete; // true when yaw alignment is complete
bool magStateInitComplete; // true when the magnetic field sttes have been initialised
uint8_t stateIndexLim; // Max state index used during matrix and array operations
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
imu_elements imuDataNew; // IMU data at the current time horizon
imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
uint8_t fifoIndexNow; // Global index for inertial and output solution at current time horizon
uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon
baro_elements baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion time horizon
uint8_t baroStoreIndex; // Baro data storage index
range_elements rangeDataNew; // Range finder data at the current time horizon
range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
uint8_t rangeStoreIndex; // Range finder data storage index
tas_elements tasDataNew; // TAS data at the current time horizon
tas_elements tasDataDelayed; // TAS data at the fusion time horizon
uint8_t tasStoreIndex; // TAS data storage index
mag_elements magDataNew; // Magnetometer data at the current time horizon
mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
uint8_t magStoreIndex; // Magnetometer data storage index
gps_elements gpsDataNew; // GPS data at the current time horizon
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
uint8_t gpsStoreIndex; // GPS data storage index
output_elements outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // output state data at the current time step
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
Vector3f velErrintegral; // integral of output predictor NED velocity tracking error (m)
Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec)
float innovYaw; // compass yaw angle innovation (rad)
uint32_t timeTasReceived_ms; // time last TAS data was received (msec)
bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
bool consistentMagData; // true when the magnetometers are passing consistency checks
bool motorsArmed; // true when the motors have been armed
bool prevMotorsArmed; // value of motorsArmed from previous frame
bool posVelFusionDelayed; // true when the position and velocity fusion has been delayed
bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
float posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
uint32_t lastPosResetD_ms; // System time at which the last position reset occurred. Returned by getLastPosDownReset
float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold