EV and GPS fusion

[ChristophTobler]

Currently the only way of fusing EV and GPS is by rotating EV measurements and fuse them as incremental position changes (assuming that the EV measurements are not in NED which is almost always the case). The downside of this is that we can't fully make use of the EV's performance. With the incremental position fusion the vehicle drifts more.

I see two options:

1. We keep the yaw from the magnetometer all the time (actual NED) and add an offset and rotation to https://github.com/PX4/ecl/blob/master/EKF/control.cpp#L289. That way we fuse the actual position.

2. We use yaw from EV if GPS isn't valid and switch to the mag if GPS gets valid and reset all EV related parameters (should also work the other way around). So if no GPS is available the can fuse actual EV position and if GPS is available we use the incremental approach.

[mhkabir]

The first approach sounds better to me.

[EliaTarasov]

In a number of cases it's better to avoid using magnetometer at all. I would vote for the second option, because it the first case we are totally depend on mag.

[ChristophTobler]

I implemented option 1 (branch) as it didn't require too much changes and did some tests.

Current Implementation (incremental pos fusion) doesn't work very good. When stationary on the ground the position drifts a lot. In air it's better (not sure why...) and can be used to fly in position mode. But the performance isn't very good... see below (EV is what the vehicle actually did -> almost ground truth)

When using option 1, performance is much better and there is no drift.

I also did some testing in sitl with switching GPS on and off and switching ev on and off (hardcoded counters) and it seems to work nicely. But this would need so more testing and review. I could open a PR with that branch if we all agree.

All tests include the rotation fix #396
logs

• master (incremental pos fusion): https://review.px4.io/plot_app?log=c933c22f-ea92-4882-b186-8283543cfc9c
• option 1 without GPS: https://review.px4.io/plot_app?log=6d6f16b9-58d0-4c95-8e0b-48e1f363ff74
• option 1 with GPS: https://review.px4.io/plot_app?log=54c354c9-e4ba-4e5a-a313-5cc611a13152

[ChristophTobler]

@EliaTarasov

In a number of cases it's better to avoid using magnetometer at all. I would vote for the second option, because it the first case we are totally depend on mag.

That's true. But for the mixed scenario EV/GPS we have to depend on the mag anyways (otherwise GPS doesn't work). So for scenarios where you know you don't have good mag readings you probably don't have GPS and then you could switch to using EV yaw.
But I can see that the performance of EV would of course be better when we would use option 2.
Thoughts?

[EliaTarasov]

Second option would also increase robustness especially in case of unreliable GPS environment.

[priseborough]

Incremental fusion requires significantly less observation noise to work effectively because noise represents error accumulated since last update.

[priseborough]

I will review the logs and see if there is something we can do with the observation noise to improve performance when we are using incremental fusion.

Direct fusion when not using GPS seems like a logical approach. The issue I see with using the magnetometer when GPS is not available, is that is usually when indoors therefore in environments where the earth field is often unreliable. If we are forced to use mag fusion when indoors, only heading fusion should be used, not 3-axis fusion.

[priseborough]

Here is a plot of the innovations and +- sqrt(innovation variance) from the delta position fusion.

The innovation variance with observation noise is too large and reducing it through retuning will reduce drift.

[priseborough]

I've inspected the code and there are a some issues with the way the ekf2 is interfaced to the vehicle_vision_position data:

1. The observation noise passed through to the ekf is the maximum of eph, epv and EKF2_EVP_NOISE parameter.
2. The ekf2 interface does not have a separate vertical and horizontal position error.

Also the vehicle_vision_position eph and epv values are unrealistically small, in the range from 0.001 to 0.004 for values that according to the vehicle_local_position uORB definition are 'Standard deviation of horizontal/vertical position error, (metres)'

[ChristophTobler]

@priseborough

Incremental fusion requires significantly less observation noise to work effectively because noise represents error accumulated since last update.

Makes sense! I retried with a smaller value in simulation and could already see a difference there.

Direct fusion when not using GPS seems like a logical approach. The issue I see with using the magnetometer when GPS is not available, is that is usually when indoors therefore in environments where the earth field is often unreliable. If we are forced to use mag fusion when indoors, only heading fusion should be used, not 3-axis fusion.

True. If we have a reliable solution to use the EV heading, I would vote for that. I only tried the first option because it required much less effort.
I guess we need to check what effects the switching between mag and GPS will have on e.g. missions. Because the heading (north, east, etc.) will change when switching.

How should we proceed?

[priseborough]

The heading change associated with your option 2) can be handled - we already have logic in place to manage the reset of states and covariance. We already use EV yaw if it is available and if enabled by EKF2_AID_MASK. The change to existing behaviour is therefore the switch to using magnetic yaw (and the yaw reset) when GPS fusion is starts.

I will have a look at the code and come up with a way to handle the yaw.

[priseborough]

For a related issue of when to stop using GPS - I have a prototype branch to handle the reversion back to non-GPs operation if already using flow or external vision here: https://github.com/PX4/ecl/tree/ekfGpsFallback-wip.

[priseborough]

One GPs fusion had commenced, the EKF was previously continuing to try and use GPS regardless of quality, provided it had a 3D lock. This makes sense when GPS is the only aiding method available, but not when external vision of optical flow is being used. This caused problems starting a flight indoors, flying outdoors, where GPS checks passed and then flying back indoors again. The degraded GPS could then cause significant position drift.

I will now tie this in to the yaw reset.

[EliaTarasov]

Question regarding attitude fusion:
Suppose we have mag and vision. Mag has low precision but provides attitude wrt NED. Vision has high precision but provides attitude only wrt to its own vision frame (but that can be rotated to NED).
Now EKF strictly separates vision yaw fusion from mag yaw fusion.
Can we fuse vision data to aid mag attitude estimation to have better NED attitude estimation when we have vision attitude aiding?

[priseborough]

That depends on the long term stability of the vision world frame reference frame rotation relative to NED. If it is stable, then we could continue to use it once we have determined the offset.

[EliaTarasov]

Okay.
Most probably it drifts with the time.
But if it is possible to estimate gyro drift (with help from the mag) then it is also possible to estimate vision drift (with the help of the mag again)?

[priseborough]

I've added functionality for the automatic selection of external vision yaw depending on GPS use to https://github.com/PX4/ecl/tree/ekfGpsFallback-wip

Conditional on the use of external vision for yaw being enabled by EKF2_AID_MASK, it will use external vision yaw for whenever the EKF is not using GPS. Otherwise the magnetometer will be used.

This will generate a yaw reset when GPS fusion starts or times out, but the reset delta is already reported in the vehicle_attitude message and consumed by the multi-copter position controller and used to adjust the attitude setpoint to prevent transients. I am not sure how well this change to setpoint works in practice.

I'm having some issues with SITL gazebo at the moment, so have not been able to test the transitions.

Edit: I've got SITL up and going and have started working through the transitions.

[priseborough]

Here's a log of a test of taking off with external vision, then enabling GPS lock using 'gpssim set -t 3'

https://logs.px4.io/plot_app?log=c97c138f-c55a-40d1-91d1-cb0a1db21bdc

The following changes were made to the iris_opt_flow config file

param set EKF2_AID_MASK 25
gpssim set -t 0

I'm running the sim using 'make posix gazebo_iris_opt_flow'

A loss of data link failsafe was triggered after GPS use started which I am still investigating, but the computer was laggy and slow at the time.

[priseborough]

The code currently uses EV data as an absolute position reference when GPs data is not available. When GPS is available (ad passes checks), it becomes the absolute position source and the vision data is used as an odometry source which helps to reduce the effect of GPS glitches.

[shylent]

@priseborough
I understand, that only one absolute yaw reference may be used at a given time, but there may be cases, where one might want to use vision yaw even if magnetometer is available.

For example, in our case, we take off and land into an enclosure, that is a source of electromagnetic interference (this can't be fully eliminated), that affect magnetometer readings significantly (up to 20-30 degrees offset). After takeoff is completed and the vehicle is out of the enclosure, magnetometer readings slowly come back to normal. The same happens in reverse during landing.

Vision positioning is only available during takeoff and landing and is implemented via markers, that are placed in an enclosure (which means, that transforming vision position to NED frame is trivial, since the orientation of enclosure w.r.t. north is known a priori). GPS fix is available at all times, even inside the enclosure (which means, that vision yaw is never fused due to https://github.com/PX4/ecl/blob/master/EKF/control.cpp#L559)

In such a case it would be extremely beneficial to use vision yaw while it is available and use magnetometer otherwise.

Is it feasible to add a (non-default!) code path, where this behaviour is implemented? We'd like your judgement on that. If it is feasible, then we are willing to implement such a feature and flight test it extensively. Perhaps a separate issue should be opened to track this?

[jkflying]

@shylent I've seen a very similar use-case using UWB positioning/orientation. Maybe using a vision-specific message isn't right here, it would be better to do some kind of high-accuracy position/orientation, or even something with the waypoints being specified in this local coordinate frame and instead what is estimated is the offset between the frames.

[jkflying]

I believe this is now solved with #642