EKF3: improve source switching (GPS<->NonGPS)

[rmackay9]

This is a list of issues to improve the EKF3 source switching that was added with PR: https://github.com/ArduPilot/ardupilot/pull/14803

• [ ] support always aligning external system's heading to AHRS heading when not in use
• [ ] extend AP_NavEKF3::getVelInnovationsAndVariancesForSource to return innovations for:
  • [ ] wheel encoders
• [ ] add more lua scripts (like ahrs-sources.lua) to support automatic switching in more situations:
  • [ ] only GPS+T265 instead of GPS+T265+optical flow
• [ ] investigate why Rover EKF failsafe triggers when switching to GPS sources if GPS has been disabled for some time when it wasn't being used. I.e. switch to secondary source, set GPS_TYPE = 0, set GPS_TYPE = 1, switch to primary source.

Completed Items:

• [x] allow aligning to AHRS when gps-for-yaw used (align code can't see difference between external nav and GPS-for-yaw) (PR)
• [x] add more lua scripts to support automatic switching between GPS + optical flow (PR)
• [x] extend AP_NavEKF3::getVelInnovationsAndVariancesForSource to return innovations for optical flow (PR)
• [x] add support for switching source via MAVLink (PR) -- included in 4.2.0-dev
• [x] split EK3_SRCn_YAW = 2 (external) into "External Nav", "GPS" and maybe "GPS for yaw", "GSF"
• [x] remove need for AP_NavEKF_Source::setVelZSource (see PR: https://github.com/ArduPilot/ardupilot/pull/15815, https://github.com/ArduPilot/ardupilot/pull/15969)
• [x] add lua script for GPS + wheel encoders (PR: https://github.com/ArduPilot/ardupilot/pull/15932)
• [x] add wiki page with setup instructions (linked from here)
• [x] performance optimisation of AP_NavEKF_Source::configured_in_storage() using temp variable because once true it will always be true (see PR: https://github.com/ArduPilot/ardupilot/pull/15874)
• [x] check GPS + wheel encoders work (see issue: https://github.com/ArduPilot/ardupilot/issues/15125)

Relevant wiki pages:

• GPS/Non-GPS transitions

[diegodab]

Hello, does anyone know how to automatically switch between GPS and optical flow?

[rmackay9]

@diegodab,

Probably best to ask these sorts of questions in the Copter-4.1 forum but in any case, adding a lua script to support automatically switching between GPS and optical flow is still on the to-do list (see "GPS + optical flow" above). The best we have at the moment is a script that handles GPS, T265 and OpticalFlow (e.g. more than you want).

[diegodab]

@rmackay9 Hi, thank you for answering. The Lua script hat handles the GNSS, T265 and optical flow reports the following error:

$lua main.lua lua: main.lua:47: attempt to index a nil value (global 'rc') stack traceback: main.lua:47: in function 'update' (...tail calls...) [C]: in ?

Does it work even though it gives that message when executing?

[yubo105139]

Please tell me, I need to switch t265 with gps automatically . Is there any way to achieve this?

[rmackay9]

@yubo105139,

Yes, on the GPS / Non-GPS Transitions page we have a link to a lua script that can do this.

[yubo105139]

@rmackay9 🙏 thanks!!!

[christomaszewski]

I'm trying to wrap my head around how these source sets interact with the lanes on the EKF. I've seen a previous comment that for now the lanes all use the currently active source set but has there been any discussion/movement on being able to assign a separate source set to different ekf lanes? Is this something that is better than simply switching source sets because the ekf will be running separate instances for each source set all the time?

[rmackay9]

Hi @christomaszewski,

That has been discussed but we're not actively working on it. The two other possibilities that are similar to what you're discussing is "sensor affinity" and the other is that both EKF2 and EKF3 can be enabled and then the EKF2 can be the estimator that uses GPS

[christomaszewski]

@rmackay9 Thanks for your reply! In my application I want to be able to switch between a typical here3 gps and my solution computed aboard a computer connected to ap via serial. I am able to send GPS_INPUT message from the computer to the ap and see them in mission planner and I have enabled the sensor affinity for GPS. Is there a way I can force the use of a specific lane on the EKF for testing and as a failsafe during flight? Once I disable GPS input into my algorithm its lane will undoubtedly be worse than the lane using the standard GPS but I would like to verify that the vehicle can still operate using my solution.

[rmackay9]

Hi @christomaszewski,

It's high on my list to write a developer wiki page on this topic because it comes up a lot so apologies for not having that ready (yet). In any case, I think it would be better to use one of the other supported messages like ODOMETRY or VISION_POSITION_ESTIMATE+VISION_VELOCITY_ESTIMATE instead of GPS_INPUT. Then enable EKF2 (EK2_ENABLE=1) and make it the primary AHRS (set AHRS_EKF_TYPE=2) then fly it and both EKF2 and EKF3 estimates will be stored in the logs.

If you really want to stick with the fake GPS then I think all GPSs are logged and so that would be an easy way to compare the results from the real GPS and the fake. That would just be testing your new non-GPS method's outputs though, it wouldn't show how easily the EKF consumes it.

[christomaszewski]

Hi @rmackay9, Thanks for the suggestions. I'll look into those. Is it possible/advisable to switch between the AHRS using EKF2 and EKF3 midflight?

It would be great if I could come up with some sort of configuration that would allow me to revert to using the standard gps if my non-gps solution goes completely off the rails. But I'd still like to use my solution if it is only marginally worse than the true gps if that's possible.

I understand there might not be anything that can fit my exact use case at the moment but thank you for all of your help!

[rmackay9]

Hi @christomaszewski,

I don't think it is possible to change between EK2 and EK3 inflight although you could try changing AHRS_EKF_TYPE. If it does work it probably won't be a smooth transition because we don't official support switching between them

I think the best way for a production vehicle is to change the solution over to send ODOMETRY messages and then setup the EK3_SRC1/2/3 so that the user (or a Lua script) can change between the two solutions

[christomaszewski]

Hey @rmackay9

Is there some convenient way to convert from the data in an emulated GPS_INPUT message and Imu data to an ODOMETRY message? I saw a comment mentioning it was possible on an old ardurover forum post but have not been able to track down an example.

On a side note, it would be awesome if GPS1 and GPS2 could be specified independently for the EKF sources.

[christomaszewski]

Hey @rmackay9 So I messed around with this all day but I can't seem to get ardupilot to use any ODOMETRY messages I pass it. It seems like it wants a VISION_POSITION_ESTIMATE but those don't let me specify the position using gps coordinates so I'm not sure how to convert my computed position into something using in whatever frame the visual position messages are expecting to be in. If you can point me to any advice on this I would appreciate it. Thanks!

[clydemcqueen]

It looks like ODOMETRY only supports a single local frame (meters from the origin) for position, not a global (lat, lon) frame.

Here is where the frames are enforced:

https://github.com/ArduPilot/ardupilot/blob/9543eecf233d20a0a01c07711bfa49d2e2c13aa7/libraries/GCS_MAVLink/GCS_Common.cpp#L3787

You'll have to convert lat, lon to meters from the origin to use this message.

[rmackay9]

@christomaszewski (and @clydemcqueen),

Ah, right, sorry, I'd forgotten that our visual odometry library doesn't accept a Location (e.g. lat, lon, alt). If the system that you're working on actually produces lat,lon,alt then maybe we should find a way to make visual odometry accept it. We've got a GSoC project that also needs this actually as well.

If your system doesn't provide lat, lon, alt and instead is providing a position from some arbitrary origin then it should be possible to just feed in that position using VISION_POSITION_ESTIMATE. By default AP will assume that's an offset from the EKF origin but if a real GPS is used as well then it will automatically shift that external position to match the GPS when that external position is not being consumed by the EKF

[christomaszewski]

@rmackay9 Yes the system I am working on produces full pose estimates in ECEF or Lat/Long/Altitude so I would love a way to feed that in directly to the auto pilot as an EKF source instead of simply a fake GPS using GPS_INPUT. I think this would be very useful for integrating other standalone INS units as well. For example we could feed the fused output from a vectornav vn-300 into the EKF directly vs trying to spilt it up and feed it in as separate sensors.

On a side note, I think it would be very helpful to have support for specifying an individual GPS as an EKF source (i.e. GPS1 or GPS2) vs. just specifying GPS in general. Perhaps the source field could be expanded to include GPS1, GPS2, and Combined GPS (which would use whatever GPS Blending rule you have set in the parameters - I believe this is the behavior now when you set the GPS as an EKF source).

[rmackay9]

Hi @christomaszewski,

BTW, AP supports the vector nav systems already as an external ahrs https://ardupilot.org/copter/docs/common-external-ahrs.html

[omkshirsagar90]

@diegodab, @rmackay9 Probably best to ask these sorts of questions in the Copter-4.1 forum but in any case, adding a lua script to support automatically switching between GPS and optical flow is still on the to-do list (see "GPS + optical flow" above). The best we have at the moment is a script that handles GPS, T265 and OpticalFlow (e.g. more than you want).

i am trying to use this script with arkflow sensor but when script is enabled with rc switch the source always switches to source 2 i.e. optical flow.i am not able to switch it to source 1 i.e. GPS with the script. i have set scr_user1 param to 3,scr_user2 to 0.3,scr_user3 to 50, SCR_USER4 to 0.15. I can switch between sources manually and all works fine but in autonmatic switching script only source 2 is being used

[rmackay9]

Hi @omkshirsagar90, it sounds like maybe the transmitter only has a two position switch? If "yes" then maybe it's "endpoint" could be adjusted so that when the switch is raised it moves to about 1500pwm?

[omkshirsagar90]

https://drive.google.com/file/d/1vcjejLbAbqTm6X5kOqBHZC38T5wIuNHz/view?usp=sharing We have a 3 position switch for scripting1 and source switching. Manually i am able to switch between sources, the script switch is also working fine but the script is not able to switch between sources. the script always switches to optical flow. Even if the gps has 32 sat counts and hdop 0.5 the script wont switch to gps after moving the copter above user 1 set altitude.all the tests are performed in the poshold mode The log file is attached to the drive link. please have a look . https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Scripting/examples/ahrs-source-gps-optflow.lua