This repository has been archived by the owner on May 1, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 509
/
control.cpp
1739 lines (1399 loc) · 68 KB
/
control.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
/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file control.cpp
* Control functions for ekf attitude and position estimator.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include <mathlib/mathlib.h>
void Ekf::controlFusionModes()
{
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
// send alignment status message to the console
if (_control_status.flags.baro_hgt) {
ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else if (_control_status.flags.ev_hgt) {
ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else if (_control_status.flags.gps_hgt) {
ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else if (_control_status.flags.rng_hgt) {
ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
} else {
ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
}
}
}
// check for intermittent data (before pop_first_older_than)
const baroSample &baro_init = _baro_buffer.get_newest();
_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
const gpsSample &gps_init = _gps_buffer.get_newest();
_gps_hgt_intermittent = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
// check for arrival of new sensor data at the fusion time horizon
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
_delta_time_baro_us = _baro_sample_delayed.time_us;
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
// if we have a new baro sample save the delta time between this sample and the last sample which is
// used below for baro offset calculations
if (_baro_data_ready) {
_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
}
// calculate 2,2 element of rotation matrix from sensor frame to earth frame
// this is required for use of range finder and flow data
_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
// Get range data from buffer and check validity
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed);
checkRangeDataValidity();
if (_range_data_ready && !_rng_hgt_faulty) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}
// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
// This means we stop looking for new data until the old data has been fused.
if (!_flow_data_ready) {
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
}
// check if we should fuse flow data for terrain estimation
if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
// only fuse flow for terrain if range data hasn't been fused for 5 seconds
_flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000;
// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
}
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// control use of observations for aiding
controlMagFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlAirDataFusion();
controlBetaFusion();
controlDragFusion();
controlHeightFusion();
// For efficiency, fusion of direct state observations for position and velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, baro, range finder, etc)
controlVelPosFusion();
// Additional data from an external vision pose estimator can be fused.
controlExternalVisionFusion();
// Additional NE velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();
// check if we are no longer fusing measurements that directly constrain velocity drift
update_deadreckoning_status();
}
void Ekf::controlExternalVisionFusion()
{
// Check for new external vision data
if (_ev_data_ready) {
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
// external vision aiding selection logic
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
// check for a external vision measurement that has fallen behind the fusion time horizon
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
// turn on use of external vision measurements for position
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
_control_status.flags.ev_pos = true;
resetPosition();
ECL_INFO_TIMESTAMPED("EKF commencing external vision position fusion");
}
// turn on use of external vision measurements for velocity
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
_control_status.flags.ev_vel = true;
resetVelocity();
ECL_INFO_TIMESTAMPED("EKF commencing external vision velocity fusion");
}
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
&& !_ev_rot_mat_initialised) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
_ev_rot_mat_initialised = true;
ECL_INFO_TIMESTAMPED("EKF external vision aligned");
}
}
}
// external vision yaw aiding selection logic
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// don't start using EV data unless daa is arriving frequently
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observation quaternion
// get the roll, pitch, yaw estimates from the quaternion states
Quatf q_init(_state.quat_nominal);
Eulerf euler_init(q_init);
// get initial yaw from the observation quaternion
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
Quatf q_obs(ev_newest.quat);
Eulerf euler_obs(q_obs);
euler_init(2) = euler_obs(2);
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quatf quat_before_reset = _state.quat_nominal;
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quatf(euler_init);
uncorrelateQuatStates();
// adjust the quaternion covariances estimated yaw error
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_dec = false;
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
ECL_INFO_TIMESTAMPED("EKF commencing external vision yaw fusion");
}
}
// determine if we should start using the height observations
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
// don't start using EV data unless data is arriving frequently
if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
setControlEVHeight();
resetHeight();
}
}
// determine if we should use the vertical position observation
if (_control_status.flags.ev_hgt) {
_fuse_height = true;
}
// determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) {
_fuse_pos = true;
// correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_ev_sample_delayed.pos(0) -= pos_offset_earth(0);
_ev_sample_delayed.pos(1) -= pos_offset_earth(1);
_ev_sample_delayed.pos(2) -= pos_offset_earth(2);
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.fusion_mode & MASK_USE_GPS) {
_fuse_hpos_as_odom = true;
} else {
_fuse_hpos_as_odom = false;
}
if (_fuse_hpos_as_odom) {
if (!_hpos_prev_available) {
// no previous observation available to calculate position change
_fuse_pos = false;
_hpos_prev_available = true;
} else {
// calculate the change in position since the last measurement
Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev;
// rotate measurement into body frame is required when fusing with GPS
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
// use the change in position since the last measurement
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.5f);
}
// record observation and estimate for use next time
_pos_meas_prev = _ev_sample_delayed.pos;
_hpos_pred_prev(0) = _state.pos(0);
_hpos_pred_prev(1) = _state.pos(1);
} else {
// use the absolute position
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
}
_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0);
_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1);
// observation 1-STD error
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {
// don't reset velocity if we have another source of aiding constraining it
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) {
resetVelocity();
}
resetPosition();
}
}
// innovation gate size
_posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f);
}else{
_vel_pos_innov[3] = 0.0f;
_vel_pos_innov[4] = 0.0f;
}
// determine if we should use the velocity observations
if (_control_status.flags.ev_vel) {
_fuse_hor_vel = true;
_fuse_vert_vel = true;
Vector3f vel_aligned{_ev_sample_delayed.vel};
// rotate measurement into correct earth frame if required
if (_params.fusion_mode & MASK_ROTATE_EV) {
vel_aligned = _ev_rot_mat * _ev_sample_delayed.vel;
}
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
vel_aligned -= vel_offset_earth;
_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0);
_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1);
_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {
// don't reset velocity if we have another source of aiding constraining it
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) {
resetVelocity();
}
}
// observation 1-STD error
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = fmaxf(_ev_sample_delayed.velErr, 0.01f);
// innovation gate size
_vvelInnovGate = _hvelInnovGate = fmaxf(_params.ev_vel_innov_gate, 1.0f);
}
// Fuse available NED position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_vert_vel = _fuse_hor_vel = false;
_fuse_pos = _fuse_height = false;
_fuse_hpos_as_odom = false;
}
// determine if we should use the yaw observation
if (_control_status.flags.ev_yaw) {
fuseHeading();
}
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
&& (_time_last_imu >= _time_last_ext_vision)
&& ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false;
_control_status.flags.ev_vel = false;
_control_status.flags.ev_yaw = false;
ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped");
}
}
void Ekf::controlOpticalFlowFusion()
{
// Check if on ground motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) {
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
const float accel_norm = _accel_vec_filt.norm();
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|| (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
if (motion_is_excessive) {
_time_bad_motion_us = _imu_sample_delayed.time_us;
} else {
_time_good_motion_us = _imu_sample_delayed.time_us;
}
} else {
_time_bad_motion_us = 0;
_time_good_motion_us = _imu_sample_delayed.time_us;
}
// Accumulate autopilot gyro data across the same time interval as the flow sensor
_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
if (_flow_data_ready) {
// Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching
float gps_err_norm_lim;
if (_control_status.flags.opt_flow) {
gps_err_norm_lim = 0.7f;
} else {
gps_err_norm_lim = 1.0f;
}
// Check if we are in-air and require optical flow to control position drift
bool flow_required = _control_status.flags.in_air &&
(_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel) // is completely reliant on optical flow
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
if (!_inhibit_flow_use && _control_status.flags.opt_flow) {
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5);
bool flight_motion_not_ok = _control_status.flags.in_air && !_range_aid_mode_enabled;
if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) {
_inhibit_flow_use = true;
}
} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){
// allow use of optical flow if motion is suitable or we are reliant on it for flight navigation
bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6);
bool flight_motion_ok = _control_status.flags.in_air && _range_aid_mode_enabled;
if (preflight_motion_ok || flight_motion_ok || flow_required) {
_inhibit_flow_use = false;
}
}
// Handle cases where we are using optical flow but are no longer able to because data is old
// or its use has been inhibited.
if (_control_status.flags.opt_flow) {
if (_inhibit_flow_use) {
_control_status.flags.opt_flow = false;
_time_last_of_fuse = 0;
} else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) {
_control_status.flags.opt_flow = false;
}
}
// optical flow fusion mode selection logic
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude
&& !_inhibit_flow_use
&& get_terrain_valid()) // we have a valid distance to ground estimate
{
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid and use is not inhibited , start using optical flow aiding
if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout
_control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
if (flow_aid_only) {
resetVelocity();
resetPosition();
// align the output observer to the EKF states
alignOutputFilter();
}
}
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
_control_status.flags.opt_flow = false;
}
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
if (_control_status.flags.opt_flow
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos
&& !_control_status.flags.ev_vel) {
bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
if (do_reset) {
resetVelocity();
resetPosition();
}
}
// Only fuse optical flow if valid body rate compensation data is available
if (calcOptFlowBodyRateComp()) {
bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
if (!flow_quality_good && !_control_status.flags.in_air) {
// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
_flowRadXYcomp.zero();
} else {
// compensate for body motion to give a LOS rate
_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);
}
} else {
// don't use this flow data and wait for the next data to arrive
_flow_data_ready = false;
}
}
// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
// but use a relaxed time criteria to enable it to coast through bad range finder data
if (_control_status.flags.opt_flow && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)10e6)) {
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
}
_flow_data_ready = false;
}
}
void Ekf::controlGpsFusion()
{
// Check for new GPS data that has fallen behind the fusion time horizon
if (_gps_data_ready) {
// GPS yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_GPSYAW)
&& ISFINITE(_gps_sample_delayed.yaw)
&& _control_status.flags.tilt_align
&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
&& ((_time_last_imu - _time_last_gps) < (2 * GPS_MAX_INTERVAL))) {
if (resetGpsAntYaw()) {
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements and disable all other yaw fusion
_control_status.flags.gps_yaw = true;
_control_status.flags.ev_yaw = false;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_dec = false;
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
ECL_INFO_TIMESTAMPED("EKF commencing GPS yaw fusion");
}
}
// fuse the yaw observation
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6);
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
// If the heading is not aligned, reset the yaw and magnetic field states
// Do not use external vision for yaw if using GPS because yaw needs to be
// defined relative to an NED reference frame
if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req) {
_control_status.flags.ev_yaw = false;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
if (_mag_inhibit_yaw_reset_req) {
_mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
}
}
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
// if we are not already aiding with optical flow, then we need to reset the position and velocity
// otherwise we only need to reset the position
_control_status.flags.gps = true;
if (!_control_status.flags.opt_flow) {
if (!resetPosition() || !resetVelocity()) {
_control_status.flags.gps = false;
}
} else if (!resetPosition()) {
_control_status.flags.gps = false;
}
if (_control_status.flags.gps) {
ECL_INFO_TIMESTAMPED("EKF commencing GPS fusion");
_time_last_gps = _time_last_imu;
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
_control_status.flags.gps = false;
// Reset position state to external vision if we are going to use absolute values
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
resetPosition();
}
ECL_WARN_TIMESTAMPED("EKF GPS data quality poor - stopping use");
}
// handle the case when we now have GPS, but have not been using it for an extended period
if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something
bool do_reset = ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max)
&& ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max)
&& ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max)
&& ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
// We haven't had an absolute position fix for a longer time so need to do something
do_reset = do_reset || ((_time_last_imu - _time_last_pos_fuse) > (2 * _params.reset_timeout_max));
if (do_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw
_control_status.flags.mag_align_complete = realignYawGPS();
}
resetVelocity();
resetPosition();
_velpos_reset_request = false;
ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS");
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
// Only use GPS data for position and velocity aiding if enabled
if (_control_status.flags.gps) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;
// correct position and height for offset relative to IMU
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
_gps_sample_delayed.hgt += pos_offset_earth(2);
// calculate observation process noise
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
// if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate
_posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit);
} else {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
// calculate innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
// set innovation gate size
_posInnovGateNE = fmaxf(_params.gps_pos_innov_gate, 1.0f);
_hvelInnovGate = _vvelInnovGate = fmaxf(_params.gps_vel_innov_gate, 1.0f);
}
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
_control_status.flags.gps = false;
ECL_WARN_TIMESTAMPED("EKF GPS data stopped");
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
// Handle the case where we are fusing another position source along GPS,
// stop waiting for GPS after 1 s of lost signal
_control_status.flags.gps = false;
ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF");
}
}
void Ekf::controlHeightSensorTimeouts()
{
/*
* Handle the case where we have not fused height measurements recently and
* uncertainty exceeds the max allowable. Reset using the best available height
* measurement source, continue using it after the reset and declare the current
* source failed if we have switched.
*/
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical innovations being positive and not stale.
// Clipping causes the average accel reading to move towards zero which makes the INS think it is falling and produces positive vertical innovations
float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim);
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are independent
(sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error
(_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); // vertical velocity data is fresh
// record time of last bad vert accel
if (bad_vert_accel) {
_time_bad_vert_accel = _time_last_imu;
} else {
_time_good_vert_accel = _time_last_imu;
}
// declare a bad vertical acceleration measurement and make the declaration persist
// for a minimum of 10 seconds
if (_bad_vert_accel_detected) {
_bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
} else {
_bad_vert_accel_detected = bad_vert_accel;
}
// check if height is continuously failing because of accel errors
bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us);
// check if height has been inertial deadreckoning for too long
bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6);
// reset the vertical position and velocity states
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
// boolean that indicates we will do a height reset
bool reset_height = false;
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
const gpsSample &gps_init = _gps_buffer.get_newest();
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
const baroSample &baro_init = _baro_buffer.get_newest();
bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// check for inertial sensing errors in the last 10 seconds
bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
bool reset_to_gps = !_gps_hgt_intermittent && gps_hgt_accurate && !prev_bad_vert_accel;
// reset to GPS if GPS data is available and there is no Baro data
reset_to_gps = reset_to_gps || (!_gps_hgt_intermittent && !baro_hgt_available);
// reset to Baro if we are not doing a GPS reset and baro data is available
bool reset_to_baro = !reset_to_gps && baro_hgt_available;
if (reset_to_gps) {
// set height sensor health
_baro_hgt_faulty = true;
// reset the height mode
setControlGPSHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to GPS");
} else if (reset_to_baro) {
// set height sensor health
_baro_hgt_faulty = false;
// reset the height mode
setControlBaroHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF baro hgt timeout - reset to baro");
} else {
// we have nothing we can reset to
// deny a reset
reset_height = false;
}
}
// handle the case we are using GPS for height
if (_control_status.flags.gps_hgt) {
// check if GPS height is available
const gpsSample &gps_init = _gps_buffer.get_newest();
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
// check the baro height source for consistency and freshness
const baroSample &baro_init = _baro_buffer.get_newest();
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[9][9]) * sq(_params.baro_innov_gate);
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;
// if GPS height is unavailable and baro data is available, reset height to baro
reset_to_baro = reset_to_baro || (_gps_hgt_intermittent && baro_data_fresh);
// if we cannot switch to baro and GPS data is available, reset height to GPS
bool reset_to_gps = !reset_to_baro && !_gps_hgt_intermittent;
if (reset_to_baro) {
// set height sensor health
_baro_hgt_faulty = false;
// reset the height mode
setControlBaroHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to baro");
} else if (reset_to_gps) {
// reset the height mode
setControlGPSHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF gps hgt timeout - reset to GPS");
} else {
// we have nothing to reset to
reset_height = false;
}
}
// handle the case we are using range finder for height
if (_control_status.flags.rng_hgt) {
// check if range finder data is available
const rangeSample &rng_init = _range_buffer.get_newest();
bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check if baro data is available
const baroSample &baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// reset to baro if we have no range data and baro data is available
bool reset_to_baro = !rng_data_available && baro_data_available;
// reset to range data if it is available
bool reset_to_rng = rng_data_available;
if (reset_to_baro) {
// set height sensor health
_rng_hgt_faulty = true;
_baro_hgt_faulty = false;
// reset the height mode
setControlBaroHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
} else if (reset_to_rng) {
// set height sensor health
_rng_hgt_faulty = false;
// reset the height mode
setControlRangeHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
} else {
// we have nothing to reset to
reset_height = false;
}
}
// handle the case where we are using external vision data for height
if (_control_status.flags.ev_hgt) {
// check if vision data is available
const extVisionSample &ev_init = _ext_vision_buffer.get_newest();
bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
// check if baro data is available
const baroSample &baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// reset to baro if we have no vision data and baro data is available
bool reset_to_baro = !ev_data_available && baro_data_available;
// reset to ev data if it is available
bool reset_to_ev = ev_data_available;
if (reset_to_baro) {
// set height sensor health
_baro_hgt_faulty = false;
// reset the height mode
setControlBaroHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to baro");
} else if (reset_to_ev) {
// reset the height mode
setControlEVHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF ev hgt timeout - reset to ev hgt");
} else {
// we have nothing to reset to
reset_height = false;
}
}
// Reset vertical position and velocity states to the last measurement
if (reset_height) {
resetHeight();
// Reset the timout timer
_time_last_hgt_fuse = _time_last_imu;
}
}
}
void Ekf::controlHeightFusion()
{
// set control flags for the desired primary height source