7 replies to this topic

[Corvus Corax]

Hi, dear INS developers

Several people are - or have been working on ways to implement optical SLAM on MAV's (I am trying to do this for my thesis as well).

OpenPilot is almost the perfect underlying flight control platform for such an attempt.

For that to work smoothly, the INS board needs to support one important feature, and that is means for external correction of its internal representation.

Assume an indoor environment where - in the worst case - GPS is unavailable and magnetic data gets highly unreliable, optical SLAM is the only save way of navigation. Or even worse - a mixed outdoor/indoor environment, like flying into caves or ruins where both magnetometers and GPS become unavailable during flight.

In such cases, a slam algorithm on an external board can provide the INS with external attitude, velocity and position data - already nicely translated to the openpilot reference frame - even with dynamic uncertainty data supplied for each of those on the fly. There's only one caveat:

SLAM is slow, the data supplied by it will be

1. relatively infrequent (between once every 10 or every 100 ms - like GPS or even longer)

2. outdated! Even more so than GPS, SLAM takes time. That starts with the image acquisition, optical filtering, feature detection, ... Takes between 10 and 100ms depending on hardware and calculation method.

To use this data I propose the following:

1. Introduce a (32bit INT)Timestamp in the AttitudeRaw object (and possible also AttitudeActual), that is simply an internal cycle counter of the INS algorithm in use.

2. Introduce a new object INSCorrection, which allows to feed correction data to the INS. It should contain the following fields:

Timestamp (uint32_t the corresponding timestamp when the external measurement was made)
delta-Attitude (quaternion q1,q2,q3,q4, (un)certainty)
delta-Velocity (X,Y,Z, (un)certainty(in each axis))
delta-Position (X,Y,Z, (un)certainty(in each axis))

Whenever the INSCorrection UAVObject gets updated the following should happen:

- The INS both knows the reference cycle count when the external measurement was made (T1) and its current internal cycle count (T2)

- Assuming that the gyroscope sensor works correctly, it can be assumed that the attitude error around all 3 axis is still the same in T2 as it was in T1 (plus an additional a per-cycle uncertainty factor depending on how long ago T1 was) so in the next update cycle, the attitude in time T2 corrected by the SAME delta-Attitude (rotational error) as supplied should be taken into account with a certainty derived from the supplied certainty reduced slightly by the age-uncertainty (difference between T1 and T2)

- The same goes for the velocity, which is basically an integration over accelerometers. Take the velocity in T2, apply the same correction, and include in the next cycle with the calculated total uncertainty

- position is a bit more tricky, since the assumption of "same error" doesn't necessarily hold. I propose two approaches:

1. simply correct the exact same way and just add the delta (hoping that as the velocity error approaches zero, so will eventually the position error over many SLAM based corections)

2. assume the velocity error had remained mostly constant, and calculate a corected position by

(current position - delta-Position - (deltaVelocity*(deltaTime)))

that too won't be perfect, but it at least allows a crude assumption regarding the additional error introduced since T1 - either way - and depending on the corection mechanism used, I would set the certainty decrease of this position "measurement" (or educated guess) over time much higher than for attitude and velocity.

So 2 questions:

1. Can this be implemented at all with the current INS approach?

2. Would you who are working on INS put that feature in please )?

It would not only allow SLAM, but for any external sensor data to be fed to the INS - like sonic/light distance sensors, radar distance, possibly pivot/airspeed, ..., ...,

[joecnc2006]

If the Sonic sensor can be used in conjunction with the GPS and or when GPS is not present the sonic could kick in, say at an altitude of 20 meters, this would allow for better self landing also. A pinging radar type system would be interesting to see how that would work in obstacle avoidance.

[peabody124]

The correction steps are already written like that to deal with relatively infrequent data.  GPS/Baro/Mag are all only incorporated when they have new values.

[Kenn Sebesta]

Corvus Corax, on 04 October 2011 - 09:53 AM, said:

Hi, dear INS developers

Several people are - or have been working on ways to implement optical SLAM on MAV's (I am trying to do this for my thesis as well).

OpenPilot is almost the perfect underlying flight control platform for such an attempt.

For that to work smoothly, the INS board needs to support one important feature, and that is means for external correction of its internal representation.

Assume an indoor environment where - in the worst case - GPS is unavailable and magnetic data gets highly unreliable, optical SLAM is the only save way of navigation. Or even worse - a mixed outdoor/indoor environment, like flying into caves or ruins where both magnetometers and GPS become unavailable during flight.

In such cases, a slam algorithm on an external board can provide the INS with external attitude, velocity and position data - already nicely translated to the openpilot reference frame - even with dynamic uncertainty data supplied for each of those on the fly. There's only one caveat:

SLAM is slow, the data supplied by it will be

1. relatively infrequent (between once every 10 or every 100 ms - like GPS or even longer)

2. outdated! Even more so than GPS, SLAM takes time. That starts with the image acquisition, optical filtering, feature detection, ... Takes between 10 and 100ms depending on hardware and calculation method.

To use this data I propose the following:

1. Introduce a (32bit INT)Timestamp in the AttitudeRaw object (and possible also AttitudeActual), that is simply an internal cycle counter of the INS algorithm in use.

2. Introduce a new object INSCorrection, which allows to feed correction data to the INS. It should contain the following fields:

Timestamp (uint32_t the corresponding timestamp when the external measurement was made)
delta-Attitude (quaternion q1,q2,q3,q4, (un)certainty)
delta-Velocity (X,Y,Z, (un)certainty(in each axis))
delta-Position (X,Y,Z, (un)certainty(in each axis))

Whenever the INSCorrection UAVObject gets updated the following should happen:

- The INS both knows the reference cycle count when the external measurement was made (T1) and its current internal cycle count (T2)

- Assuming that the gyroscope sensor works correctly, it can be assumed that the attitude error around all 3 axis is still the same in T2 as it was in T1 (plus an additional a per-cycle uncertainty factor depending on how long ago T1 was) so in the next update cycle, the attitude in time T2 corrected by the SAME delta-Attitude (rotational error) as supplied should be taken into account with a certainty derived from the supplied certainty reduced slightly by the age-uncertainty (difference between T1 and T2)

- The same goes for the velocity, which is basically an integration over accelerometers. Take the velocity in T2, apply the same correction, and include in the next cycle with the calculated total uncertainty

- position is a bit more tricky, since the assumption of "same error" doesn't necessarily hold. I propose two approaches:

1. simply correct the exact same way and just add the delta (hoping that as the velocity error approaches zero, so will eventually the position error over many SLAM based corections)

2. assume the velocity error had remained mostly constant, and calculate a corected position by

(current position - delta-Position - (deltaVelocity*(deltaTime)))

that too won't be perfect, but it at least allows a crude assumption regarding the additional error introduced since T1 - either way - and depending on the corection mechanism used, I would set the certainty decrease of this position "measurement" (or educated guess) over time much higher than for attitude and velocity.

So 2 questions:

1. Can this be implemented at all with the current INS approach?

2. Would you who are working on INS put that feature in please )?

It would not only allow SLAM, but for any external sensor data to be fed to the INS - like sonic/light distance sensors, radar distance, possibly pivot/airspeed, ..., ...,

Why not just keep a buffer of past measurements, up to the expected delay, and then perform a series of state predictions and corrections all the way up until the current time? This would seem the simplest thing to do. There should be plenty of processor overhead for this, as your delay isn't likely to be that long, perhaps several hundred ms, and thus there would only be 10-50 prediction/correction cycles, depending on EKF speed. Could even get lazy and cut the number of EKF updates in half by averaging pairs of sensor measurements together. For the purpose of having a nice 3-D position estimate, this might do the ticket.

I think this would be the most mathematically correct way of handling things, and most likely the easiest way from a programming point of view since it's only a buffer of past measurements and a call to the EKF routine.

In any case, you're absolutely right, the INS needs to be able to take in an external correction. I can imagine a CC flying in a Vicon equipped room, where the CC handles all the flying tasks, but the absolute position and orientation estimates come from the mocap system. (By "I can imagine", I mean this might be happening at BU in the coming year.)

[Corvus Corax]

peabody124, on 04 October 2011 - 01:51 PM, said:

The correction steps are already written like that to deal with relatively infrequent data.  GPS/Baro/Mag are all only incorporated when they have new values.

yeah I know, but to my knowledge they still assume that at the point when such an update occurs it's current data.
a SLAM won't give you that, a slam system would tell you "i just finished my processing, and 50ms ago the position was <state> - the state we have now I'm gonna tell you in another 50 ms
so we need some way to introduce historic data - which requires either
- keep a ring buffer of historic states and re-do all calculations since every time we get an update from the "slower" algorithm
(which is expensive on CPU and memory, and would cause "jumps")
or
- plot-ahead the current position based on the exact (but outdated) data plus the data already calculated by the filter.

or am I mistaken?

[Corvus Corax]

that being said - the calculation to create estimated current data that can be introduced into the running filter loop can be done outside the EKF chip by whatever component runs the SLAM. so in principle the ability to merge in external updates as we have it IS enough IF the external component (SLAM) can get all the data it needs (like propabilistic/reliance data for each time step - so far we have no UAVObject for those but tht should be comparably easy)

[peabody124]

Corvus Corax, on 11 October 2011 - 07:11 PM, said:

yeah I know, but to my knowledge they still assume that at the point when such an update occurs it's current data.

Ah, correct.

Quote

so we need some way to introduce historic data - which requires either
- keep a ring buffer of historic states and re-do all calculations since every time we get an update from the "slower" algorithm
(which is expensive on CPU and memory, and would cause "jumps")
or
- plot-ahead the current position based on the exact (but outdated) data plus the data already calculated by the filter.

You should read what is done to account for GPS latency.  It's normally the former.  The prediction method would probably have worse performance than ignoring the problem.  But remember it would be the older data you would be predicting into the future, i.e. the SLAM results.  If you could do that well it might work.

[Max]

Corvus - I'm wondering if you've got any progress on this yourself? We're looking to do exactly the same thing - SLAM, sonar, and INS data all EKF'd together; but with the INS and SLAM correcting each other. (all this for a quad, in our case).

We'd figured it would be best to keep a ring buffer and recalculate; if we get SLAM data every second or so, there shouldn't be too large a "jump". If so, we might have to implement some sort of scan matching using LIDAR / depth cameras / etc, and feed that into the EKF, too. But I'd be interested to hear if you've made any breakthroughs yourself...!