forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 2
/
AP_NavEKF3_Measurements.cpp
1235 lines (1039 loc) · 52.8 KB
/
AP_NavEKF3_Measurements.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 <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Logger/AP_Logger.h>
extern const AP_HAL::HAL& hal;
/********************************************************
* OPT FLOW AND RANGE FINDER *
********************************************************/
// Read the range finder and take new measurements if available
// Apply a median filter
void NavEKF3_core::readRangeFinder(void)
{
uint8_t midIndex;
uint8_t maxIndex;
uint8_t minIndex;
// get theoretical correct range when the vehicle is on the ground
// don't allow range to go below 5cm because this can cause problems with optical flow processing
const RangeFinder *_rng = AP::rangefinder();
if (_rng == nullptr) {
return;
}
rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);
// limit update rate to maximum allowed by data buffers
if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {
// reset the timer used to control the measurement rate
lastRngMeasTime_ms = imuSampleTime_ms;
// store samples and sample time into a ring buffer if valid
// use data from two range finders if available
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
AP_RangeFinder_Backend *sensor = _rng->get_backend(sensorIndex);
if (sensor == nullptr) {
continue;
}
if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == RangeFinder::Status::Good)) {
rngMeasIndex[sensorIndex] ++;
if (rngMeasIndex[sensorIndex] > 2) {
rngMeasIndex[sensorIndex] = 0;
}
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
} else {
continue;
}
// check for three fresh samples
bool sampleFresh[2][3] = {};
for (uint8_t index = 0; index <= 2; index++) {
sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
}
// find the median value if we have three fresh samples
if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
minIndex = 1;
maxIndex = 0;
} else {
minIndex = 0;
maxIndex = 1;
}
if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
midIndex = maxIndex;
} else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
midIndex = minIndex;
} else {
midIndex = 2;
}
// don't allow time to go backwards
if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
}
// limit the measured range to be no less than the on-ground range
rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
// get position in body frame for the current sensor
rangeDataNew.sensor_idx = sensorIndex;
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew);
// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms;
} else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
// before takeoff we assume on-ground range value if there is no data
rangeDataNew.time_ms = imuSampleTime_ms;
rangeDataNew.rng = rngOnGnd;
rangeDataNew.time_ms = imuSampleTime_ms;
// don't allow time to go backwards
if (imuSampleTime_ms > rangeDataNew.time_ms) {
rangeDataNew.time_ms = imuSampleTime_ms;
}
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew);
// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms;
}
}
}
}
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
// protect against NaN
if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) {
return;
}
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
return;
}
// subtract delay from timestamp
timeStamp_ms -= delay_ms;
bodyOdmDataNew.body_offset = &posOffset;
bodyOdmDataNew.vel = delPos * (1.0f/delTime);
bodyOdmDataNew.time_ms = timeStamp_ms;
bodyOdmDataNew.angRate = delAng * (1.0f/delTime);
bodyOdmMeasTime_ms = timeStamp_ms;
// simple model of accuracy
// TODO move this calculation outside of EKF into the sensor driver
bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);
storedBodyOdm.push(bodyOdmDataNew);
}
void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
// This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
// It uses the exisiting body frame velocity fusion.
// TODO implement a dedicated wheel odometry observation model
// rate limiting to 50hz should be done by the caller
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if ((delTime < dtEkfAvg) || !statesInitialised) {
return;
}
wheelOdmDataNew.hub_offset = &posOffset;
wheelOdmDataNew.delAng = delAng;
wheelOdmDataNew.radius = radius;
wheelOdmDataNew.delTime = delTime;
wheelOdmMeasTime_ms = timeStamp_ms;
// because we are currently converting to an equivalent velocity measurement before fusing
// the measurement time is moved back to the middle of the sampling period
wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
storedWheelOdm.push(wheelOdmDataNew);
}
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset)
{
// limit update rate to maximum allowed by sensor buffers
if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
return;
}
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis
// A positive Y rate is produced by a positive sensor rotation about the Y axis
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowMeaTime_ms = imuSampleTime_ms;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// by definition if this function is called, then flow measurements have been provided so we
// need to run the optical flow takeoff detection
detectOptFlowTakeoff();
// calculate rotation matrices at mid sample time for flow observations
stateStruct.quat.rotation_matrix(Tbn_flow);
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// correct flow sensor body rates for bias and write
ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
// the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
if (delTimeOF > 0.001f) {
// first preference is to use the rate averaged over the same sampling period as the flow sensor
ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
} else if (imuDataNew.delAngDT > 0.001f){
// second preference is to use most recent IMU data
ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
} else {
// third preference is use zero
ofDataNew.bodyRadXYZ.z = 0.0f;
}
// write uncorrected flow rate measurements
// note correction for different axis and sign conventions used by the px4flow sensor
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write the flow sensor position in body frame
ofDataNew.body_offset = &posOffset;
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
// record time last observation was received so we can detect loss of data elsewhere
flowValidMeaTime_ms = imuSampleTime_ms;
// estimate sample time of the measurement
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
// Correct for the average intersampling delay due to the filter updaterate
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
// Save data to buffer
storedOF.push(ofDataNew);
}
}
/********************************************************
* MAGNETOMETER *
********************************************************/
// check for new magnetometer data and update store measurements if available
void NavEKF3_core::readMagData()
{
if (!_ahrs->get_compass()) {
allMagSensorsFailed = true;
return;
}
const Compass &compass = AP::compass();
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = compass.get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
if (compass.learn_offsets_enabled()) {
// while learning offsets keep all mag states reset
InitialiseVariablesMag();
wasLearningCompass_ms = imuSampleTime_ms;
} else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
wasLearningCompass_ms = 0;
// force a new yaw alignment 1s after learning completes. The
// delay is to ensure any buffered mag samples are discarded
yawAlignComplete = false;
InitialiseVariablesMag();
}
// limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && ((compass.last_update_usec() - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
frontend->logging.log_compass = true;
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
break;
}
}
}
// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = compass.get_offsets(magSelectIndex);
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) {
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
}
lastMagOffsets = nowMagOffsets;
lastMagOffsetsValid = true;
// store time of last measurement update
lastMagUpdate_us = compass.last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = compass.get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = compass.consistent();
// save magnetometer measurement to buffer to be fused later
storedMag.push(magDataNew);
}
}
/********************************************************
* Inertial Measurements *
********************************************************/
/*
* Read IMU delta angle and delta velocity measurements and downsample to 100Hz
* for storage in the data buffers used by the EKF. If the IMU data arrives at
* lower rate than 100Hz, then no downsampling or upsampling will be performed.
* Downsampling is done using a method that does not introduce coning or sculling
* errors.
*/
void NavEKF3_core::readIMUData()
{
const AP_InertialSensor &ins = AP::ins();
// calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
uint8_t accel_active, gyro_active;
if (ins.use_accel(imu_index)) {
accel_active = imu_index;
} else {
accel_active = ins.get_primary_accel();
}
if (ins.use_gyro(imu_index)) {
gyro_active = imu_index;
} else {
gyro_active = ins.get_primary_gyro();
}
if (gyro_active != gyro_index_active) {
// we are switching active gyro at runtime. Copy over the
// bias we have learned from the previously inactive
// gyro. We don't re-init the bias uncertainty as it should
// have the same uncertainty as the previously active gyro
stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
gyro_index_active = gyro_active;
}
if (accel_active != accel_index_active) {
// switch to the learned accel bias for this IMU
stateStruct.accel_bias = inactiveBias[accel_active].accel_bias;
accel_index_active = accel_active;
}
// update the inactive bias states
learnInactiveBiases();
// run movement check using IMU data
updateMovementCheck();
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
accelPosOffset = ins.get_imu_pos_offset(accel_index_active);
imuDataNew.accel_index = accel_index_active;
// Get delta angle data from primary gyro or primary if not available
readDeltaAngle(gyro_index_active, imuDataNew.delAng);
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(gyro_index_active),1.0e-4f);
imuDataNew.gyro_index = gyro_index_active;
// Get current time stamp
imuDataNew.time_ms = imuSampleTime_ms;
// Accumulate the measurement time interval for the delta velocity and angle data
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
// use the most recent IMU index for the downsampled IMU
// data. This isn't strictly correct if we switch IMUs between
// samples
imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
imuDataDownSampledNew.accel_index = imuDataNew.accel_index;
// Rotate quaternon atitude from previous to new and normalise.
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
imuQuatDownSampleNew.normalize();
// Rotate the latest delta velocity into body frame at the start of accumulation
Matrix3f deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// Apply the delta velocity to the delta velocity accumulator
imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
// Keep track of the number of IMU frames since the last state prediction
framesSincePredict++;
/*
* If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
* then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
* than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
* IMU data.
*/
if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5f)) && startPredictEnabled) ||
(imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
// Time stamp the data
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
// Write data to the FIFO IMU buffer
storedIMU.push_youngest_element(imuDataDownSampledNew);
// calculate the achieved average time step rate for the EKF using a combination spike and LPF
float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// zero the accumulated IMU data and quaternion
imuDataDownSampledNew.delAng.zero();
imuDataDownSampledNew.delVel.zero();
imuDataDownSampledNew.delAngDT = 0.0f;
imuDataDownSampledNew.delVelDT = 0.0f;
imuQuatDownSampleNew[0] = 1.0f;
imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
framesSincePredict = 0;
// set the flag to let the filter know it has new IMU data and needs to run
runUpdates = true;
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element();
// protect against delta time going to zero
float minDT = 0.1f * dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
updateTimingStatistics();
// correct the extracted IMU data for sensor errors
delAngCorrected = imuDataDelayed.delAng;
delVelCorrected = imuDataDelayed.delVel;
correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);
} else {
// we don't have new IMU data in the buffer so don't run filter updates on this time step
runUpdates = false;
}
}
// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
const AP_InertialSensor &ins = AP::ins();
if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel);
dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f);
return true;
}
return false;
}
/********************************************************
* Global Position Measurement *
********************************************************/
// check for new valid GPS data and update stored measurement if available
void NavEKF3_core::readGpsData()
{
// check for new GPS data
// limit update rate to avoid overflowing the FIFO buffer
const AP_GPS &gps = AP::gps();
if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = false;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = gps.last_message_time_ms();
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
// Use the driver specified delay
float gps_delay_sec = 0;
gps.get_lag(gps_delay_sec);
gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
// Correct for the average intersampling delay due to the filter updaterate
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent the time stamp falling outside the oldest and newest IMU data in the buffer
gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);
// Get which GPS we are using for position information
gpsDataNew.sensor_idx = gps.primary_sensor();
// read the NED velocity from the GPS
gpsDataNew.vel = gps.velocity();
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!gps.speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
}
gpsPosAccuracy *= (1.0f - alpha);
float gpsPosAccRaw;
if (!gps.horizontal_accuracy(gpsPosAccRaw)) {
gpsPosAccuracy = 0.0f;
} else {
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
}
gpsHgtAccuracy *= (1.0f - alpha);
float gpsHgtAccRaw;
if (!gps.vertical_accuracy(gpsHgtAccRaw)) {
gpsHgtAccuracy = 0.0f;
} else {
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
}
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (gps.num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.0f;
} else if (gps.num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.4f;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f;
}
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
if (gps.have_vertical_velocity() && (frontend->_fusionModeGPS == 0) && !frontend->inhibitGpsVertVelUse) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data before and after alignment
calcGpsGoodToAlign();
// Post-alignment checks
calcGpsGoodForFlight();
// see if we can get an origin from the frontend
if (!validOrigin && frontend->common_origin_valid) {
setOrigin(frontend->common_EKF_origin);
}
// Read the GPS location in WGS-84 lat,long,height coordinates
const struct Location &gpsloc = gps.location();
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) {
setOrigin(gpsloc);
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination();
// Set the height of the NED origin
ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;
// Set the uncertainty of the GPS origin height
ekfOriginHgtVar = sq(gpsHgtAccuracy);
}
if (gpsGoodToAlign && !have_table_earth_field) {
const Compass *compass = _ahrs->get_compass();
if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc);
table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
gpsloc.lng*1.0e-7));
have_table_earth_field = true;
if (frontend->_mag_ef_limit > 0) {
// initialise earth field from tables
stateStruct.earth_magfield = table_earth_field_ga;
}
}
}
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) {
gpsDataNew.pos = EKF_origin.get_distance_NE(gpsloc);
if ((frontend->_originHgtMode & (1<<2)) == 0) {
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else {
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
}
storedGPS.push(gpsDataNew);
// declare GPS available for use
gpsNotAvailable = false;
}
// if the GPS has yaw data then input that as well
float yaw_deg, yaw_accuracy_deg;
if (AP::gps().gps_yaw_deg(yaw_deg, yaw_accuracy_deg)) {
// GPS modules are rather too optimistic about their
// accuracy. Set to min of 5 degrees here to prevent
// the user constantly receiving warnings about high
// normalised yaw innovations
const float min_yaw_accuracy_deg = 5.0f;
yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2);
}
} else {
// report GPS fix status
gpsCheckStatus.bad_fix = true;
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
}
}
}
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
const AP_InertialSensor &ins = AP::ins();
if (ins_index < ins.get_gyro_count()) {
ins.get_delta_angle(ins_index,dAng);
frontend->logging.log_imu = true;
return true;
}
return false;
}
/********************************************************
* Height Measurements *
********************************************************/
// check for new pressure altitude measurement data and update stored measurement if available
void NavEKF3_core::readBaroData()
{
// check to see if baro measurement has changed so we know if a new measurement has arrived
// limit update rate to avoid overflowing the FIFO buffer
const AP_Baro &baro = AP::baro();
if (baro.get_last_update() - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
frontend->logging.log_baro = true;
baroDataNew.hgt = baro.get_altitude();
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent
if (expectGndEffectTakeoff) {
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
}
// time stamp used to check for new measurement
lastBaroReceived_ms = baro.get_last_update();
// estimate of time height measurement was taken, allowing for delays
baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
baroDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);
// save baro measurement to buffer to be fused later
storedBaro.push(baroDataNew);
}
}
// calculate filtered offset between baro height measurement and EKF height estimate
// offset should be subtracted from baro measurement to match filter estimate
// offset is used to enable reversion to baro from alternate height data source
void NavEKF3_core::calcFiltBaroOffset()
{
// Apply a first order LPF with spike protection
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
}
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void NavEKF3_core::correctEkfOriginHeight()
{
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
// calculate the variance of our a-priori estimate of the ekf origin height
float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
if (activeHgtSource == HGT_SOURCE_BARO) {
// Use the baro drift rate
const float baroDriftRate = 0.05f;
ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
} else if (activeHgtSource == HGT_SOURCE_RNG) {
// use the worse case expected terrain gradient and vehicle horizontal speed
const float maxTerrGrad = 0.25f;
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
} else {
// by definition our height source is absolute so cannot run this filter
return;
}
lastOriginHgtTime_ms = imuDataDelayed.time_ms;
// calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
// when not using GPS as height source
float originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];
// calculate the correction gain
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
// calculate the innovation
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
// check the innovation variance ratio
float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
// correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if (ratio < 25.0f && gpsAccuracyGood) {
ekfGpsRefHgt -= (double)(gain * innovation);
ekfOriginHgtVar -= MAX(gain * ekfOriginHgtVar , 0.0f);
}
}
/********************************************************
* Air Speed Measurements *
********************************************************/
// check for new airspeed data and update stored measurements if available
void NavEKF3_core::readAirSpdData()
{
// if airspeed reading is valid and is set by the user to be used and has been updated then
// we take a new reading, convert from EAS to TAS and set the flag letting other functions
// know a new measurement is available
const AP_Airspeed *aspeed = _ahrs->get_airspeed();
if (aspeed &&
aspeed->use() &&
(aspeed->last_update_ms() - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.tas = aspeed->get_raw_airspeed() * AP::ahrs().get_EAS2TAS();
timeTasReceived_ms = aspeed->last_update_ms();
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
// Correct for the average intersampling delay due to the filter update rate
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
// Save data into the buffer to be fused when the fusion time horizon catches up with it
storedTAS.push(tasDataNew);
}
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
}
/********************************************************
* Range Beacon Measurements *
********************************************************/
// check for new range beacon data and push to data buffer if available
void NavEKF3_core::readRngBcnData()
{
// get the location of the beacon data
const AP_Beacon *beacon = AP::beacon();
// exit immediately if no beacon object
if (beacon == nullptr) {
return;
}
// get the number of beacons in use
N_beacons = beacon->count();
// search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
bool newDataToPush = false;
uint8_t numRngBcnsChecked = 0;
// start the search one index up from where we left it last time
uint8_t index = lastRngBcnChecked;
while (!newDataToPush && numRngBcnsChecked < N_beacons) {
// track the number of beacons checked
numRngBcnsChecked++;
// move to next beacon, wrap index if necessary
index++;
if (index >= N_beacons) {
index = 0;
}
// check that the beacon is healthy and has new data
if (beacon->beacon_healthy(index) &&
beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index])
{
// set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index);
rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
// set the range noise
// TODO the range library should provide the noise/accuracy estimate for each beacon
rngBcnDataNew.rngErr = frontend->_rngBcnNoise;
// set the range measurement
rngBcnDataNew.rng = beacon->beacon_distance(index);
// set the beacon position
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index);
// identify the beacon identifier
rngBcnDataNew.beacon_ID = index;
// indicate we have new data to push to the buffer
newDataToPush = true;
// update the last checked index
lastRngBcnChecked = index;
}
}
// Check if the beacon system has returned a 3D fix
if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) {
rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
}
// Check if the range beacon data can be used to align the vehicle position
if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) {
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
const float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) {
rngBcnGoodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set
if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) {
// get origin from beacon system
Location origin_loc;
if (beacon->get_origin(origin_loc)) {
setOriginLLH(origin_loc);
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination();
// Set the uncertainty of the origin height
ekfOriginHgtVar = sq(beaconVehiclePosErr);
}
}
} else {
rngBcnGoodToAlign = false;
}
} else {
rngBcnGoodToAlign = false;
}
// Save data into the buffer to be fused when the fusion time horizon catches up with it
if (newDataToPush) {
storedRangeBeacon.push(rngBcnDataNew);
}
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed, imuDataDelayed.time_ms);
// Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
if (rngBcnDataToFuse) {
rngBcnDataDelayed.beacon_posNED.x += bcnPosOffsetNED.x;
rngBcnDataDelayed.beacon_posNED.y += bcnPosOffsetNED.y;
}
}
/********************************************************
* Independant yaw sensor measurements *
********************************************************/
void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if (((timeStamp_ms - yawMeasTime_ms) < frontend->sensorIntervalMin_ms) || !statesInitialised) {
return;
}
yawAngDataNew.yawAng = yawAngle;
yawAngDataNew.yawAngErr = yawAngleErr;
yawAngDataNew.type = type;
yawAngDataNew.time_ms = timeStamp_ms;
storedYawAng.push(yawAngDataNew);
yawMeasTime_ms = timeStamp_ms;
}
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF3_core::writeDefaultAirSpeed(float airspeed)
{
defaultAirSpeed = airspeed;
}
/********************************************************
* External Navigation Measurements *
********************************************************/
void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
// protect against NaN
if (pos.is_nan() || isnan(posErr)) {
return;
}
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if (((timeStamp_ms - extNavMeasTime_ms) < frontend->extNavIntervalMin_ms) || !statesInitialised) {
return;
} else {
extNavMeasTime_ms = timeStamp_ms;
}
ext_nav_elements extNavDataNew {};
if (resetTime_ms != extNavLastPosResetTime_ms) {
extNavDataNew.posReset = true;
extNavLastPosResetTime_ms = resetTime_ms;
} else {
extNavDataNew.posReset = false;
}
extNavDataNew.pos = pos;
extNavDataNew.posErr = posErr;
// calculate timestamp
timeStamp_ms = timeStamp_ms - delay_ms;
// Correct for the average intersampling delay due to the filter update rate
timeStamp_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
extNavDataNew.time_ms = timeStamp_ms;
// store position data to buffer
storedExtNav.push(extNavDataNew);
// protect against attitude or angle being NaN
if (!quat.is_nan() && !isnan(angErr)) {
// extract yaw from the attitude
float roll_rad, pitch_rad, yaw_rad;
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
// ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
const float yaw_accuracy_rad = MAX(angErr, radians(5.0f));
writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2);
}
}