forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 4
/
AP_NavEKF3.cpp
2075 lines (1811 loc) · 92.8 KB
/
AP_NavEKF3.cpp
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
#include <AP_HAL/AP_HAL.h>
#include "AP_NavEKF3_core.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "AP_DAL/AP_DAL.h"
#include <new>
/*
parameter defaults for different types of vehicle. The
APM_BUILD_DIRECTORY is taken from the main vehicle directory name
where the code is built.
*/
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Replay)
// copter defaults
#define VELNE_M_NSE_DEFAULT 0.3f
#define VELD_M_NSE_DEFAULT 0.5f
#define POSNE_M_NSE_DEFAULT 0.5f
#define ALT_M_NSE_DEFAULT 2.0f
#define MAG_M_NSE_DEFAULT 0.05f
#define GYRO_P_NSE_DEFAULT 1.5E-02f
#define ACC_P_NSE_DEFAULT 3.5E-01f
#define GBIAS_P_NSE_DEFAULT 1.0E-03f
#define ABIAS_P_NSE_DEFAULT 2.0E-02f
#define MAGB_P_NSE_DEFAULT 1.0E-04f
#define MAGE_P_NSE_DEFAULT 1.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
#define MAG_I_GATE_DEFAULT 300
#define MAG_CAL_DEFAULT 3
#define GLITCH_RADIUS_DEFAULT 25
#define FLOW_MEAS_DELAY 10
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_DEFAULT 1
#define WIND_P_NSE_DEFAULT 0.2
#elif APM_BUILD_TYPE(APM_BUILD_Rover)
// rover defaults
#define VELNE_M_NSE_DEFAULT 0.5f
#define VELD_M_NSE_DEFAULT 0.7f
#define POSNE_M_NSE_DEFAULT 0.5f
#define ALT_M_NSE_DEFAULT 2.0f
#define MAG_M_NSE_DEFAULT 0.05f
#define GYRO_P_NSE_DEFAULT 1.5E-02f
#define ACC_P_NSE_DEFAULT 3.5E-01f
#define GBIAS_P_NSE_DEFAULT 1.0E-03f
#define ABIAS_P_NSE_DEFAULT 2.0E-02f
#define MAGB_P_NSE_DEFAULT 1.0E-04f
#define MAGE_P_NSE_DEFAULT 1.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
#define MAG_I_GATE_DEFAULT 300
#define MAG_CAL_DEFAULT 2
#define GLITCH_RADIUS_DEFAULT 25
#define FLOW_MEAS_DELAY 10
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_DEFAULT 1
#define WIND_P_NSE_DEFAULT 0.1
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// plane defaults
#define VELNE_M_NSE_DEFAULT 0.5f
#define VELD_M_NSE_DEFAULT 0.7f
#define POSNE_M_NSE_DEFAULT 0.5f
#define ALT_M_NSE_DEFAULT 3.0f
#define MAG_M_NSE_DEFAULT 0.05f
#define GYRO_P_NSE_DEFAULT 1.5E-02f
#define ACC_P_NSE_DEFAULT 3.5E-01f
#define GBIAS_P_NSE_DEFAULT 1.0E-03f
#define ABIAS_P_NSE_DEFAULT 2.0E-02f
#define MAGB_P_NSE_DEFAULT 1.0E-04f
#define MAGE_P_NSE_DEFAULT 1.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
#define MAG_I_GATE_DEFAULT 300
#define MAG_CAL_DEFAULT 0
#define GLITCH_RADIUS_DEFAULT 25
#define FLOW_MEAS_DELAY 10
#define FLOW_M_NSE_DEFAULT 0.15f
#define FLOW_I_GATE_DEFAULT 500
#define CHECK_SCALER_DEFAULT 150
#define FLOW_USE_DEFAULT 2
#define WIND_P_NSE_DEFAULT 0.1
#else
// build type not specified, use copter defaults
#define VELNE_M_NSE_DEFAULT 0.5f
#define VELD_M_NSE_DEFAULT 0.7f
#define POSNE_M_NSE_DEFAULT 0.5f
#define ALT_M_NSE_DEFAULT 2.0f
#define MAG_M_NSE_DEFAULT 0.05f
#define GYRO_P_NSE_DEFAULT 1.5E-02f
#define ACC_P_NSE_DEFAULT 3.5E-01f
#define GBIAS_P_NSE_DEFAULT 1.0E-03f
#define ABIAS_P_NSE_DEFAULT 2.0E-02f
#define MAGB_P_NSE_DEFAULT 1.0E-04f
#define MAGE_P_NSE_DEFAULT 1.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
#define MAG_I_GATE_DEFAULT 300
#define MAG_CAL_DEFAULT 3
#define GLITCH_RADIUS_DEFAULT 25
#define FLOW_MEAS_DELAY 10
#define FLOW_M_NSE_DEFAULT 0.25f
#define FLOW_I_GATE_DEFAULT 300
#define CHECK_SCALER_DEFAULT 100
#define FLOW_USE_DEFAULT 1
#define WIND_P_NSE_DEFAULT 0.1
#endif // APM_BUILD_DIRECTORY
#ifndef EK3_PRIMARY_DEFAULT
#define EK3_PRIMARY_DEFAULT 0
#endif
// This allows boards to default to using a specified number of IMUs and EKF lanes
#ifndef HAL_EKF_IMU_MASK_DEFAULT
#define HAL_EKF_IMU_MASK_DEFAULT 3 // Default to using two IMUs
#endif
// Define tuning parameters
const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable EKF3
// @Description: This enables EKF3. Enabling EKF3 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_TYPE=3. A reboot or restart will need to be performed after changing the value of EK3_ENABLE for it to take effect.
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF3, _enable, 1, AP_PARAM_FLAG_ENABLE),
// GPS measurement parameters
// 1 was GPS_TYPE
// @Param: VELNE_M_NSE
// @DisplayName: GPS horizontal velocity measurement noise (m/s)
// @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set horizontal velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS horizontal velocity measurements.
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF3, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT),
// @Param: VELD_M_NSE
// @DisplayName: GPS vertical velocity measurement noise (m/s)
// @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set vertical velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS vertical velocity measurements.
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VELD_M_NSE", 3, NavEKF3, _gpsVertVelNoise, VELD_M_NSE_DEFAULT),
// @Param: VEL_I_GATE
// @DisplayName: GPS velocity innovation gate size
// @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD set to 0 the velocity innovations will be clipped instead of rejected if they exceed the gate size and a smaller value of EK3_VEL_I_GATE not exceeding 300 is recommended to limit the effect of GPS transient errors.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("VEL_I_GATE", 4, NavEKF3, _gpsVelInnovGate, VEL_I_GATE_DEFAULT),
// @Param: POSNE_M_NSE
// @DisplayName: GPS horizontal position measurement noise (m)
// @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("POSNE_M_NSE", 5, NavEKF3, _gpsHorizPosNoise, POSNE_M_NSE_DEFAULT),
// @Param: POS_I_GATE
// @DisplayName: GPS position measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD has been set to 0 the horizontal position innovations will be clipped instead of rejected if they exceed the gate size so a smaller value of EK3_POS_I_GATE not exceeding 300 is recommended to limit the effect of GPS transient errors.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("POS_I_GATE", 6, NavEKF3, _gpsPosInnovGate, POS_I_GATE_DEFAULT),
// @Param: GLITCH_RAD
// @DisplayName: GPS glitch radius gate size (m)
// @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position. If EK3_GLITCH_RAD set to 0 the GPS innovations will be clipped instead of rejected if they exceed the gate size set by EK3_VEL_I_GATE and EK3_POS_I_GATE which can be useful if poor quality sensor data is causing GPS rejection and loss of navigation but does make the EKF more susceptible to GPS glitches. If setting EK3_GLITCH_RAD to 0 it is recommended to reduce EK3_VEL_I_GATE and EK3_POS_I_GATE to 300.
// @Range: 10 100
// @Increment: 5
// @User: Advanced
// @Units: m
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
// The EKF now takes its GPS delay form the GPS library with the default delays
// specified by the GPS_DELAY and GPS_DELAY2 parameters.
// Height measurement parameters
// 9 was ALT_SOURCE
// @Param: ALT_M_NSE
// @DisplayName: Altitude measurement noise (m)
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. A larger value for EK3_ALT_M_NSE may be required when operating with EK3_SRCx_POSZ = 0. This parameter also sets the noise for the 'synthetic' zero height measurement that is used when EK3_SRCx_POSZ = 0.
// @Range: 0.1 100.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("ALT_M_NSE", 10, NavEKF3, _baroAltNoise, ALT_M_NSE_DEFAULT),
// @Param: HGT_I_GATE
// @DisplayName: Height measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD set to 0 the vertical position innovations will be clipped instead of rejected if they exceed the gate size and a smaller value of EK3_HGT_I_GATE not exceeding 300 is recommended to limit the effect of height sensor transient errors.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("HGT_I_GATE", 11, NavEKF3, _hgtInnovGate, HGT_I_GATE_DEFAULT),
// @Param: HGT_DELAY
// @DisplayName: Height measurement delay (msec)
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("HGT_DELAY", 12, NavEKF3, _hgtDelay_ms, 60),
// Magnetometer measurement parameters
// @Param: MAG_M_NSE
// @DisplayName: Magnetometer measurement noise (Gauss)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.01 0.5
// @Increment: 0.01
// @User: Advanced
// @Units: Gauss
AP_GROUPINFO("MAG_M_NSE", 13, NavEKF3, _magNoise, MAG_M_NSE_DEFAULT),
// @Param: MAG_CAL
// @DisplayName: Magnetometer default fusion mode
// @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 5 uses an external yaw sensor with simple heading fusion. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. EK3_MAG_CAL = 6 uses an external yaw sensor with fallback to compass when the external sensor is not available if we are flying. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 and setting COMPASS_ENABLE to 0. If this is done, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK. A yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF) will then be used to align the yaw when flight commences and there is sufficient movement.
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor (Deprecated in 4.1+ see EK3_SRCn_YAW),6:External yaw sensor with compass fallback (Deprecated in 4.1+ see EK3_SRCn_YAW)
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),
// @Param: MAG_I_GATE
// @DisplayName: Magnetometer measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("MAG_I_GATE", 15, NavEKF3, _magInnovGate, MAG_I_GATE_DEFAULT),
// Airspeed measurement parameters
// @Param: EAS_M_NSE
// @DisplayName: Equivalent airspeed measurement noise (m/s)
// @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("EAS_M_NSE", 16, NavEKF3, _easNoise, 1.4f),
// @Param: EAS_I_GATE
// @DisplayName: Airspeed measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("EAS_I_GATE", 17, NavEKF3, _tasInnovGate, 400),
// Rangefinder measurement parameters
// @Param: RNG_M_NSE
// @DisplayName: Range finder measurement noise (m)
// @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("RNG_M_NSE", 18, NavEKF3, _rngNoise, 0.5f),
// @Param: RNG_I_GATE
// @DisplayName: Range finder measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("RNG_I_GATE", 19, NavEKF3, _rngInnovGate, 500),
// Optical flow measurement parameters
// @Param: MAX_FLOW
// @DisplayName: Maximum valid optical flow rate
// @Description: This sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
// @Range: 1.0 4.0
// @Increment: 0.1
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("MAX_FLOW", 20, NavEKF3, _maxFlowRate, 2.5f),
// @Param: FLOW_M_NSE
// @DisplayName: Optical flow measurement noise (rad/s)
// @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 1.0
// @Increment: 0.05
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("FLOW_M_NSE", 21, NavEKF3, _flowNoise, FLOW_M_NSE_DEFAULT),
// @Param: FLOW_I_GATE
// @DisplayName: Optical Flow measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("FLOW_I_GATE", 22, NavEKF3, _flowInnovGate, FLOW_I_GATE_DEFAULT),
// @Param: FLOW_DELAY
// @DisplayName: Optical Flow measurement delay (msec)
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("FLOW_DELAY", 23, NavEKF3, _flowDelay_ms, FLOW_MEAS_DELAY),
// State and Covariance Predition Parameters
// @Param: GYRO_P_NSE
// @DisplayName: Rate gyro noise (rad/s)
// @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
// @Range: 0.0001 0.1
// @Increment: 0.0001
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("GYRO_P_NSE", 24, NavEKF3, _gyrNoise, GYRO_P_NSE_DEFAULT),
// @Param: ACC_P_NSE
// @DisplayName: Accelerometer noise (m/s^2)
// @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
// @Range: 0.01 1.0
// @Increment: 0.01
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ACC_P_NSE", 25, NavEKF3, _accNoise, ACC_P_NSE_DEFAULT),
// @Param: GBIAS_P_NSE
// @DisplayName: Rate gyro bias stability (rad/s/s)
// @Description: This state process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier.
// @Range: 0.00001 0.001
// @User: Advanced
// @Units: rad/s/s
AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
// 27 previously used for EK2_GSCL_P_NSE parameter that has been removed
// @Param: ABIAS_P_NSE
// @DisplayName: Accelerometer bias stability (m/s^3)
// @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
// @Range: 0.00001 0.02
// @User: Advanced
// @Units: m/s/s/s
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
// @Param: WIND_P_NSE
// @DisplayName: Wind velocity process noise (m/s^2)
// @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
// @Range: 0.01 2.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("WIND_P_NSE", 30, NavEKF3, _windVelProcessNoise, WIND_P_NSE_DEFAULT),
// @Param: WIND_PSCALE
// @DisplayName: Height rate to wind process noise scaler
// @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser.
// @Range: 0.0 2.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("WIND_PSCALE", 31, NavEKF3, _wndVarHgtRateScale, 1.0f),
// @Param: GPS_CHECK
// @DisplayName: GPS preflight check
// @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat.
// @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
// @User: Advanced
AP_GROUPINFO("GPS_CHECK", 32, NavEKF3, _gpsCheck, 31),
// @Param: IMU_MASK
// @DisplayName: Bitmask of active IMUs
// @Description: 1 byte bitmap of IMUs to use in EKF3. A separate instance of EKF3 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF3 will fail to start.
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("IMU_MASK", 33, NavEKF3, _imuMask, HAL_EKF_IMU_MASK_DEFAULT),
// @Param: CHECK_SCALE
// @DisplayName: GPS accuracy check scaler (%)
// @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error.
// @Range: 50 200
// @User: Advanced
// @Units: %
AP_GROUPINFO("CHECK_SCALE", 34, NavEKF3, _gpsCheckScaler, CHECK_SCALER_DEFAULT),
// @Param: NOAID_M_NSE
// @DisplayName: Non-GPS operation position uncertainty (m)
// @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
// @Range: 0.5 50.0
// @User: Advanced
// @Units: m
AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF3, _noaidHorizNoise, 10.0f),
// @Param: BETA_MASK
// @DisplayName: Bitmask controlling sidelip angle fusion
// @Description: 1 byte bitmap controlling use of sideslip angle fusion for estimation of non wind states during operation of 'fly forward' vehicle types such as fixed wing planes. By assuming that the angle of sideslip is small, the wind velocity state estimates are corrected whenever the EKF is not dead reckoning (e.g. has an independent velocity or position sensor such as GPS). This behaviour is on by default and cannot be disabled. When the EKF is dead reckoning, the wind states are used as a reference, enabling use of the small angle of sideslip assumption to correct non wind velocity states (eg attitude, velocity, position, etc) and improve navigation accuracy. This behaviour is on by default and cannot be disabled. The behaviour controlled by this parameter is the use of the small angle of sideslip assumption to correct non wind velocity states when the EKF is NOT dead reckoning. This is primarily of benefit to reduce the buildup of yaw angle errors during straight and level flight without a yaw sensor (e.g. magnetometer or dual antenna GPS yaw) provided aerobatic flight maneuvers with large sideslip angles are not performed. The 'always' option might be used where the yaw sensor is intentionally not fitted or disabled. The 'WhenNoYawSensor' option might be used if a yaw sensor is fitted, but protection against in-flight failure and continual rejection by the EKF is desired. For vehicles operated within visual range of the operator performing frequent turning maneuvers, setting this parameter is unnecessary.
// @Bitmask: 0:Always,1:WhenNoYawSensor
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("BETA_MASK", 36, NavEKF3, _betaMask, 0),
// control of magnetic yaw angle fusion
// @Param: YAW_M_NSE
// @DisplayName: Yaw measurement noise (rad)
// @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 1.0
// @Increment: 0.05
// @User: Advanced
// @Units: rad
AP_GROUPINFO("YAW_M_NSE", 37, NavEKF3, _yawNoise, 0.5f),
// @Param: YAW_I_GATE
// @DisplayName: Yaw measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("YAW_I_GATE", 38, NavEKF3, _yawInnovGate, 300),
// @Param: TAU_OUTPUT
// @DisplayName: Output complementary filter time constant (centi-sec)
// @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
// @Range: 10 50
// @Increment: 5
// @User: Advanced
// @Units: cs
AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF3, _tauVelPosOutput, 25),
// @Param: MAGE_P_NSE
// @DisplayName: Earth magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: Gauss/s
AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF3, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT),
// @Param: MAGB_P_NSE
// @DisplayName: Body magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: Gauss/s
AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF3, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT),
// @Param: RNG_USE_HGT
// @DisplayName: Range finder switch height percentage
// @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFNDx_MAX_CM) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point.
// @Range: -1 70
// @Increment: 1
// @User: Advanced
// @Units: %
AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF3, _useRngSwHgt, -1),
// @Param: TERR_GRAD
// @DisplayName: Maximum terrain gradient
// @Description: Specifies the maximum gradient of the terrain below the vehicle when it is using range finder as a height reference
// @Range: 0 0.2
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("TERR_GRAD", 43, NavEKF3, _terrGradMax, 0.1f),
// @Param: BCN_M_NSE
// @DisplayName: Range beacon measurement noise (m)
// @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("BCN_M_NSE", 44, NavEKF3, _rngBcnNoise, 1.0f),
// @Param: BCN_I_GTE
// @DisplayName: Range beacon measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the range beacon measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("BCN_I_GTE", 45, NavEKF3, _rngBcnInnovGate, 500),
// @Param: BCN_DELAY
// @DisplayName: Range beacon measurement delay (msec)
// @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("BCN_DELAY", 46, NavEKF3, _rngBcnDelay_ms, 50),
// @Param: RNG_USE_SPD
// @DisplayName: Range finder max ground speed
// @Description: The range finder will not be used as the primary height source when the horizontal ground speed is greater than this value.
// @Range: 2.0 6.0
// @Increment: 0.5
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF3, _useRngSwSpd, 2.0f),
// @Param: ACC_BIAS_LIM
// @DisplayName: Accelerometer bias limit
// @Description: The accelerometer bias state will be limited to +- this value
// @Range: 0.5 2.5
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ACC_BIAS_LIM", 48, NavEKF3, _accBiasLim, 1.0f),
// @Param: MAG_MASK
// @DisplayName: Bitmask of active EKF cores that will always use heading fusion
// @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MAG_MASK", 49, NavEKF3, _magMask, 0),
// @Param: OGN_HGT_MASK
// @DisplayName: Bitmask control of EKF reference height correction
// @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position or to the reported EKF origin height (default).
// @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("OGN_HGT_MASK", 50, NavEKF3, _originHgtMode, 0),
// @Param: VIS_VERR_MIN
// @DisplayName: Visual odometry minimum velocity error
// @Description: This is the 1-STD odometry velocity observation error that will be assumed when maximum quality is reported by the sensor. When quality is between max and min, the error will be calculated using linear interpolation between VIS_VERR_MIN and VIS_VERR_MAX.
// @Range: 0.05 0.5
// @Increment: 0.05
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VIS_VERR_MIN", 51, NavEKF3, _visOdmVelErrMin, 0.1f),
// @Param: VIS_VERR_MAX
// @DisplayName: Visual odometry maximum velocity error
// @Description: This is the 1-STD odometry velocity observation error that will be assumed when minimum quality is reported by the sensor. When quality is between max and min, the error will be calculated using linear interpolation between VIS_VERR_MIN and VIS_VERR_MAX.
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VIS_VERR_MAX", 52, NavEKF3, _visOdmVelErrMax, 0.9f),
// @Param: WENC_VERR
// @DisplayName: Wheel odometry velocity error
// @Description: This is the 1-STD odometry velocity observation error that will be assumed when wheel encoder data is being fused.
// @Range: 0.01 1.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
// @Param: FLOW_USE
// @DisplayName: Optical flow use bitmask
// @Description: Controls if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator.
// @User: Advanced
// @Values: 0:None,1:Navigation,2:Terrain
// @RebootRequired: True
AP_GROUPINFO("FLOW_USE", 54, NavEKF3, _flowUse, FLOW_USE_DEFAULT),
// @Param: HRT_FILT
// @DisplayName: Height rate filter crossover frequency
// @Description: Specifies the crossover frequency of the complementary filter used to calculate the output predictor height rate derivative.
// @Range: 0.1 30.0
// @Units: Hz
AP_GROUPINFO("HRT_FILT", 55, NavEKF3, _hrt_filt_freq, 2.0f),
// @Param: MAG_EF_LIM
// @DisplayName: EarthField error limit
// @Description: This limits the difference between the learned earth magnetic field and the earth field from the world magnetic model tables. A value of zero means to disable the use of the WMM tables.
// @User: Advanced
// @Range: 0 500
// @Units: mGauss
AP_GROUPINFO("MAG_EF_LIM", 56, NavEKF3, _mag_ef_limit, 50),
// @Param: GSF_RUN_MASK
// @DisplayName: Bitmask of which EKF-GSF yaw estimators run
// @Description: 1 byte bitmap of which EKF3 instances run an independant EKF-GSF yaw estimator to provide a backup yaw estimate that doesn't rely on magnetometer data. This estimator uses IMU, GPS and, if available, airspeed data. EKF-GSF yaw estimator data for the primary EKF3 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK3_GSF_USE_MASK and EK3_GSF_RST_MAX parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK3_GSF_USE to 0.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GSF_RUN_MASK", 57, NavEKF3, _gsfRunMask, 3),
// @Param: GSF_USE_MASK
// @DisplayName: Bitmask of which EKF-GSF yaw estimators are used
// @Description: A bitmask of which EKF3 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK3_GSF_RUN_MASK parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF3 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF3 instance.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GSF_USE_MASK", 58, NavEKF3, _gsfUseMask, 3),
// 59 was GSF_DELAY which was never released in a stable version
// @Param: GSF_RST_MAX
// @DisplayName: Maximum number of resets to the EKF-GSF yaw estimate allowed
// @Description: Sets the maximum number of times the EKF3 will be allowed to reset its yaw to the estimate from the EKF-GSF yaw estimator. No resets will be allowed unless the use of the EKF-GSF yaw estimate is enabled via the EK3_GSF_USE_MASK parameter.
// @Range: 1 10
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GSF_RST_MAX", 60, NavEKF3, _gsfResetMaxCount, 2),
// @Param: ERR_THRESH
// @DisplayName: EKF3 Lane Relative Error Sensitivity Threshold
// @Description: lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError, lowering this makes lane switching more sensitive to smaller error differences
// @Range: 0.05 1
// @Increment: 0.05
// @User: Advanced
AP_GROUPINFO("ERR_THRESH", 61, NavEKF3, _err_thresh, 0.2),
// @Param: AFFINITY
// @DisplayName: EKF3 Sensor Affinity Options
// @Description: These options control the affinity between sensor instances and EKF cores
// @User: Advanced
// @Bitmask: 0:EnableGPSAffinity,1:EnableBaroAffinity,2:EnableCompassAffinity,3:EnableAirspeedAffinity
// @RebootRequired: True
AP_GROUPINFO("AFFINITY", 62, NavEKF3, _affinity, 0),
AP_SUBGROUPEXTENSION("", 63, NavEKF3, var_info2),
AP_GROUPEND
};
// second table of parameters. allows us to go beyond the 64 parameter limit
const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @Group: SRC
// @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp
AP_SUBGROUPINFO(sources, "SRC", 1, NavEKF3, AP_NavEKF_Source),
// @Param: DRAG_BCOEF_X
// @DisplayName: Ballistic coefficient for X axis drag
// @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_DRAG_MCOEF parameter.
// @Range: 0.0 1000.0
// @Units: kg/m/m
// @User: Advanced
AP_GROUPINFO("DRAG_BCOEF_X", 2, NavEKF3, _ballisticCoef_x, 0.0f),
// @Param: DRAG_BCOEF_Y
// @DisplayName: Ballistic coefficient for Y axis drag
// @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_DRAG_MCOEF parameter.
// @Range: 50.0 1000.0
// @Units: kg/m/m
// @User: Advanced
AP_GROUPINFO("DRAG_BCOEF_Y", 3, NavEKF3, _ballisticCoef_y, 0.0f),
// @Param: DRAG_M_NSE
// @DisplayName: Observation noise for drag acceleration
// @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters
// @Range: 0.1 2.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("DRAG_M_NSE", 4, NavEKF3, _dragObsNoise, 0.5f),
// @Param: DRAG_MCOEF
// @DisplayName: Momentum coefficient for propeller drag
// @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with different propellers. It is higher for ducted rotors. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_DRAG_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEF to a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters.
// @Range: 0.0 1.0
// @Increment: 0.01
// @Units: 1/s
// @User: Advanced
AP_GROUPINFO("DRAG_MCOEF", 5, NavEKF3, _momentumDragCoef, 0.0f),
// @Param: OGNM_TEST_SF
// @DisplayName: On ground not moving test scale factor
// @Description: This parameter is adjust the sensitivity of the on ground not moving test which is used to assist with learning the yaw gyro bias and stopping yaw drift before flight when operating without a yaw sensor. Bigger values allow the detection of a not moving condition with noiser IMU data. Check the XKFM data logged when the vehicle is on ground not moving and adjust the value of OGNM_TEST_SF to be slightly higher than the maximum value of the XKFM.ADR, XKFM.ALR, XKFM.GDR and XKFM.GLR test levels.
// @Range: 1.0 10.0
// @Increment: 0.5
// @User: Advanced
AP_GROUPINFO("OGNM_TEST_SF", 6, NavEKF3, _ognmTestScaleFactor, 2.0f),
// @Param: GND_EFF_DZ
// @DisplayName: Baro height ground effect dead zone
// @Description: This parameter sets the size of the dead zone that is applied to negative baro height spikes that can occur when taking off or landing when a vehicle with lift rotors is operating in ground effect ground effect. Set to about 0.5m less than the amount of negative offset in baro height that occurs just prior to takeoff when lift motors are spooling up. Set to 0 if no ground effect is present.
// @Range: 0.0 10.0
// @Increment: 0.5
// @User: Advanced
AP_GROUPINFO("GND_EFF_DZ", 7, NavEKF3, _baroGndEffectDeadZone, 4.0f),
// @Param: PRIMARY
// @DisplayName: Primary core number
// @Description: The core number (index in IMU mask) that will be used as the primary EKF core on startup. While disarmed the EKF will force the use of this core. A value of 0 corresponds to the first IMU in EK3_IMU_MASK.
// @Range: 0 2
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("PRIMARY", 8, NavEKF3, _primary_core, EK3_PRIMARY_DEFAULT),
// @Param: LOG_LEVEL
// @DisplayName: Logging Level
// @Description: Determines how verbose the EKF3 streaming logging is. A value of 0 provides full logging(default), a value of 1 only XKF4 scaled innovations are logged, a value of 2 both XKF4 and GSF are logged, and a value of 3 disables all streaming logging of EKF3.
// @Range: 0 3
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOG_LEVEL", 9, NavEKF3, _log_level, 0),
// @Param: GPS_VACC_MAX
// @DisplayName: GPS vertical accuracy threshold
// @Description: Vertical accuracy threshold for GPS as the altitude source. The GPS will not be used as an altitude source if the reported vertical accuracy of the GPS is larger than this threshold, falling back to baro instead. Set to zero to deactivate the threshold check.
// @Range: 0.0 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f),
AP_GROUPEND
};
NavEKF3::NavEKF3()
{
AP_Param::setup_object_defaults(this, var_info);
AP_Param::setup_object_defaults(this, var_info2);
}
// Initialise the filter
bool NavEKF3::InitialiseFilter(void)
{
if (_enable == 0 || _imuMask == 0) {
return false;
}
const auto &ins = AP::dal().ins();
AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF3);
imuSampleTime_us = AP::dal().micros64();
// remember expected frame time
const float loop_rate = ins.get_loop_rate_hz();
if (!is_positive(loop_rate)) {
return false;
}
_frameTimeUsec = 1e6 / loop_rate;
// expected number of IMU frames per prediction
_framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
// convert parameters if necessary
convert_parameters();
#endif
#if APM_BUILD_TYPE(APM_BUILD_Replay)
if (ins.get_accel_count() == 0) {
return false;
}
#endif
if (core == nullptr) {
// don't run multiple filters for 1 IMU
uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set_and_default(_imuMask.get() & mask);
// initialise the setup variables
for (uint8_t i=0; i<MAX_EKF_CORES; i++) {
coreSetupRequired[i] = false;
coreImuIndex[i] = 0;
}
num_cores = 0;
// count IMUs from mask
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_imuMask & (1U<<i)) {
coreSetupRequired[num_cores] = true;
coreImuIndex[num_cores] = i;
num_cores++;
}
}
// check if there is enough memory to create the EKF cores
if (AP::dal().available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 not enough memory");
_enable.set(0);
num_cores = 0;
return false;
}
//try to allocate from CCM RAM, fallback to Normal RAM if not available or full
core = (NavEKF3_core*)AP::dal().malloc_type(sizeof(NavEKF3_core)*num_cores, AP::dal().MEM_FAST);
if (core == nullptr) {
_enable.set(0);
num_cores = 0;
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 allocation failed");
return false;
}
// Call constructors on all cores
for (uint8_t i = 0; i < num_cores; i++) {
new (&core[i]) NavEKF3_core(this);
}
}
// Set up any cores that have been created
// This specifies the IMU to be used and creates the data buffers
// If we are unable to set up a core, return false and try again next time the function is called
bool core_setup_success = true;
for (uint8_t core_index=0; core_index<num_cores; core_index++) {
if (coreSetupRequired[core_index]) {
coreSetupRequired[core_index] = !core[core_index].setup_core(coreImuIndex[core_index], core_index);
if (coreSetupRequired[core_index]) {
core_setup_success = false;
}
}
}
// exit with failure if any cores could not be setup
if (!core_setup_success) {
return false;
}
// set relative error scores for all cores to 0
resetCoreErrors();
// Set the primary initially to be users selected primary
primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
// invalidate shared origin
common_origin_valid = false;
// initialise the cores. We return success only if all cores
// initialise successfully
bool ret = true;
for (uint8_t i=0; i<num_cores; i++) {
ret &= core[i].InitialiseFilterBootstrap();
}
// set last time the cores were primary to 0
memset(coreLastTimePrimary_us, 0, sizeof(coreLastTimePrimary_us));
// zero the structs used capture reset events
memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
return ret;
}
/*
return true if a new core index has a better score than the current
core
*/
bool NavEKF3::coreBetterScore(uint8_t new_core, uint8_t current_core) const
{
const NavEKF3_core &oldCore = core[current_core];
const NavEKF3_core &newCore = core[new_core];
if (!newCore.healthy()) {
// never consider a new core that isn't healthy
return false;
}
if (newCore.have_aligned_tilt() != oldCore.have_aligned_tilt()) {
// tilt alignment is most critical, if one is tilt aligned and
// the other isn't then use the tilt aligned lane
return newCore.have_aligned_tilt();
}
if (newCore.have_aligned_yaw() != oldCore.have_aligned_yaw()) {
// yaw alignment is next most critical, if one is yaw aligned
// and the other isn't then use the yaw aligned lane
return newCore.have_aligned_yaw();
}
// if both cores are aligned then look at relative error scores
return coreRelativeErrors[new_core] < coreRelativeErrors[current_core];
}
/*
Update Filter States - this should be called whenever new IMU data is available
Execution speed governed by SCHED_LOOP_RATE
*/
void NavEKF3::UpdateFilter(void)
{
AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF3);
if (!core) {
return;
}
imuSampleTime_us = AP::dal().micros64();
for (uint8_t i=0; i<num_cores; i++) {
// if we have not overrun by more than 3 IMU frames, and we
// have already used more than 1/3 of the CPU budget for this
// loop then suppress the prediction step. This allows
// multiple EKF instances to cooperate on scheduling
bool allow_state_prediction = true;
if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
AP::dal().ekf_low_time_remaining(AP_DAL::EKFType::EKF3, i)) {
allow_state_prediction = false;
}
core[i].UpdateFilter(allow_state_prediction);
}
// If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
// Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
// due to initial alignment fluctuations and race conditions
if (!runCoreSelection) {
static uint64_t lastUnhealthyTime_us = 0;
if (!core[primary].healthy() || lastUnhealthyTime_us == 0) {
lastUnhealthyTime_us = imuSampleTime_us;
}
runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
}
const bool armed = AP::dal().get_armed();
// core selection is only available after the vehicle is armed, else forced to lane 0 if its healthy
if (runCoreSelection && armed) {
// update this instance's error scores for all active cores and get the primary core's error score
float primaryErrorScore = updateCoreErrorScores();
// update the accumulated relative error scores for all active cores
updateCoreRelativeErrors();
bool betterCore = false;
bool altCoreAvailable = false;
uint8_t newPrimaryIndex = primary;
// loop through all available cores to find if an alternative core is available
for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
if (coreIndex != primary) {
float altCoreError = coreRelativeErrors[coreIndex];
// an alternative core is available for selection based on 2 conditions -
// 1. healthy and states have been updated on this time step
// 2. has relative error less than primary core error
// 3. not been the primary core for at least 10 seconds
altCoreAvailable = coreBetterScore(coreIndex, newPrimaryIndex) &&
imuSampleTime_us - coreLastTimePrimary_us[coreIndex] > 1E7;
if (altCoreAvailable) {
// if this core has a significantly lower relative error to the active primary, we consider it as a
// better core and would like to switch to it even if the current primary is healthy
betterCore = altCoreError <= -BETTER_THRESH; // a better core if its relative error is below a substantial level than the primary's
// handle the case where the secondary core is faster to complete yaw alignment which can happen
// in flight when oeprating without a magnetomer
const NavEKF3_core &newCore = core[coreIndex];
const NavEKF3_core &oldCore = core[primary];
betterCore |= newCore.have_aligned_yaw() && !oldCore.have_aligned_yaw();
newPrimaryIndex = coreIndex;
}
}
}
altCoreAvailable = newPrimaryIndex != primary;
// Switch cores if another core is available and the active primary core meets one of the following conditions -
// 1. has a bad error score
// 2. is unhealthy
// 3. is healthy, but a better core is available
// also update the yaw and position reset data to capture changes due to the lane switch
if (altCoreAvailable && (primaryErrorScore > 1.0f || !core[primary].healthy() || betterCore)) {
updateLaneSwitchYawResetData(newPrimaryIndex, primary);
updateLaneSwitchPosResetData(newPrimaryIndex, primary);
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
resetCoreErrors();
coreLastTimePrimary_us[primary] = imuSampleTime_us;
primary = newPrimaryIndex;
lastLaneSwitch_ms = AP::dal().millis();
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 lane switch %u", primary);
}
}
const uint8_t user_primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
if (primary != user_primary && core[user_primary].healthy() && !armed) {
// when on the ground and disarmed force the selected primary
// core. This avoids us ending with with a lottery for which
// IMU is used in each flight. Otherwise the alignment of the
// timing of the core selection updates with the timing of GPS
// updates can lead to a core other than the first one being
// used as primary for some flights. As different IMUs may
// have quite different noise characteristics this leads to
// inconsistent performance