WIP: Pozyx addition to precision landing

[AndreasAntener]

@ndepal Thanks for your push on precision landing! We are working on the same problem and have decided to use the UWB system Pozyx. This adds the initial driver for Pozyx. Thanks to the APM team for preparing that setup and the docs for it (see links below). I'm also using this PR to start the discussion on some open issues.

Discussion

• Abstraction of beacon data: is it ok to just leave that up to the landing estimator? We should also consider input from ROS. Important: publish common quality measure of landing target estimate.
• As per discussion with @priseborough the estimator integration of the beacon data should be revised. Ideally fusion happens on the raw data.
• High-level use of the precision landing: I would suggest adding a new waypoint command for it (MAVLink). I can imagine a few parameters that go with it (more than a waypoint can handle but lets see..).
• Activating complex precision landing logic: I guess that's a FlightTasks discussion?
• Further development of the Pozyx driver (approach and direction)

TODOs (this PR)

• Add new parameters to specify sensor offset from center of the UAV to align with landing platform
• Update the Arduino sketch (publish more data)
• Figure out how good the data is we get from Pozyx
• Upstream test
• Check integration, style and add to relevant configs

I have tested the driver with some simple mission functionality on our branch but not upstream. The driver expects a serial message coming from an Arduino connected to the Pozyx tag.

1. https://www.pozyx.io/
2. http://ardupilot.org/copter/docs/common-pozyx.html
3. https://github.com/ArduPilot/ardupilot/blob/master/Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino

[dagar]

Awesome! Does this require the same physical setup where you interface with an arduino running the Pozyx code?

[AndreasAntener]

@dagar correct. The Arduino sketch simply uses the Pozyx library to do the configuration of the beacons and then send messages via serial to the FC.

We've tested a setup with the 4 anchors paced in a 2x2 meter square.

[ndepal]

Cool! I'm very curious to see this in action. What kind of range and accuracy do you get from pozyx?

Ideally fusion happens on the raw data

I thought about this when writing the landing target estimator. This would probably mean increasing the state-space of the estimator. The price for this would be paid by everybody, even those who don't use precision landing. (I'm assuming that the state space dimensions are statically defined). I figure'd I would have a hard time getting that into upstream, so I discounted that idea.
For what it's worth, we're not seeing dramatic landing performance improvements with estimator fusion on (although we're testing together with RTK), so it's also not super valuable to us. That could of course change with a tighter integration.

[LorenzMeier]

@dpettenuzzo FYI

[priseborough]

@nicolaerosia fusion of range measurements to known locations in the main nav filter does not require additional states just a new observation model (which ArduPilot EKF3 that has shared math with PX4 ekf2 already has). Complications are:

1. offset of the range beacon system origin - solved using a 3-state auxiliary filter
2. ambiguity created by co-planar beacon constellations - solved by duplicating the 3-state filter from 1) for each hypothesis and using the innovation sequences compared to determine which side of the beacon 'plane' the vehicle was operating. Can also be solved more simply by specifying a geometric operational constraint, eg operating above the beacons always.
3. Numerical stability when very close to beacons - solved by switching to using beacon location as a position observation with range used as a spherical uncertainty when close.
4. If pseudo range (delta range) fusion is available, then a range offset state would needs to be estimated, but the Pozyx does output an absolute range measurement.

I implemented solutions to 1) and 2) in ArduPilot and prototyped a solution to 3), but would revisit them if implementing support in PX4 knowing what I now do about the likely use cases.

The amount of work required to implement and test all this is significant so it would be prudent to start using the Pozyx reported position as a simple cartesian position observation and gain more knowledge with the sensor error characteristics and operational limitations in the flight environment first.

[ndepal]

@priseborough Thanks for the insight.

I see how when fusing it as known position measurements it can be done without additional states.
I was thinking about how it would be done when trying to fuse velocity measurements (as it is done now, analogous to optical flow).

I see two benefits of that approach:

1. Easier setup for the user. The location of the beacon(s) does not have to be configured.
2. Easier fusion of beacon and GPS measurements as well as easier handling of gaining/losing track of the beacon: With a defined lat/lon position of the beacon, a disagreement of the two absolute positioning sources would have to be handled. Especially when the beacon comes in range/FOV or goes out of it, there is a potential for a position estimate jump.

How do you handle (2) in APM?

[priseborough]

The 3-state range beacon initialisation filter position estimate is used to set an offset between the GPS and range beacon, however the offset is only set once at commencement of range beacon fusion. It then switches to using range beacons as the primary reference (doesn't use both). when going back the other way, it resets the position states back to the GPS. This causes a jump in position, but position resets are reported to the position control loops which compensate for the change similarly to how we do it in PX4.

[ndepal]

Interesting.

What does that mean for me if I care about the vehicle's global position, rather than only smooth behavior? E.g. if I fly a mission starting over the beacon and then fly out of it's range. There will be a position estimate jump which is compensated for by the position controller also shifting its position setpoint. How does this affect the remainder of the mission? Will that offset in the position controller be kept (and thus the vehicle flies with an offset to the mission waypoints) or will it somehow smoothly be reduced back to 0?

[AndreasAntener]

Will that offset in the position controller be kept (and thus the vehicle flies with an offset to the mission waypoints) or will it somehow smoothly be reduced back to 0?

I don't know how it works currently, but we thought that in missions you want to remove the offset over time (quickly). For 2 reasons: The offset shifts the mission, and when coming back to the beacon system after a while the offset might also be wrong due to the drift in your global positioning system. Keeping the offset makes sense for manual (posctl) flights though.

[LorenzMeier]

@AndreasAntener Do you have any additional input or commits? It looks like this just needs a little more effort to get in. I've talked a bit to Paul about generic infrastructure sensors like markers and UWB and how we could start to build out an interface for the estimation pipeline in ECL.

[dk7xe]

Would be nice if the solution can not only be used for precision landing but also for indoor/GPS independent waypoint navigation.

[cmic0]

@AndreasAntener I am in the process of testing your PR but I have faced some issues in setting up the system.
Did you modify something on the pozyx/arduino side in order to get the system work with px4?
I had to change the #define CONFIG_TX_GAIN 33.5f to 33 and disable automatic configuration in order to make the Arduino Sketch work but still uORB messages are not being published.

I am now looking more into detail but maybe is just a missing piece in setting up the system.

[thomasgubler]

Additional WIP work done by @cmic0 is here: https://github.com/Auterion/Firmware/tree/pozyx_rebased

THINGS DONE:

• Fixed stack overflow issue on FMU side
• Rebased on upstream

TODO:

• Understand why z measurements are not taken into account.
• Proper Testing
• Refactor serial driver with yaml architecture (as all the other serial drivers)

[julianoes]

@cmic0 what's the state here?

[cmic0]

@julianoes last time I've been working on this I just had to do some flight testing outside..unluckily february was not the best time for testing so went in stale.
I can see some work coming for the upcoming months with a new UWB system btw.. I think this will be resurrected soon

FYI @ItsTimmy

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[TSC21]

I assume it's still relevant. @AndreasAntener @cmic0 @ItsTimmy @thomasgubler

[AmeliaEScott]

This PR should probably be closed, because I rebased and am updating it in #12956.

[TSC21]

This PR should probably be closed, because I rebased and am updating it in #12956.

@ItsTimmy I see two different drivers here though. If you have not tested #12956 with a Pozyx system, you can't really say this should be closed. I would rather have a way of changing the PR. @AndreasAntener are you still over this?

[AmeliaEScott]

Yes, I agree. I ended up changing that PR (#12956 ) to not include this driver, so this PR should still be relevant after all.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[LorenzMeier]

Thanks for your earlier contribution. Unfortunately as the project has overall evolved quite a bit, this change doesn't apply any more. Closing stale pull requests like this one is part of us working aggressively to bring down the PR review time so that we will be able to merge or reject PRs in the future in a much more timely fashion.