New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

EKF3: Add basic support for use of external nav system data #8730

Open
wants to merge 6 commits into
base: master
from

Conversation

Projects
None yet
3 participants
@priseborough
Copy link
Contributor

priseborough commented Jun 25, 2018

Adds support for fusion of external navigation position and quaternion data to EKF3. This uses the same methods and interfaces as introduce into EKF2 by commit c680b93

priseborough added some commits Dec 27, 2017

AP_NavEKF3: Decouple height source from use of external nav data
Enables other height sources to be used when using external nav data for horizontal position.
Requires use of external nav data for height to be explicitly selected by the user.
External nav data can only be used for height if it is also being used for horizontal position.
@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jun 25, 2018

@rmackay9 FYI

@peterbarker
Copy link
Contributor

peterbarker left a comment

I've done a really quick pass through. I'll have to come back and really stare at this code to do a proper review - or I can do the more fun thing and create regression tests for it :-) I'll probably do a bit of both tomorrow.

@@ -192,8 +192,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {

// @Param: ALT_SOURCE
// @DisplayName: Primary altitude sensor source
// @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK3_GPS_TYPE = 3). Setting 2 uses GPS. Setting 3 uses the range beacon data. NOTE - the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
// @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK3_GPS_TYPE = 3). Setting 2 uses GPS. Setting 3 uses the range beacon data. NOTE - the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. Setting 4 uses external nav system data, but only if data is also being used for horizontal position.

This comment has been minimized.

@peterbarker

peterbarker Jun 26, 2018

Contributor

We should note it falls back to something else

This comment has been minimized.

@priseborough

priseborough Jun 30, 2018

Contributor

The description says that it falls back to baro. I need to check the code see if that is still the case.

@@ -247,17 +247,17 @@ void NavEKF3_core::setAidingMode()
bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);

// Check if GPS is being used
bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);

This comment has been minimized.

@peterbarker

peterbarker Jun 26, 2018

Contributor

This looks like a minor regression ; gpsPosUsed was a better name :-)

This comment has been minimized.

@priseborough

priseborough Jul 2, 2018

Contributor

Position data can now come from sources other than GPS. The comment above is what should change.

@@ -561,7 +577,7 @@ void NavEKF3_core::updateFilterStatus(void)
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3); // GPS glitching is affecting navigation accuracy

This comment has been minimized.

@peterbarker

peterbarker Jun 26, 2018

Contributor

Should we really have lost the clause about _fusionModeGPS?

This comment has been minimized.

@priseborough

priseborough Jul 2, 2018

Contributor

If PV_AidingMode == AID_ABSOLUTE and we are not using external nav data, then we must be using GPS (there are only two sources of absolute position information, GPS and external nav).

if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gpsDataDelayed.vel -= velOffsetEarth;

This comment has been minimized.

@peterbarker

peterbarker Jun 26, 2018

Contributor

This is odd - why change away from the vector operation?

This comment has been minimized.

@priseborough

priseborough Jul 2, 2018

Contributor

I think it is a left over result of a previous experiment to split the horizontal and vertical velocity. Will fix.

@@ -111,7 +111,7 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
bool NavEKF3_core::getHeightControlLimit(float &height) const
{
// only ask for limiting if we are doing optical flow navigation
if (frontend->_fusionModeGPS == 3) {
if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {

This comment has been minimized.

@peterbarker

peterbarker Jun 26, 2018

Contributor

Unrelated patch for flow? Might be correct, just unrelated?

This comment has been minimized.

@priseborough

priseborough Jul 6, 2018

Contributor

It was carried across from EKF2 mods. It prevents the optical flow height limit being incorrectly applied applied when the user has inhibited GPS use via the EK2_GPS_TYPE parameter and the vehicle is using either odometry or external nav measurements to navigate.

@peterbarker

This comment has been minimized.

Copy link
Contributor

peterbarker commented Jul 17, 2018

I rebased this branch so I could play with it. There was one conflict which I resolved by adding both added checks to the conflict resolution:

    filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy

I also put a patch on top which makes our autotest suite test both EKF2 and EKF3 versions of the vision position stuff.
https://github.com/peterbarker/ardupilot/tree/pr-ekf3ExtNav

Interestingly, that code reliably passes for EKF2 but reliably fails for EKF3. EKF3 gives a variance and forced landing:

Ready to FLY APM: EKF3 IMU0 buffers, IMU=6 , OBS=2 , dt=0.0120
APM: EKF3 IMU1 buffers, IMU=6 , OBS=2 , dt=0.0120
APM: EKF3 IMU0 initialised
APM: EKF3 IMU1 initialised
APM: EKF3 IMU1 initial yaw alignment complete
APM: EKF3 IMU0 initial yaw alignment complete
APM: EKF3 IMU0 tilt alignment complete
APM: EKF3 IMU1 tilt alignment complete
APM: EKF3 IMU0 is using external nav data
APM: EKF3 IMU0 initial pos NED = 0.0,0.0,-0.1 (m)
APM: EKF3 IMU1 is using external nav data
APM: EKF3 IMU1 initial pos NED = 0.0,0.0,-0.1 (m)
Time has wrapped
('Time has wrapped', 77344, 199044)
set streamrate 10
set streamrate 10
STABILIZE> Received (SYSTEM_TIME {time_unix_usec : 0, time_boot_ms : 77644})
set streamrate 5
AUTOTEST: Reboot complete
AUTOTEST: Waiting for non-zero-lat
set streamrate 5
STABILIZE> switch 6
AUTOTEST: Waiting for mode STABILIZE
switch 6
STABILIZE> AUTOTEST: Got mode STABILIZE
Set RC switch override to 6 (PWM=1815 channel=5)
rc 3 1000
rc 3 1000
STABILIZE> arm throttle
arm throttle
STABILIZE> APM: Arming motors
Got MAVLink msg: COMMAND_ACK {command : 400, result : 0}
ARMED
AUTOTEST: ARMED
rc 3 1700
rc 3 1700
STABILIZE> AUTOTEST: Waiting for altitude between 30 and 35
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:0, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:1, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:1, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:1, min_alt:30, climb_rate: 0
APM: EKF3 IMU0 ground mag anomaly, yaw re-aligned
APM: EKF3 IMU1 ground mag anomaly, yaw re-aligned
AUTOTEST: Wait Altitude: Cur:2, min_alt:30, climb_rate: 0
APM: EKF3 IMU0 in-flight yaw alignment complete
APM: EKF3 IMU1 in-flight yaw alignment complete
AUTOTEST: Wait Altitude: Cur:3, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:3, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:4, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:5, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:6, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:7, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:7, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:8, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:9, min_alt:30, climb_rate: 0
height 10
AUTOTEST: Wait Altitude: Cur:10, min_alt:30, climb_rate: 0
AUTOTEST: Wait Altitude: Cur:11, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:12, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:14, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:15, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:16, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:18, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:19, min_alt:30, climb_rate: 1
height 20
AUTOTEST: Wait Altitude: Cur:21, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:22, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:24, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:26, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:27, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:29, min_alt:30, climb_rate: 1
AUTOTEST: Wait Altitude: Cur:31, min_alt:30, climb_rate: 1
AUTOTEST: Altitude OK
height 30
rc 3 1500
rc 3 1500
STABILIZE> AUTOTEST: TAKEOFF COMPLETE
rc 1 1600
rc 1 1600
STABILIZE> Flight battery 100 percent
height 40
rc 1 1500
rc 1 1500
STABILIZE> AUTOTEST: # Enter RTL
switch 3
switch 3
STABILIZE> Set RC switch override to 3 (PWM=1425 channel=5)
rc 3 1500
rc 3 1500
STABILIZE> RTL> Mode RTL
APM: EKF variance
LAND> Mode LAND
APM: SmartRTL deactivated: bad position
height 30
height 20
height 10

The test in question is here:
https://github.com/peterbarker/ardupilot/blob/pr-ekf3ExtNav/Tools/autotest/arducopter.py#L1135

You can run that test like this (in the same environment you would run SITL in:

./Tools/autotest/autotest.py --gdb --debug build.ArduCopter fly.ArduCopter

when my branch is checked out. Logs appear in the logs/ directory.

@priseborough any idea why the variance? Is the test broken? If so, what steps should I add to it to make it pass?

@priseborough would you prefer to get logs rather than the repeatable Python test? Note that the ekf=2 version is run as part of our regression suite on every commit to master now.

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 23, 2018

Thanks Peter, I will have a look at it today.

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 23, 2018

@peterbarker I checked out your branch at commit 47ba8c0 and got the following run error when trying to replicate your results:

BUILD SUMMARY
Build directory: /home/parallels/SourceCode/ardupilot/build/sitl
Target Text Data BSS Total

bin/arducopter 2068616 2202 44280 2115098

Build commands will be stored in build/sitl/compile_commands.json
'build' finished successfully (6.367s)

PASSED STEP: build.ArduCopter at Mon Jul 23 16:33:21 2018
check step: build.ArduCopter
RUNNING STEP: fly.ArduCopter at Mon Jul 23 16:33:21 2018
Running: ("/bin/rm -f logs/*.BIN logs/LASTLOG.TXT") in (.)
Running: "xterm" "-e" "gdb" "-x" "/tmp/x.gdb" "--args" "/home/parallels/SourceCode/ardupilot/build/sitl/bin/arducopter" "-w" "-S" "--home" "-35.362938,149.165085,584,270" "--model" "+" "--speedup" "10" "--uartF=sim:vicon:"
Exception AttributeError: "'spawn' object has no attribute 'closed'" in <bound method spawn.del of <pexpect.spawn object at 0x7f922c958d50>> ignored
FAILED STEP: fly.ArduCopter at Mon Jul 23 16:33:21 2018 (init() got an unexpected keyword argument 'encoding')
Traceback (most recent call last):
File "./Tools/autotest/autotest.py", line 506, in run_tests
if run_step(step):
File "./Tools/autotest/autotest.py", line 319, in run_step
return tester.autotest()
File "/home/parallels/SourceCode/ardupilot/Tools/autotest/arducopter.py", line 1240, in autotest
self.init()
File "/home/parallels/SourceCode/ardupilot/Tools/autotest/arducopter.py", line 97, in init
wipe=True)
File "/home/parallels/SourceCode/ardupilot/Tools/autotest/pysim/util.py", line 245, in start_SITL
child = pexpect.spawn(first, rest, logfile=sys.stdout, encoding=ENCODING, timeout=5)
TypeError: init() got an unexpected keyword argument 'encoding'
check step: fly.ArduCopter
FAILED 1 tests: ['fly.ArduCopter']

@peterbarker

This comment has been minimized.

Copy link
Contributor

peterbarker commented Jul 23, 2018

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 23, 2018

I'm running 2.7.6

This VM (running in parallels) is only used for ArduPilot dev work so I can upgrade the python version.

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 27, 2018

After some further investigation, the root cause of the autotest failure was the amount of timing jitter in the simulated Vicon data - see

screen shot 2018-07-27 at 3 52 36 pm

was causing data to be overwritten before it could be fused so that effectively data was being dropped. This can be seen in the innovations - see

screen shot 2018-07-27 at 3 54 05 pm

This hypothesis was partially confirmed by removing the simulated timing jitter here https://github.com/ardupilot/ardupilot/blob/master/libraries/SITL/SIM_Vicon.cpp#L121 - see

screen shot 2018-07-27 at 4 23 04 pm

which fixed the innovations - see

screen shot 2018-07-27 at 4 23 09 pm

The next test was to restore the timing jitter, but increase the length of the data buffer to enable one sample per EKF major frame. This did not fix the problem completely. There was still lost data.

Inspection of the code used to read the external nav data showed that data arriving at less than a 70 msec interval from the previous sample was being rejected before it was written into the buffer. Reducing this restriction to 10 msec (the underlying EKF frame time) still did not fix the problem, indicating a more fundamental issue with the jitter and time stamping.

Reducing the jitter from 300 to 200 mSec improved the innovations

screen shot 2018-07-27 at 4 47 45 pm
screen shot 2018-07-27 at 4 47 52 pm

Reducing to 100msec:
screen shot 2018-07-27 at 4 50 55 pm
screen shot 2018-07-27 at 4 51 25 pm

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 27, 2018

The net data rate goes up as the jitter is increased, so part of the problem is caused by the reduced in data rate.

What average Vicon data rate is intended for the simulation?

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 29, 2018

Another clue is a problem with the vertical position innovations, even after removing the timing jitter:

screen shot 2018-07-29 at 6 56 16 pm

@priseborough

This comment has been minimized.

Copy link
Contributor

priseborough commented Jul 29, 2018

@peterbarker In addition to the excessive timing jitter that is causing a lot of data to be thrown away before it is written to the buffer, AP::ahrs().writeExtNavData in GCS_Common .cpp is setting posErr and angErr to 0. This does not respect the interface and is causing problems in the EKF.

This does not fully explain the height innovation problem, but I will need to get either print to terminal or gdb working on the autotest to debug the height issue further.

@peterbarker

This comment has been minimized.

Copy link
Contributor

peterbarker commented Oct 3, 2018

Note #9485

@chobitsfan

This comment has been minimized.

Copy link
Contributor

chobitsfan commented Oct 4, 2018

AP::ahrs().writeExtNavData in GCS_Common .cpp is setting posErr and angErr to 0. This does not respect the interface and is causing problems in the EKF.

There is no error information provided in either MAVLink message VISION_POSITION_ESTIMATE or ATT_POS_MOCAP. I guess that is why GCS_Common.cpp set posErr and angErr to 0. I have a small fix about this. In NavEKF2_core::writeExtNavData(), I set posErr to EK2_POSNE_M_NSE if GCS_Common.cpp send it as 0. @priseborough Would you mind taking a look at #9485 ? Thank you very much

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment