EKF: support passing in an external position estimate #23903
Conversation
|
See tridge#41 |
|
Very interesting, I was thinking about doing something like this with an image tracking camera. Why wouldn't we go through the AP_VisualOdometry library though and use one of the many existing vision-position messages? |
a few reasons. First, we want the ACK/NACK as it is nacked when we are not in AID_NONE. Secondly, we need the dual timestamp to cope with much longer than normal lag - it would be beyond fusion horizon if done via vision msgs. |
|
log from a CubeOrange quadplane (MakeFlyEasy Hero): |
|
The positon and velocity states and their corresponding variances from the quadplane test flight have the expected behaviour, eg: Position sets do not directly affect the velocity states 
The position state variance set to the expected value as specified by p3 in the mavlink command Velocity state variances do not change at the position set events The log can be replayed successfully. |
|
I'd like to discuss this a bit before we merge. Obviously the functionality is great, it's just the architecture and possibly message name. Also I've got a similar change that adds a position reset for vision-position sources and I'd like us to make sure they're consistent. PR coming for that later today. |
|
Here's my related PR:
|
|
needs #if for flash cost, 2M boards only |
| const uint32_t timeStampConstrained_ms = MAX(MIN(timestamp_ms, imuSampleTime_ms), imuSampleTime_ms - 5000); | ||
| const int32_t delta_ms = int32_t(imuDataDelayed.time_ms - timeStampConstrained_ms); | ||
| const ftype delaySec = 1E-3F * ftype(delta_ms); | ||
| const Vector2F newPosNE = EKF_origin.get_distance_NE_ftype(loc) - stateStruct.velocity.xy() * delaySec; |
There was a problem hiding this comment.
@priseborough @tridge I might look at this the wrong way but shouldn't this be a positive sign here?
This allows for a position update from an external source such as someone spotting a landmark with an FPV camera.
MAVProxy support here:
ArduPilot/MAVProxy#1186
needs this mavlink PR: ArduPilot/mavlink#318