Fusion of optical flow measurements into the Navigation EKF - Progress Report

[Paul Riseborough]

Some of you would be aware that since supporting the implementation of the EKF in APM and PX4, I have been working on incorporating fusion of optical flow measurements into the EKF using a data set supplied by Lorenz.

After a number of false starts, I now have a prototype working in a proof of concept test bench https://github.com/priseborough/InertialNav/blob/optflow-wip/code/estimator_23states.cpp which uses the raw flow messages and can do the necessary calibration of the sensor focal length automatically. This involved addition of a state to the navigation EKF to estimate the local terrain elevation, update of all the filter equations to accommodate the additional state, implementation of the equations for the fusion of the optical flow rates and implementation of a separate EKF to estimate the sensor focal length scale factor. Some earlier attempts were made to try and integrate estimation of the focal length into the main filter, but this ran into problems with filter stability.

The good news is that this implementation is compatible data available on the existing MAVLINK optical_flow message and has also shown it is possible to estimate height above ground without use of a range sensor (if you have a independent measure of velocity like GPS), or can it can constrain drift when GPS is not available if you have a means of estimating height above ground, eg laser).

The first plot shows the convergence of the focal length scale factor over a flight, optical flow rate innovations and he laser range (in red) superimposed over the optical flow derived height estimate (in blue). The second plot shows the attitude estimate from the filter compared with the PX4 on-board solution.

These are preliminary results and there has been no filter tuning or data quality checking, but it shows the optical flow can provide an estimate of height above ground when laser readings are unavailable and that the optical flow fusion is working correctly. The combination of a laser and optical flow sensor, enables height above ground to be estimated across a larger range of heights.

The next step is to gather some more data sets (i will try to get one recorded from a copter) and do some more work on tuning and data quality checks. The prototype is currently not using the flow quality measure, so that will be one item to look at. Once the improvements will be rolled back into the optflow-wip branch and the testbench is producing reliable results, I can then start an APM implementation

Thank you to Lorenz and the PX4 team for the flight data, 3DR for providing a sensor to experiment with, and Randy for his patch integrating the PX4Flow sensor, which i will try to gather data with this weekend.

Regards,

Paul

[Peter Plischka]

Hi Paul,

great that it goes on.

If you have to test something, let me know.

I have an optical flow sensor here. A laser sensor so far I could not get.

regards Peter

[David Pawlak]

Invaluable work. You are and have been writing your name in UAV history.

Thanks

[Craig Elder]

This is looking great Paul.
Thank you so very much!

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Paul Riseborough]

I flew Randy's px4flow2 branch on the weekend to get some copter data with a px4flow sensor on APM and have run into a issue that could be due to my PiXhawk + PX4Flow setup as it doesn't happen on Randy's hardware.

If the first application of power is by usb (either PC or using a USB wall adaptor) then I get the
FMT, 12, 24, OF, IBhhfff, TimeMS,Qual,RawX,RawY,VelX,VelY,Dist
header in the flashlog followed by the OF messages at a 50Hz rate. I can plug the battery in after it boots up and remove USB power, and it doesn't seem to matter when i arm realtive to application of battery power or removal of usb power.

If however the first application of power is with the battery, then I only get the header in the flashlog, and no further OF messages are logged.

Has anyone else tried logging data from a flight using Randy's px4flow2 branch?

[Meier Lorenz]

Hi Paul,

Try putting a

sleep 5

(vary a bit between 1 and 5)

In the startup script before probing the flow sensor. It might be that FMU tries to talk to it when its not yet ready. Its probably just a few micro or milliseconds one would have to wait.

-Lorenz

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com<mailto:drones-discus...@googlegroups.com>.

[Paul Riseborough]

Thanks, I'll give that a go. I was able to get some flow flight logs today by connecting to the usb first, so now I'm reworking my original APMPlane flashlog  conversion scripts to work with the copter format.

[Paul Riseborough]

RANGE FINDER FUSION DISABLED - HEIGHT ABOVE GROUND (BLUE) ESTIMATED USING OPTICAL FLOW, RANGE FINDER IN RED, NAV PRESSURE ALT IN GREEN

GPS TURNED OFF AT 50 SECONDS - ONLY MAGNETOMETER AND OPTICAL FLOW FUSION

I was able to tweak the test-bench to use the PX4Flow quality measure along with some other tweaks and now the optical flow estimated height above ground converges to a good match with the laser when the plane approaches the landing point. The bottom graph in the first figure shows the raw (uncompensated for aircraft pitch and roll) laser range range in red, the EKF navigation height (height above pressure reference) in green and the EKF height above ground in blue. The laser was only used for comparison purposes in this example and was not fused as part of the solution. The initial focal length estimate has changed from previous results presented, hence the convergence on a different scale factor value.

You can see that as the plane descend towards the runway and levels out at 5m altitude before climbing, that the EKF is able to provide an estimate of height above ground that agrees closely with the raw laser reading until the plane climbs away, turns and the slant range measured by the laser increases.

Another test was to disable airspeed fusion and turn off GPS at the 50 second mark  (which would normally lead to a runaway divergence in velocity), leaving only the magnetometer and optical flow fusion enabled. The filter was able to dead reckon its position with a drift of 500m over 5 minutes, or 100 metres a minute. The second figure shows the position estimated over this time period. The drift direction and speed is very consistent, but its error source is not known.

I was able to log PX4Flow data on APMCopter this weekend and was hoping to run it through the test harness, but have run into  what appears to be a problem with aliasing on the down sampled 50Hz data which degrades the inertial solution on replay to the extent that I am unable to use the data. I will have a go at implementing a moving average to down sample the inertial data from the 400Hz to 50Hz before it is logged and see if that solves the problem.

Regards,

Paul

[Randy Mackay]

Paul,

 

     Nice to see the update.  I will sort out that start-up issue in the next couple of days, certainly before the weekend.

 

     So the alt estimate based on optflow looks good especially compared to the baro.  I guess the laser range finder alt is all over the place because the vehicle is rolling or pitching.

 

     The drift of 100m / minute seems really high.  I guess it depends a bit on how quickly it gets to high speed, i.e. if that’s all in the last 10 seconds, then great, but if it’s immediately starts drifting by about 1.5m/s that’s not so useful.  Anyway, early days still I guess.

 

     The optflow came up on the call today.  We decided we should merge the optflow changes into AC master and also make the sonar that’s on the sensor available as a range-finder like any other sonar or laser range finder.

 

     Are there any changes you need made to the code on the sensor?  .. maybe you’re talking to Lorenz about that but just wondering.

 

-Randy

--

You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.

[Jesus Alvarez]

Paul,

Maybe this idea is crazy and nonsense but... what if the pxflow sensor orientation could be configurable?

Lets see (I see) the pxflow as a "suction pad". The idea is that Loiter (or whatever mode that uses the pxflow) could not only track terrain below but whatever surface it has in the orientation the user has defined.

So for example I am thinking in the inspection of an hydraulic dam. If I could install pxflow in the front of the copter and tell him the orientation pxflow is facing, then I could "stitch" the copter to the given position in the dam wall.

 

In the extreme case, we could have more than one pxflow facing different orientation....

Just thinking...

[Robert Lefebvre]

I support this crazy idea.  That graffiti project I was working on a year ago really could have used this.  And the applications are endless.

Anything we can do in the direction of helping a UAV navigate indoors will be time well-spent. 

--

[Daniel Nugent]

Take it a step further and run a low resolution SLAM algorithm for mapping. Not familiar what kind of resolution and processing is required for SLAM. Would be cool for cheap obstacle avoidance. Anyone have any knowledge on SLAM?

Optical flow only works in 2 dimensions right now, correct? How does it behave on uneven terrain?

Daniel

[Paul Riseborough]

Randy,

I'm not too concerned about the drift at this stage. This data was taken from a plane flying with a 4mm focal length lens (16mm is the standard lens fitted for copter use) banking up to 60 degrees so the optical flow fusion was not active all the time and the camera resolution was reduced compared to what would be flown on a copter.

I will make some mods to the APM copter logging to do the 400 to 50Hz  IMU downsampling and try again this weekend to get some copter data I can use in the replay. This will be with a 4.6mm focal length lens.

The best lens to use will depend on the range of speed and height the optical flow is required to operate over. For position  and low speed loiter control with copters, a narrow fov lens like the 16mm one fitted as standard to the PX4Flow is a better choice. For fixed wing applications where terrain height estimation is required and the sensor is there to augment the GPS and provide the ability to ride out periods without GPS, a larger fov, smaller focal length lens will be a better choice.

The PX4Flow project has a table at the bottom of http://pixhawk.org/modules/px4flow showing the theoretical maximum velocity that can be measured as a function of height and focal length. Some margin needs to be subtracted from these values to allow for body angular rates.

With respect to code changes on the sensor, the existing data raw flow data has been sufficient so far. I was worried about time alignment with the angular rate data from the platform, but the timing offset seems to be fairly consistent. I still plan to see if using the flow sensor internal rates can provide a better result which will involve adding those rates to the message (I will hijack the velocity messages when i do this).

There is a question about the effect of polling rate. The APM code is polling sensor at 50Hz and is getting fresh data each time. if the sensor is polled at a lower rate, eg 10Hz, will it return the sum of the optical flow since the last poll, or is it driven by an internal update rate? I can dig in the code and perform a quick test to answer that.

[Gary McCray]

Hi All,

Actually I think some of what Paul is doing right now is using the actual optical flow for altitude determination or at least variance.

What might be interesting would be 2 optical flow cameras set up as a stereo pair (interocular distance adjustable).

The on board processors could probably do quite a bit to set up for true stereo distance imaging.

And a secondary processor (say Odroid U3 could do real point cloud, obstacle avoidance or SLAM (or all 3).

I suppose I'm getting a bit ahead of things, but not by much I'll bet.

Best,

Gary

[Randy Mackay]

 

     I also support this crazy idea.

 

     For the SLAM stuff it would probably make sense to try and use the Google Tango.  Of course I know extremely little about it and how to interface with it.

          https://www.ifixit.com/Teardown/Project+Tango+Teardown/23835

 

     Here in Japan there is talk (and some action) on trying to use multicopters for looking under bridges since the place started falling apart a few years ago.

          http://edition.cnn.com/2012/12/03/world/asia/japan-tunnel-collapse-bolts/

     It’s tricky because of the gps-might-not-work and the object avoidance problems.  As Rob says, it’s very similar to the graffiti project he did.

     I think last year we couldn’t do it but this year we might.

 

     I think one interesting test once Paul has his code a bit more integrated would be to fly a multicopter from outdoors to indoors with a px4flow attached.  For example, maybe into a tunnel and see what happens (and send a log to Paul).

 

-Randy

 

From: drones-...@googlegroups.com [mailto:drones-...@googlegroups.com] On Behalf Of Daniel Nugent
Sent: July 23, 2014 5:16 AM
To: drones-...@googlegroups.com
Subject: [Bulk] Re: [drones-discuss] Re: Fusion of optical flow measurements into the Navigation EKF - Progress Report

 

Take it a step further and run a low resolution SLAM algorithm for mapping. Not familiar what kind of resolution and processing is required for SLAM. Would be cool for cheap obstacle avoidance. Anyone have any knowledge on SLAM?

 

Optical flow only works in 2 dimensions right now, correct? How does it behave on uneven terrain?

 

Daniel

On Tuesday, July 22, 2014 2:38:05 PM UTC-5, robert.lefebvre wrote:

I support this crazy idea.  That graffiti project I was working on a year ago really could have used this.  And the applications are endless.

 

Anything we can do in the direction of helping a UAV navigate indoors will be time well-spent. 

On 22 July 2014 15:19, Jesus Alvarez <wja...@gmail.com> wrote:

Paul,

 

Maybe this idea is crazy and nonsense but... what if the pxflow sensor orientation could be configurable?

Lets see (I see) the pxflow as a "suction pad". The idea is that Loiter (or whatever mode that uses the pxflow) could not only track terrain below but whatever surface it has in the orientation the user has defined.

 

So for example I am thinking in the inspection of an hydraulic dam. If I could install pxflow in the front of the copter and tell him the orientation pxflow is facing, then I could "stitch" the copter to the given position in the dam wall.

 

In the extreme case, we could have more than one pxflow facing different orientation....

 

 

Just thinking...

El martes, 22 de julio de 2014 12:36:50 UTC+2, Paul Riseborough escribió:

RANGE FINDER FUSION DISABLED - HEIGHT ABOVE GROUND (BLUE) ESTIMATED USING OPTICAL FLOW, RANGE FINDER IN RED, NAV PRESSURE ALT IN GREEN

GPS TURNED OFF AT 50 SECONDS - ONLY MAGNETOMETER AND OPTICAL FLOW FUSION

I was able to tweak the test-bench to use the PX4Flow quality measure along with some other tweaks and now the optical flow estimated height above ground converges to a good match with the laser when the plane approaches the landing point. The bottom graph in the first figure shows the raw (uncompensated for aircraft pitch and roll) laser range range in red, the EKF navigation height (height above pressure reference) in green and the EKF height above ground in blue. The laser was only used for comparison purposes in this example and was not fused as part of the solution. The initial focal length estimate has changed from previous results presented, hence the convergence on a different scale factor value.

You can see that as the plane descend towards the runway and levels out at 5m altitude before climbing, that the EKF is able to provide an estimate of height above ground that agrees closely with the raw laser reading until the plane climbs away, turns and the slant range measured by the laser increases.

Another test was to disable airspeed fusion and turn off GPS at the 50 second mark  (which would normally lead to a runaway divergence in velocity), leaving only the magnetometer and optical flow fusion enabled. The filter was able to dead reckon its position with a drift of 500m over 5 minutes, or 100 metres a minute. The second figure shows the position estimated over this time period. The drift direction and speed is very consistent, but its error source is not known.

I was able to log PX4Flow data on APMCopter this weekend and was hoping to run it through the test harness, but have run into  what appears to be a problem with aliasing on the down sampled 50Hz data which degrades the inertial solution on replay to the extent that I am unable to use the data. I will have a go at implementing a moving average to down sample the inertial data from the 400Hz to 50Hz before it is logged and see if that solves the problem.

Regards,

Paul

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Paul Riseborough]

It might be crazy, but I like the sound of it. I'll have a think about what type of auxiliary filter structure would be required to track  vertical surfaces sensors - eg what filter states, etc. If the surface is relatively flat relative to the speed of the vehicle, then the problem becomes easier.

[Paul Riseborough]

Rather than try to update the proof of concept test harness to import the APMCopter logs, I have changed tack and decided to invest the time getting the optical flow maths used to produce the results shared in this thread into the APM AP_NavEKF library. This takes advantage of the existing APM EKF replay facility which already works with the APM copter logs. This will also shorten the time required to get to closed loop flight testing once the Replay starts producing reliable results.

[Jesus Alvarez]

Nice to see crazy ideas going forward!!

I have a 550 quad and a lidar lite funded. If I add a pxflow I volunteer to test against walls.

[Paul Riseborough]

I was able to get the data logged on a Pixhawk + 3DR quad (running Randys px4flow2 logging branch) imported into the test harness and replayed through the EKF using only IMU, optical flow, baro alt and magnetometer measurements. IMU data is down sampled at sampled at 50 Hz for the log, so there are some additional aliasing errors that will not be present when it is run on the board.

The attached figures show the estimated velocity and position in blue compared to the GPS. The flight had a range of speeds and tilt angles up to 15 m/s and 40 degrees .

After some initial drift, the position offset when the filter had learned the bias errors and there was another period of higher drift when it was doing the 40 degree tilt speed runs.

This data was gathered using a 4.6 mm focal length lens which is unnecessarily low resolution for a copter application, so I will try the 16 mm lens next with the new PX4FLow firmware that allows a faster flow velocity to be measured.

The sonar was unreliable over the grass, so I have not used it's data at all.

[Tom Coyle]

Hi Paul,

I was unaware that there is a version of the EKF firmware that will run an APM2.5/2.6.

Can you please direct me to where I can download it as the present version of the APM ArduRover2 firmware does not include an EKF enable parameter?

Regards,

Tom C ArduRover2 Developer

On Thursday, July 17, 2014 11:25:03 PM UTC-4, Paul Riseborough wrote:

[Paul Riseborough]

Tom,

EKF will never be able to run on the APM 2.5/2.6 hardware.

When I used the term APM I am referring to the APM codebase (aka ArduPilot) to differentiate from my Inertial nav testbench https://github.com/priseborough/InertialNav

Apologies for the confusion.

Regards,

Paul

[Paul Riseborough]

I am trying to get the PX4Flow data from Randy's px4flow2 branch into the AP_NavEKF library, but am running into my lack of programming knowledge relating to the rules for different classes. If someone can provide me with some advice as to how I should reference the optical flow data inside the AP_NAvEL=KF library, it would be much appreciated.

TXS,

Paul

[Randy Mackay]

Paul,

 

     I can help you with this.  Ping me on skype when you’re free.

 

     Similar to how the NavEKF uses the baro, when the NavEKF object is first instantiated in ArduCopter.pde, you’ll want to pass in the opticalflow object into the NavEKF’s constructor.  The NavEKF will need an internal reference to an OpticalFlow object similar to the “_baro” that it already has.

 

-Randy

--

[Tom Coyle]

Hi Paul,

Okay. Understood:-)

Regards,

Tom C ArduRover2 Developer

[pierange...@gmail.com]

Hi Paul,

I'm new of this world and you will excuse me if I will do obvious questions also.

We have multicopter with Pixhawk and we are using qgroundcontrol Px4 Autopilot.

My first question is,

number of sensors that can supported and can be installed on Pixhawk.

Second

We would want to use Lidar Lite also but in this moment not is available, then we bought PX4FLOW KIT and some MB sensor, for our project we should use 6 minimum, is this possible?

If yes, can you give me some recommendations?

Thanks in advance

Pierangelo

[Paul Riseborough]

Pierangelo,

At the moment I'm dealing with integrating data from a single flow sensor and range finder into the the ardupilot AP_NavEKF library, so you're ahead of me with this question.

You could use a combination of I2C and serial interfaces so the maximum number would depend on how many of the serial ports were free, how many other devices you have connected to the I2C port, and how rapidly you wanted to poll the sensors. According to the PX4Flow spec sheet, the serial interfaces can be daisy chained for a second sensor.

Have you tried contacting the PX4Flow development team about the use of multiple flow sensors - Given the device is set up to enable it to daisy chain, they may have done experiments in this area.

Regards,

Paul

[Paul Riseborough]

Some interesting observations from further data gathering flights on the 3DR quad with a 10Hz polling rate and both the 4.6mm and 16 mm lens used. the PX4Flow sensors firmware was upgraded to the latest version from master which now uses a 400fps  camera rate enabling a faster flow rate to be tracked. It was anticipated that this would make the 16m lens the preferred option for copter testing.

The flight profile included periods of loiter at heights between 1 and 20 metres and a grid search in auto at 10 m/s. It was windy with large tilt angles on the upwind segments.

1) A10Hz polling rate was useable provided the vehicle body rate used for motion compensation was an average of the body rates across the measurement interval, not a snapshot taken in the middle.
2) The 16 mm lens outperformed the 4.6mm lens in this application as expected
3) There was less drift closer to the ground as expected
4) Rapid tilt transients and higher speeds degrade accuracy as expected
5) Without a range finder, it was not feasible to do focal length scale factor estimation with the 10Hz sample rate. The averaging associated with the 10Hz sampling takes out a lot of the angular rate information that the estimation relies on to estimate scale factor when the range to ground is uncertain.
6) The optical flow rate innovation sequences are close to zero mean and mostly within the 1-sigma innovation variance bound, so the filter tuning is about right.
7) The sonar attached to the px4flow unit is unreliable over grass so it's data has been ignored for this analysis.

The attached figures show the position and velocity estimates and the optical flow rate innovations for a replay through the test bench in  https://github.com/priseborough/InertialNav/tree/optflow-wip with useGPS set to false.

With the position and velocity plots, blue is the dead reckoned estimate from the EKF, red is GPS. For the innovation lots, blue are the measurement innovations and red represents the +- sqrt of the  innovation variance from the EKF.

I'm waiting now for LIDAR-Lite units to arrive so I can start testing the optical flow and range finding as an integrated capability. Getting the px4flow data into AP_NavEKF has also turned out to be a bit more complicated that first anticipated, but Randy is now taking a look at my branch.

[Jaime Machuca]

This is awesome Paul, if you need help with testing or gathering more data for replay, let me know I have a PX4flow around and could set up a rig to use it to capture data on my flights.

Jaime 

Sent from my iPhone

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

<PositionWithNoGPS.png>

<FlowInnovations.png>

<VelocityWithNoGPS.png>

[Paul Riseborough]

A quick summary of where the work is at

Timing Analysis

Preliminary timing analysis revealed hat the computational load associated with addition of the extra state (going from 22 to 23) in addition to the fusion of the optical flow observations were going to push the worst case timing to 1.5 msec. My target for the development of the EKF has been to limit the worst case load to about 50% and to keep the average load to less than 20%. I also had concerns about integration risk associated with the changes required to add the additional state and the potential effect on stability of the main filter.

The solution was to pull the terrain position state out of the main filter and locate it and the focal length scale factor in a separate 2-state EKF that uses optical flow measurements and range finder measurements if available to estimate focal length scale factor and terrain position. These two states are then used by the main 22 state filter when it fuses the optical flow observations. This has been implemented and tested in the prototyping test-bench here:

https://github.com/priseborough/InertialNav/tree/optflow22-wip

The advantages to this approach are reduced integration risk for APM, and reduced computational load. Mathematically this is a suboptimal approach as it discards information on correlation of these states to states in the main filter, however results  on the test-bench showed the performance to be equivalent to the 23-state filter.

ArduPilot Implementation

A preliminary design for implementation in ArduPilot of both a 23 state and 22 state optical flow implementations have has been written and had limited flight testing (mostly ride-along, but some in the loop testing with GPS enabled). This is the current 22-state design

https://github.com/priseborough/ardupilot/tree/optflow-wip

Further work is required to finalise a 'pull' interface with the px4flow sensor that provides non-motion compensated optical flow rates and gyro rates averaged across the period from the last measurement request.

Px4Flow Modifications

I have been experimenting with different averaging strategies in the px4flow unit. This involves averaging the flow measurements internally within the px4flow sensor before they are transmitted. Preliminary test results indicate that a 10H update rate is sufficient. The px4flow developers are working on a better method of averaging readings that uses the time between data requests over the I2C interface.

https://github.com/PX4/Flow/issues/21

This will enable the autopilot to control the rate at which it receives flow measurements, potentially making the update rate adapt to conditions (faster rate during maneouvring or when operating without GPS)

The other modification is providing the flow sensor internal gyro rates averaged using the same the measurements times as the flow readings. This will allow for tighter motion compensation, and these gyro measurements can be corrected for bias errors within the EKF before they are used for the motion compensation.

https://github.com/PX4/Flow/issues/21

I have also done some experimentation with the klt-flow branch,

https://github.com/PX4/Flow/tree/klt_flow

however in the process a sign error was introduced, resulted in a filter divergence, fly-away and minor crash which unfortunately damaged the lens assembly on my flow sensor. Thankfully Tridge has been able to lend me one until the replacement arrives. I need to check the sign on the outputs on the klt-flow branch before flying it again.

Flight Test Results

The attached figures show the results from a very windy day over grass last Saturday morning. Blue is the nav solution (using baro, optical flow and magnetometer only), red is GPS. The strong wind was causing a visible 'wave' or ripple effect on the grass, which may have been responsible for the  velocity drift of around 0.5 m/s as it is in the direction of wind drift. This is not an unexpected result, and is a limitation of optical flow technology. There was also a larger buildup of error when  high speed flight at high tilt angles was performed, but this has improved from previous results now that data is being averaged internally within the px4flow sensor. However the average rate of drift over the flight was less than 0.2 m/sec which is a good result considering the range of speed and height covered.

TO DO

In no particular order:

• Update interface with flow sensor and flight test
• Integrate range finder measurements into the ArduPilot implementation
• Update the ArduPilot Tools/Replay facility to read in flow and range finder logs
• Clean up the optflow22-wip branch in my InertialNav RePo and get it back into master
• Look at failure detection and isolation strategies for flow and range finder measurements

Still plenty to do!

Regards,

Paul

[Robert Lefebvre]

Great stuff Paul, thanks for the update.

Wind blowing the grass around... wow, I never thought of that problem before.  Great example of how every sensor technology has it's weaknesses, and effective fusion of data is required to make a robust system.

[john...@gmail.com]

An possible even harder surface to track would be water. Both open sea with waves and still water reflecting the sky and surroundings etc.

[Paul Riseborough]

I'm not fusing in any range finder data in these results, which means the  filter was having to estimate height above ground and range to centre of the image using the baro. I know this vehicle experiences a  few metres of baro height offset in flight due to aerodynamic effects and sensor drift. The velocity errors during manoeuvres are expected to reduce when I get a working range finder integrated. Sonar is useless given the range of heights, speeds and surfaces.

There is a small lake that i could fly over to test the effect of wave movement - but not until I modify it to have positive buoyancy. There have been a few stories of quads 'lost at sea'.

[David Pawlak]

It's good to have a good sense of humor in all of this. But I KNOW you are having a blast"

Thanks,

[Paul Riseborough]

This weekend, I modified the px4flow  code to output the following quantities:

• gyro X and Y rates in mrad/sec averaged over the time from the last output
• optical flow rates expressed in rad/sec averaged over the time from the last output. Effectively velocity/range or the velocity that would be output at 1m range
• quality averaged over the time from the last output.

The output rate was reduced to 10Hz, and only measurement points corresponding to valid flow measurements (quality >0) contributed to the averages. The modifications to the px4flow code can be found here:

https://github.com/priseborough/px4flow/commit/8d4e185d2c1f5e401199a754b8f54f06206c7dac

The attached plots show the position velocity estimates and optical flow innovations from a flight with 11 minutes of hovering with a repositioning and height change in the middle. The GPS fusion was turned off at 100 seconds into the flight, so the last 600 seconds are without GPS. For the position and velocity plots, estimates are in blue, GPS is in red. The innovations plot has the innovations in blue and the red envelope is the filters 1-sigma estimate.

The flow sensors internal gyro measurements are much better time synchronised with the flow measurements than the measurements from the pixhawks own gyros, so the innovations are significantly lower and the drift performance has improved sigificantly. The gyro measurements from the flow sensor are then compensated for bias errors within the EKF. The results are still suffering from lack of range finder data, however overall these are good results. Between 100 and 400 seconds when it was being flown in stabilise at about 1 to 2  metres height, the position drift in the solution was less than a metre. When it was repositioned and the altitude was increased to 10metres, the position drifted about 20 metres over the 300 seconds.

The modifications to the inertial nav code used to generate these results can be found here:

https://github.com/priseborough/InertialNav/commit/7f8b0109c6abffd96afdc7e4fd1444ef6b9a85e0

[Paul Riseborough]

Here are some results from a replay of another flight from this weekend flown using the modified px4flow code. In the replay of this data, the GPS was turned off 10 seconds after start. The problem with running with no GPS from before takeoff is that on the ground and up to about 50cm, the image is out of focus, so the flow measurement quality is poor and the flow fusion errors are much more sensitive to range/height estimation errors. Given a working range finder, we should be able to get around this by leaving the EKF in static mode (fusing zero NE velocity observations) until the range finder has detected takeoff and the flow quality has passed a threshold.

As before, with the position and velocity plots, blue is the filters estimate obtained using the px4flow data and red is the GPS reference. The innovation plot shows the Line of Sight optical flow rate innovations in blue, with the red envelope being the filters estimated 1-sigma uncertainty (+-square root of the innovation variance).

This flight had less baro drift, so the range estimate used by the optical flow fusion was better. It also included some repositioning and a circuit flown at 6 m/s. At 325 seconds onwards and at an altitude of 15m, the copter was put through a series of rapid high amplitude and pitch oscillations which create an increase in in the optical flow measurement innovations. Importantly however, the drift in position is gradual during these large amplitude manoeuvres is gradual.

At this stage of testing it looks likely that a low drift optical flow position hold will be achievable for heights between 1 to 15m using the standard 16mm lens, with performance degrading progressively as height increases, and that a velocity control mode will also be achievable.

[Robert Lefebvre]

Really great stuff Paul.

The fact that the optical flow sensor goes out of focus below 1m will obviously hurt our ability to leverage the optical flow for really stable landings. Would it make sense to consider having a second sensor with the lens focus set closer?

--

[David Pawlak]

Or what kind of an effect would autofocus have?

That's usually an in-built camera things, but if it could work (see below), it might be cheaper than a second PX4Flow.

An focus adjustable lense which we adjust with a servo output based on laser or sonar height.

[Paul Riseborough]

I've been thinking about this. Provided the sink rate is 0.5m, then the maximum time it has to go free inertial is going to be 1 second, which is acceptable. There is also the control strategy which from loss of optical flow data to touchdown could be to hold the pitch and roll orientation averaged over the previous 1 second. On copters with taller undercarriages (the lens on the 3DR quad is touching the grass), this period will be less.

[Gary McCray]

I actually think managing to stay acceptably in focus to 20" is a great accomplishment.

Normal position holding or intentional navigation would normally be well above that height in any case.

Possibly we ought to consider 1 meter the minimum height for safe optical flow maneuvering.

Generally the only time you would go below that is when you were intentionally landing.

And inertial ought to be able to hold it during a landing without any trouble.

It might be nice to have something happen when you are below 1 meter (or starting to lose or not in focus), possibly a recognizable alarm or LED sequence.

Best Regards,

Gary

[Robert Lefebvre]

My thinking is that having zero lateral motion can be more important as you touch down, than any other phase.  For copters anyway. A GPS glitch causing even 0.1m/s during touch down can cause problems.  So it makes a lot of sense to me to try to get the benefits of optical flow at this time.

[David Pawlak]

If one were to be brave enough to run and test this configuration....

Could we have a quick validation of the steps required.

If I understand it you have a base flying test environment here:

https://github.com/priseborough/ardupilot/tree/optflow-wip

To that, I think the latest changes are edited from here:

https://github.com/priseborough/InertialNav/commit/7f8b0109c6abffd96afdc7e4fd1444ef6b9a85e0

I notice this seems to use a 23 state estimator Wasn't the optflow-wip based on the 22 state + 2 state EKFs?

Then there's also changes to the PX4Flow itself:  

https://github.com/priseborough/px4flow/commit/8d4e185d2c1f5e401199a754b8f54f06206c7dac

From what I understand, there may be more upcoming changes to this code in the future.

Had you made any other changes in the PX4Code that I haven't mentioned.

[Paul Riseborough]

The InertialNav commit you reference is from https://github.com/priseborough/InertialNav/tree/optflow22-wip which uses the 22+2 state structure, however the file names have not been changed yet so ignore the 23 in the file name.

I will check the other repo's you reference and see if I can create a tag at a configuration that has flown.

So far the ardupilot implementation is the only one that has flown real time, and that has only been with optical flow data augmenting GPS. All the GPS denied results have been obtained on replay using the InertialNav implementation. I am expecting a laser range finder any time soon, so I am waiting until I get that integrated into the APM implementation before turning off GPS in flight.

I also need to get the ardupilot Tools/Replay working with flow and range finder logs, but i will need Tridges help with that.

Regards,

Paul

[David Pawlak]

OK Paul, thanks

I know it's still quite early WIP, but I thought it was a good time to start getting myself synched in. This is an app that I really need, so the sooner I can come up to speed, the better.

[Paul Riseborough]

I received a Lidar-Lite today and found that i cannot run both the Lidar and PX4Flow sensor together on the I2C bus. Connecting the PX4Flow sensor to the I2C bus causes the Lidar readings to become intermittent. More investigation required.

[Craig Elder]

Paul, both the Lidar and the PX4Flow share the same I2C address of 0x42h.  The Lidar cannot be changed but the flow can have the address re-programmed.

On Mon, Oct 13, 2014 at 3:44 AM, Paul Riseborough <p_rise...@live.com.au> wrote:

I received a Lidar-Lite today and found that i cannot run both the Lidar and PX4Flow sensor together on the I2C bus. Connecting the PX4Flow sensor to the I2C bus causes the Lidar readings to become intermittent. More investigation required.

[Paul Riseborough]

I offset the address by one by bridging the 0 bit solder pad on the back of the px4flow board (the three pads control the address offset). The interference has gone. I could have changed the base address in code (its defined by I2C1_OWNADDRESS_1_BASE in the i2c.h  file).

[Craig Elder]

Dang. I think we need to see if Dennis & Co. can change the address on the lidar.

[Randy Mackay]

     I’ve pushed several commits to master just now to enable the px4flow sensor for Pixhawk.  I’ve also made some changes to OF_Loiter but I haven’t test-flown it and I don’t think it’ll successfully hold position yet.

 

     Because of the I2C address issue, for the moment, I’ve left out this one commit that starts the px4flow driver.  So if people want to test the sensor they’ll need to cherry pick this into their ardupilot branch.

              https://github.com/rmackay9/rmackay9-ardupilot/commit/8e1bc5a87380be2e690ca52055013c9746f1f223

 

-Randy

[Paul Riseborough]

I've still got the bug with my setup that i need to power up with USB for the FMU to initialise the flow sensor. If i power up first via the power module, I don't get any flow. I will see if delaying the initialisation works - maybe the FMU is trying to initialise before the flow sensor is ready.

[David Pawlak]

Randy,

 

Can you elucidate a bit on the differences between what you have done and what Paul is doing?

Do I remember right that Paul used your initial drivers/interfaces to get his project strated?

How far does yours go? Paul's goes deeper into the EKF fusion... or did you two collaborate to get a single starting point into master?

[Randy Mackay]

David,

 

      So basically I’m helping Paul by getting a driver working in ardupilot so that he can get the data from the sensor.

 

     In addition I did a small amount of work to fix up OF_Loiter which is a weak version of what Paul’s doing.  It relies just on the sensor, not on any kind of smart mixing of data.  The only reason OF_Loiter will survive is if Paul’s stuff doesn’t allow using the optflow sensor without a GPS.

 

-Randy

 

From: drones-...@googlegroups.com [mailto:drones-...@googlegroups.com] On Behalf Of David Pawlak
Sent: 15-Oct-14 8:44 PM
To: drones-...@googlegroups.com
Subject: [Bulk] [drones-discuss] Re: Fusion of optical flow measurements into the Navigation EKF - Progress Report

 

Randy,

 

Can you elucidate a bit on the differences between what you have done and what Paul is doing?

 

Do I remember right that Paul used your initial drivers/interfaces to get his project strated?

 

How far does yours go? Paul's goes deeper into the EKF fusion... or did you two collaborate to get a single starting point into master?

 

On Thursday, July 17, 2014 11:25:03 PM UTC-4, Paul Riseborough wrote

Some of you would be aware that since supporting the implementation of the EKF in APM and PX4, I have been working on incorporating fusion of optical flow measurements into the EKF using a data set supplied by Lorenz.

After a number of false starts, I now have a prototype working in a proof of concept test bench https://github.com/priseborough/InertialNav/blob/optflow-wip/code/estimator_23states.cpp which uses the raw flow messages and can do the necessary calibration of the sensor focal length automatically. This involved addition of a state to the navigation EKF to estimate the local terrain elevation, update of all the filter equations to accommodate the additional state, implementation of the equations for the fusion of the optical flow rates and implementation of a separate EKF to estimate the sensor focal length scale factor. Some earlier attempts were made to try and integrate estimation of the focal length into the main filter, but this ran into problems with filter stability.

The good news is that this implementation is compatible data available on the existing MAVLINK optical_flow message and has also shown it is possible to estimate height above ground without use of a range sensor (if you have a independent measure of velocity like GPS), or can it can constrain drift when GPS is not available if you have a means of estimating height above ground, eg laser).

The first plot shows the convergence of the focal length scale factor over a flight, optical flow rate innovations and he laser range (in red) superimposed over the optical flow derived height estimate (in blue). The second plot shows the attitude estimate from the filter compared with the PX4 on-board solution.

These are preliminary results and there has been no filter tuning or data quality checking, but it shows the optical flow can provide an estimate of height above ground when laser readings are unavailable and that the optical flow fusion is working correctly. The combination of a laser and optical flow sensor, enables height above ground to be estimated across a larger range of heights.

The next step is to gather some more data sets (i will try to get one recorded from a copter) and do some more work on tuning and data quality checks. The prototype is currently not using the flow quality measure, so that will be one item to look at. Once the improvements will be rolled back into the optflow-wip branch and the testbench is producing reliable results, I can then start an APM implementation

Thank you to Lorenz and the PX4 team for the flight data, 3DR for providing a sensor to experiment with, and Randy for his patch integrating the PX4Flow sensor, which i will try to gather data with this weekend.

Regards,

Paul

--

[David Pawlak]

So if I were to get this up and running now, it would be a good start to an eventual finished product. From here, I imagine, the idea will be to pull in Paul's work, bit by bit.

I suspect that in the end, it will all change by quite a bit, to incorporate Paul's changes in PX4Flow firmware and phasing out the actual OP_Loiter. But starting with these commits, 
at least we'd have communication with the PXFlow, and begin to get the feel of it and associated firmware.

Does Arducopter log any PX4Flow data, from when it was working with APM? Is there code around for that?

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discuss+unsubscribe@googlegroups.com.

[Paul Riseborough]

There are a few items to work through:

- Gather some flight data with the Lidar-Lite and Px4Flow unit. (does AC3.3 log sonar?)
- Update the replay so that flow and sonar data can be used by the filter (I need help with this)
- Tweak the filter design using the replay and do some flight demonstrations without GPS.
- Coordinate required px4flow interface changes with PX4 and ArduPilot developments. This conversation has already been started with the PX4Flow developers, but haven't yet run the proposed changes past the ardupilot leads.
- Capture requirements (eg do we have a non-GPS mode of operation locked from arming or it need to be selectable in flight, what should we do if the flow sensor or laser stops working, etc)
- Update the design to address the requirements captured
- More testing and bug fixing in a development branch
- Into master
- more flight testing and bug-fixing
- etc

[Craig Elder]

>>> (does AC3.3 log sonar?)

yes and so does 3.2

Lidar replaces the Sonar Altitude in the CTUN message

http://copter.ardupilot.com/wiki/downloading-and-analyzing-data-logs-in-mission-planner/#Message_Details_APMCopter_specific

[Paul Riseborough]

SUCCESS!

The EKF has just flown closed loop in position hold without using GPS for 4 minutes with no user corrections required. After a quick hop in stabilise to check it out, I landed, switched across to EKF, took off and switched to position hold. It was in a backyard with high gusty winds and the position hold was solid. See attached plot of demanded vs estimated position. The EKF position diverges after landing because the optical flow sensor was down in the grass and the flow quality dropped below the acceptance threshold. During the 4 minutes of  position hold, I didn't have to correct the position input, but did perform a small repositioning manoeuvre. The altitude was kept between 1 and 2 metres for most of the time.

The laser range finder wasn't correcting the height output from the filter in this flight, so that will be the next step after putting in logic to stop the  position from wandering away when on the ground.

-Paul

[Jesus Alvarez]

That's a big step forward.
Well done!

[David Pawlak]

Thanks Paul, it's getting exciting!

Can't wait, I really need this function!!

On Thursday, July 17, 2014 11:25:03 PM UTC-4, Paul Riseborough wrote:

[Paul Riseborough]

Did another test flight today over a larger range of speed and height (up to 4m/s and 10m). The good news was that the drift on the EKF estimated position using optical flow was negligible, however there were a few issues:

• The range finder code in APM currently limits the max range to 4m. I remembered Tridge had warned me about that when I looked at the log, but it was a good test of the filters ability to handle range dropouts.
• The flow sensor scale factor estimate is too slow to converge.I will have another look at the derivation because it should be able to use angular motion to update the estimate.
  
• The position hold loops are very sensitive to noise in the EKF position and/or velocity output. This sometimes results in a limit cycle can build where:

1. Angular motion of the vehicle couples into the optical flow rates due to the scale factor error
2. The perturbation in optical flow rates cause perturbation in the EKF velocity and position outputs
3. The perturbation in EKF outputs causes the control loops to demand a change in angle so back to 1. and the amplitude can build
   

• The I2C bus dropped out 6 minutes into the flight with the external mag, optical flow and range finder measurements stopping. This caused the EKF solution (which was hard coded to not use GPS and rely on optical flow) to diverged. When I re-powered the vehicle, the I2C bus was working again.

I will see if I can get it to replicate the I2C failure on the ground and then replace the cable from the breakout board to the Pixhawk, which has had the connector inserts removed and re-inserted to twist the wires.

-Paul

[Andrew Tridgell]

Hi Paul,

> Did another test flight today over a larger range of speed and height (up
> to 4m/s and 10m). The good news was that the drift on the EKF estimated
> position using optical flow was negligible

great!

> - The range finder code in APM currently limits the max range to 4m. I

> remembered Tridge had warned me about that when I looked at the log, but it
> was a good test of the filters ability to handle range dropouts.

that should be fixed in master, but you do need to set the RNGFND_MAX_CM
parameter to the right value. It then tells the PX4 driver what the
maximum range is. You can test it on the ground by watching
RANGEFINDER.distance while pointing across a large room.

> - The I2C bus dropped out 6 minutes into the flight with the external

> mag, optical flow and range finder measurements stopping. This caused the
> EKF solution (which was hard coded to not use GPS and rely on optical flow)
> to diverged. When I re-powered the vehicle, the I2C bus was working again.

darn, I wonder if we have another I2C software bug?

> I will see if I can get it to replicate the I2C failure on the ground and
> then replace the cable from the breakout board to the Pixhawk, which has
> had the connector inserts removed and re-inserted to twist the wires.

ok, I hope it is a hw fault ....

Cheers, Tridge

[Paul Riseborough]

Tridge,

The I2C dropout happened again on the bench, this time with a different cable from the breakout board to the Pixhawk. This  Pixhawk has never missed a beat before I added the additional sensor (LIDAR-Lite) to the I2C bus. The Pixhawk is an engineering dev unit (labelled E4), but my understanding was that a different IMU was the only difference. I will swap Pixhawks to a production unit and repeat the test.

The maximum range was my mistake - I had changed the RNGFND2_MAX_CM parameter by mistake (I really do need to get a laptop with a daylight readable screen)

Regards,

Paul

[Paul Riseborough]

Changed to a production board and same result - after 8 minutes of running pre-armed on the bench, data from all three I2C sensors stops. I will now try removing each sensor (LIDAR-Lite, PX4Flow and external Magnetometer) in turn and see if it is a result of all 3 sesnors or one in particular.

[Dennis Corey]

Hi Paul,

I don't know if the drivers have been changed in the Pixhawk code, but the LIDAR-Lite will return a "nack" on the I2C line if it is busy taking an acquisition or processing the data.  We made this change in our code because repeated read or write requests to our processor were creating interrupts in the processing and eventually causing stack overflows in the data registers with unpredictable results.  So we simply return a "nack" until we're ready to talk.  While the acquisition and signal processing happens in just a few milliseconds, the timing of the data acquisition and processing can vary based on distance or signal strength as the signal correlation record is being built.  In past versions of the Pixhawk driver I know that everything related to LIDAR-Lite was very much timing based, which might be problematic if the sensor is repeatedly returning "nacks" to read or write requests on the bus.  Since I don't know how the Pixhawk code handles repeated "nacks" on the I2C bus.  Could this be an issue resulting from a timeout condition?

Hope this helps...

Cheers,

Dennis

[Paul Riseborough]

A quick update. The optical flow position hold without GPS has now had three flights although PX4Flow lockups have been an issue. When this happens, all communication on the I2C bus is lost, so it loses external compass, range and flow measurements. This only happens if the external Mag, LIDAR-Lite and PX4Flow sensor are all on the same bus, so I have temporarily disconnected the external compass until a longer term fix is found.

Attached is a link to a video taken during position hold without use of GPS. At the end of the video the copter loses altitude and flies out of view. This was caused by the flow sensor freezing,  and because GPS is not being used, the inertial nav position and velocity estimates started to diverge. Since then the external compass has been removed, and the code modified to switch to GPS velocities if the PX4Flow sensor freezes. These are temporary fixes to allow the testing to continue.

https://drive.google.com/file/d/0B6YbG6fDf0m4Rm0tTzFHMnVFcEk/view?usp=sharing

-Paul

[Randy Mackay]

Paul,

 

     That is absolutely fantastic!!

 

-Randy

 

From: drones-...@googlegroups.com [mailto:drones-...@googlegroups.com] On Behalf Of Paul Riseborough
Sent: 26-Oct-14 10:58 AM
To: drones-...@googlegroups.com
Subject: [Bulk] [drones-discuss] Re: Fusion of optical flow measurements into the Navigation EKF - Progress Report

 

A quick update. The optical flow position hold without GPS has now had three flights although PX4Flow lockups have been an issue. When this happens, all communication on the I2C bus is lost, so it loses external compass, range and flow measurements. This only happens if the external Mag, LIDAR-Lite and PX4Flow sensor are all on the same bus, so I have temporarily disconnected the external compass until a longer term fix is found.

--

[PL Dennis]

Impressive!

Dennis

--
You received this message because you are subscribed to a topic in the Google Groups "drones-discuss" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/drones-discuss/SqgXeojbiZU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to drones-discus...@googlegroups.com.

[David Pawlak]

Great going Paul!!

I2C is SUCH a pain. I'd love to see the end of it.

[Paul Riseborough]

The updated PX4Flow firmware https://github.com/PX4/Flow, and interface https://github.com/PX4/Firmware/tree/px4flow_integral_i2c, has now been integrated into my ekfOptFlow-wip branch

https://github.com/priseborough/ardupilot/tree/ekfOptFlow-wip

It has passed bench and captive carry testing, but I'm waiting for a replacement prop and the weather to clear before flight testing can resume.

The new interface provides the integrated optical flow and inertial rate between the current and last measurement request. This means no data is lost or double counted, and the data rate required is lower (the EKF is polling the sensor at 10Hz) which means less traffic on the I2C bus.

The new PX4Flow firmware also contains a fix for a bug that could explain the sensor lockups that were occurring. So far there has been no recurrence of the lockup which is good news.

Thank you to Lorenz, Dominic and Jon for advice during the integration.

[Paul Riseborough]

APM:Plane had its first flight today with optical flow running in the EKF. APM:Copter has already flown using optical flow fusion, However this was the first flight of the new PX4Flow interface that outputs integrated flow and inertial rates. The log can be found here:

https://drive.google.com/file/d/0B6YbG6fDf0m4V0FvMXpBdzVkX1E/view?usp=sharing

The flight was done using a Skyhunter airframe fitted with a Pixhawk, dual UBlox LEAH-6H GPS, digital airspeed sensor and PX4Flow sensor. No range finder was fitted for this test (It is currently fitted to a copter airframe) The software configuration was:

PX4Firmware : https://github.com/PX4/Firmware/tree/px4flow_integral_i2c, commit 739c407cfd14d065b104977924875b3ee40d5e25. This  implements the new px4flow interface that outputs integrated flow and inertial rates

Pixhawk FMU : https://github.com/priseborough/ardupilot/tree/ekfOptFlow-wip, commit 7bbee961646350701378077c65d8a6a46836e23d.This adds PX4Flow and range finder fusion and logging using the new flow sensor interface for both Plane and Copter.

PX4Flow sensor : https://github.com/PX4/Flow/tree/master, commit 02e3393fe0f100529ab7e10a4eb04a29c4ff3634. This outputs optical flow and inertial rates integrated over the time lapsed since the sensor was last polled.

The first two plots show the optical flow innovations from the main 22-state filter (the main navigation filter) and also the 2-state filter that is used to estimate terrain height, and if fitted with a range finder, also optical flow scale factor. The units are mrad/sec. The innovations for both filters settled to zero mean, and the innovation noise was less the 0.3 rad/sec RMS flow sensor noise parameter setting.

The third plot shows the terrain height estimated by the 2-state filter. The terrain height estimate was stable once the plane was airborne, but without the laser range finder fitted, there is no way to tell how accurate it was and it is is also estimating too slowly, so further tuning is required. However the estimate looks reasonable, the innovations were not biased and the plane was flying at 100m altitude, so this is a good first result. With the laser fitted the two sensor should be complementary with the laser working below 20m, and the optical flow working above that to give a terrain height estimate. Also when flying at low altitude with the laser working, the filter should be able to estimate and calibrate the optical flow scale error, which will give better height accuracy.

The next test will be to fit the range finder and do some low flying to see how the scale error estimation and calibration works in practice.

-Paul

[PL Dennis]

Hi Paul,

All around this sounds like great news.  I’m especially pleased to hear that you appear to have resolved the I2C lockup problems.  Using the laser module you should be able to have altimetry up to 40m.  

Looking forward to hear more!

Dennis
PulsedLight, Inc

--
You received this message because you are subscribed to a topic in the Google Groups "drones-discuss" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/drones-discuss/SqgXeojbiZU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

<flow innovations - 22 state filter.png><flow innovations - 2 state filter.png><estimated ground position -  2 state filter.png>

[Paul Riseborough]

Whilst attempting to improve the calibration of the flow sensors optical flow scale factor correction, I realised that most of the error was in the integrated body rates that are output by the sensor and are used by the EKF to do the angular motion compensation  There appears to be a scale factor error of circa -15% these body rates. See the attached plots showing a an overlay from the FMU gyros and the flow sensor body rates.

[Paul Riseborough]

APM:Plane was flight tested today using the LIDARLite in addition to the PX4Flow sensor. This flight incorporated a quick fix adding a Mavlink adjustable parameter FLOW_GSCALER to enable the 15% scale factor error in the PX4Flows gyro output to be calibrated.

The LIDARLite was initially flown with the RNGFND_MAX_CM set to 2000 (20m), because ground testing had shown sporadic false readings above 20m when pointing into free space. During up and away flight at a 100m altitude, a number of false returns were experienced. The RNGFND_MAX_CM parameter was then reduced to 1000. The attached figures show the laser range (red), range innovations from the EKF (green) and baro alt (blue).

The EKF was able to reject the sporadic false readings, partly because the optical flow kept the terrain height observable during up and away flight. Given the EKF is only fusing optical flow and range data at 10Hz, I will have a go at implementing a mode filter (median of last 5 samples) as a first line of defense, rather than rely solely on the EKF innovation consistency check.

The FMU SW flown for this flight was:

https://github.com/priseborough/ardupilot/tree/ekfOptFlow-wip, commit dc8312e6d9e7a0da99994002b305f2fe70c32d87.

The FLOW_GSCALER parameter was set to +15

-Paul

[Paul Riseborough]

I forgot to mention in my last post that when the LIDAR-Lite was within 30m of the ground, no false readings were experienced. Electrical noise could be a contributor, so I will try a flight with the flow sensor disconnected.

[Andrew Tridgell]

Hi Paul,

> There appears to be a scale factor error of circa -15% these body
> rates. See the attached plots showing a an overlay from the FMU gyros
> and the flow sensor body rates.

What is GYRO_SENS_DPS set to in the flow sensor?

The gyro in px4flow changes scaling based on that parameter, with the
following constants:

https://github.com/PX4/Flow/blob/master/src/gyro.c#L56

the default is 250 DPS. We'd need to check the l3gd20 docs to see if the
scaling factor is right for whatever setting you have.

Cheers, Tridge

[Robert Lefebvre]

Paul, that is very interesting that you had false readings on the lidar.  Did you see my discussion and video of object avoidance using lidar last week? I also experienced false readings, using the SF/02.  Seemed that when the sensor was pointed at the horizon it would false return. Later on I tested again, pointing it up into the sky by hand, and had no problem. Strange. I did not expect this failure with lidar. Certainly complicates things.

--

You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.

[Paul Riseborough]

Rob,

Provided they are short duration events, a  simple median filter would reject them. It does mean however that the laser needs to be sampled as fast as possible to give the median filter sufficient samples to work on. False positives are a fact of life with range finders as dust, insects, etc can cause them to trigger, so in any case our downstream algorithms need to be robust to them .

Looking at page 19 in the LIDAR-Lite manual, issue 24 Oct, they calculate the noise level in the correlation record and multiply it by a fixed value of 1.25 to give the detection threshold. Ideally that multiplier would be a user adjustable parameter, as there are many applications where reducing false positives is more important than maximising range.

The sensor does provide access to a lot of its internal signal processing data including the correlation record, so in theory we could do our own processing, but it is a lot of data to transfer.

-Paul

[Robert Lefebvre]

In my case, any filter would be useless. The false positive appeared to be persistent. It flew backwards away from a nonexistent object until I stopped it manually.

I have not spent much time troubleshooting it yet. But its potentially a really bad shortfall of the lidar I did not expect.

[PL Dennis]

Hi Rob,

I can’t speak for the SF-02 or the specific conditions that you were flying in that might have had created the false readings.  I do know that the LIDAR-Lite sensors that Paul and Tridge have been testing are preproduction units and we have identified a bug that may have contributed to the false readings that Paul was seeing with the sensor.  I’ve suggested to Paul that once he’s had a chance to develop the filter using the data he’s getting from that unit, he send it back to us so that we update the code to correct the false reading problem.  LIDAR-Lite does have the ability to identify what it determines is an invalid reading and output a zero range along with setting a flag, bit 3 of Control Register 1 to indicate reading validity.  So, post update, there shouldn’t be “false” readings - there will be zero distance readings, but those should be easy to filter out.

Cheers,

Dennis

You received this message because you are subscribed to a topic in the Google Groups "drones-discuss" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/drones-discuss/SqgXeojbiZU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to drones-discus...@googlegroups.com.

[Paul Riseborough]

Dennis,

The attached plot shows the false returns when the sensor is pointed skyward in clear conditions (the sun had set, so no sun-glare). The false returns seem to sit in a band from 8 to  35 metres. Is this the type of error you would expect given the bug you found?

Paul

[Robert Lefebvre]

Hi Dennis, can you speak to the general physical performance of Lidar systems (not specific brand implementations).  Is there a physical effect that would cause these sorts of errors?

In my case, the problem was happening while I was flying right at dusk.  Sun was very low or setting.  Lidar was aimed away from the sun, and seemed to happen when it's elevation was just above the horizon.  This only happened at about 20-40m altitude, when it had just cleared any terrain/trees etc.  So, it was fine flying at low altitude, picking up the trees and houses.  But as soon as I took it up so it was looking at the horizon, off it would go.

After thinking about it for a few minutes, I realized it must be false returns from the sky, so I took it and aimed it up into the sky, nothing.  Seems the problem happens on the horizon, or when the sun is setting?

I'll look forward to comparing the performance of the Lidarlite when I get mine. 

Thanks,

Rob

[PL Dennis]

Hi Rob,

What you’re seeing is the effect of solar background radiation overwhelming the Avalanche Photodiode Detector (APD) in the sensor you’re using.  At dusk, when the sun is low in the sky, there is a tremendous amount of reflected light from particles in the atmosphere.  This light also tends to be in the red spectrum (hence red sunsets) and even if you’re looking away from the sun, the amount of IR energy is significant.   So that would explain why as the sensor looked above the horizon you started getting the false readings.  There’s just a ton of IR energy present and even with a notch filter over the detector there is still a significant amount of background radiation present that overwhelms the system.  Even with an IR notch filter in place this is a condition that is hard to overcome.  The optical system will play a part in this as well.  Large optics, designed to collect a lot of light under normal conditions, giving the sensor greater sensitivity (range) will also collect a lot of this background radiation.  

I hope this helps!

Cheers,

Dennis
PulsedLight, Inc

[Paul Riseborough]

I hit another roadblock during Copter testing with the new flow interface in the last 24 hours. I was running into problems with a lot of jitter in the EKF, and flow measurements that were noticeably noiser than the 100msec averaged values I was using before updating the flow firmware and driver. After investigation It turns out that although the flow sensor driver is being polled every 100msec, the data it provides is always averaged over the last 25msec of readings, which means that in effect it is only averaging across 25% of the data, explaining the higher noise levels.

I have looked at the PX4Flow code here :

https://github.com/PX4/Flow/blob/master/src/i2c.c

and cannot see anything that would stop the integration. Also in the APM code here:

https://github.com/priseborough/ardupilot/blob/ekfOptFlow-wip/ArduCopter/ArduCopter.pde#L1193

the flow sensor update is only called once within the 10Hz loop. A printout to the console from within the optical flow library here:

https://github.com/priseborough/ardupilot/blob/ekfOptFlow-wip/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp#L55

confirmed that this was being called only once per 100ms, although there were always two messages in the buffer.

-Paul

[Paul Riseborough]

Lorenz has explained the reason to me. I was working on the assumption that the px4flow driver polled the sensor when queried by the APM software. However the driver runs asynchronously and polls the sensor at a hard coded rate of 50Hz (It was actually only achieving 40Hz) in the driver here:

https://github.com/PX4/Firmware/blob/px4flow_integral_i2c/src/drivers/px4flow/px4flow.cpp#L82

With PX4FLOW_CONVERSION_INTERVAL set to 100000, the integration interval is now the 100msec required. I'll report back when I  have had a chance to flight test the change.

-Paul

[David Pawlak]

Here in the i2c driver

https://github.com/PX4/Flow/blob/master/src/i2c.c 

Each time update_TX_buffer is called, the flow tx buffer is loaded with flow data and an integration is performed (as long as qual is true)

and loaded into the integral tx buffer.

Once "stop_accumulation" is set, the integration is reset to 0.

So it would seem that update_TX_buffer must be called quite a bit more often than the 10 Hz to get a decent integration over the 100ms period, and it would depend on the stability of the update_TX_buffer call rate and "qual" value to be consistent.

If qual is bad the integration freezes.

Don't know anything about the flow code, but sometimes an ignorant pair of eyes can be useful.

[Paul Riseborough]

David,

The sensor is supposed to integrate data between polls from the FMU. The FMU firmware driver controls the rate at which the sensor is polled. Provided the APM software queries the driver faster than 10Hz, it should not miss any data. The APM code has been updated to check the driver for new data at 100 Hz.

I flew with the updates and the drift rate was only 30cm per minute, mostly along the Y body axis (perpendicular to the roll axis). Jitter was reduced but is still noticeable in the roll axis. There is a noticeable difference in flow noise between the X and Y outputs with the X axis being noisier. The camera image is not noisy and the flow quality was above 250, so I  don't have an explanation for it at the moment. Ground testing with two flow units against a featured wall showed both units had more flow noise for the X axis.

I think I now need to have a trawl through the flow sensor code and do some tests logging data at different points to see where the difference is occurring.

-Paul

[Paul Riseborough]

Here are the plots of the flow sensor optical and inertial rate outputs, where the sensor was pointed at a brick wall from 1.8 metres away and moved with a small amplitude harmonic rotation about the the X and Y axes. The sensor was rotated through 90 degrees between X and Y measurements, so that the section of wall 'swept' by the sensor would be the same each time. The difference in noise characteristics between the X and Y sensor axes is clear.

[David Pawlak]

If there is any noise on the inertial rate outputs, it seems to also be present equally in the low rates, but that could just be input noise (the sweep is "noisy").

I wonder if it would help to do a similar test without roll and pitch, that is a flat sweep isolating vector or angular compensation components.

Thinking that might let you test the filter eliminating potentially external "interference".

[Paul Riseborough]

David,

I should have explained the figures better. Red is optical rate and green is inertial rate averaged over the same interval. Scale factor differences are expected, it is the 'jaggedness' of the averaged readings for the X axis I am questioning.

A flat sweep is a good idea and something i can arrange.

-Paul

[Paul Riseborough]

A quick update on the testing. The jitter on motors due to flow noise propagation has been solved by applying a rate limiter and first order filter to the postion hold controller roll and pitch demands:

https://github.com/priseborough/ardupilot/commit/aaf1c20aab53e19a9f73b7dc44047439c98fe7e6

Here's a link to the flight video. One is captioned, the other is not captioned, but is image stabilised.

Captioned: https://www.youtube.com/watch?v=LP8kl4hGfMw&feature=youtu.be
Stablised: https://www.youtube.com/watch?v=napTp_XBmf4&feature=youtu.be

The full resolution video can be downloaded here:

https://drive.google.com/file/d/0B6YbG6fDf0m4QTdGOEdQTUwwMGc/view?usp=sharing

All the flight shown in these videos was in position hold mode with GPS disabled within the EKF. Minor repositioning  was performed between scenes on the video (the average position drift was about 30cm/minute). The conditions were very gusty and it required a lot of concentration to hold station in when in stabilise mode. The copter only had a bout a 2m radius circle to operate within, so this is not something you would attempt using GPS for position hold.

Paul

[Craig Elder]

That is really exciting Paul. It looks super cool. Congratulations.

--

[PL Dennis]

That’s really impressive Paul.  Good Work!

Dennis
PulsedLight, Inc

You received this message because you are subscribed to a topic in the Google Groups "drones-discuss" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/drones-discuss/SqgXeojbiZU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to drones-discus...@googlegroups.com.

[Paul Riseborough]

David,

I ran a test today, where I supported the quad about 20cm off a table, and focussed the lens onto a textured  towel placed underneath the camera that could be slid on the table. I slid the cloth in a series of doublets (trying to keep a constant speed) in both the X and Y direction. the results were very interesting and show a fault with the X axis optical flow processing. The Y axis output was as expected, seen in the blue trace in the attached figure. The X axis output had a peculiar effect where the initial flow reading would have the wrong sign, then catch  up, and then overshoot the steady state value. Thi plays havoc with the output when the scene is constantly changing speed and direction.

It's  looking more like a bug in the flow sensor firmware.

-Paul



On Saturday, 8 November 2014 00:57:03 UTC+11, David Pawlak wrote:

[Randy Mackay]

The position control even during yawing in particular is really impressive.

-Randy

[Paul Riseborough]

I merged the KLT (Kanade–Lucas–Tomasi)  flow extraction method from https://github.com/PX4/Flow/tree/klt_flow into the current master to see if it would provide a performance improvement.

https://github.com/priseborough/px4flow/tree/klt_flow

This was loaded onto the px4flow sensor, bench tested and given one flight test. The reduction in flow noise in the X-axis was significant and the angular motion in position hold was noticeably reduced. There was a small reduction in flow noise in the Y axis, but that could have been due to a small adjustment in optical flow scale factor made prior to the flight. The attached figures show the optical flow innovations from the EKF taken from two flights at the same altitude of ~1.5m, one running master on the px4flow sensor, and the other using the merge of klt flow and master. The units are mrad/sec (there are about 17 mrad per degree), so the RMS innovations are around 1 degree/second which is a good result.

The last two figures show an overlay of optical flow rate and gyro rate for a ground test where the sensor was held at a fixed location, pointed at a flat surface and rotated in a sinusoidal motion about the X axes. Because there is practically no translational motion of the sensor in this test, the optical flow angular rates should closely match the gyro rates. The flow output from the sensor running the klt_flow branch is almost a perfect overlay over the gyro rate, whereas the output from the sensor running master has significant differences.

The main drawback with the KLT optical flow method as currently implemented is that it does not provide a quality metric, which would make the EKF vulnerable to sensor problems like an out of focus or obscured lens, or flying over a bad surface. It does however enable testing to continue until the bug in master affecting the X axis output can be fixed.

The new optical flow interface and the ekfOptFlow-wip branch has now had 15 flights on copters and 3 flights on planes, so it's probably a good time to set up a wiki guide and provide executables for testing by those brave enough. The currrent px4flow, px4 firmware and APM FMU versions used for testing are:

https://github.com/priseborough/px4flow/tree/klt_flow #41f4ac9
https://github.com/priseborough/Firmware/tree/px4flow_integral_i2c #2685d72
https://github.com/priseborough/ardupilot/tree/ekfOptFlow-wip #f88ecd2

-Paul

[David Pawlak]

Paul, providing I can already compile master, just cloning these into desktop, where my master is, is all that needs to be done? And then compile from the WIP directory?

On Thursday, July 17, 2014 11:25:03 PM UTC-4, Paul Riseborough wrote:

Some of you would be aware that since supporting the implementation of the EKF in APM and PX4, I have been working on incorporating fusion of optical flow measurements into the EKF using a data set supplied by Lorenz.

After a number of false starts, I now have a prototype working in a proof of concept test bench https://github.com/priseborough/InertialNav/blob/optflow-wip/code/estimator_23states.cpp which uses the raw flow messages and can do the necessary calibration of the sensor focal length automatically. This involved addition of a state to the navigation EKF to estimate the local terrain elevation, update of all the filter equations to accommodate the additional state, implementation of the equations for the fusion of the optical flow rates and implementation of a separate EKF to estimate the sensor focal length scale factor. Some earlier attempts were made to try and integrate estimation of the focal length into the main filter, but this ran into problems with filter stability.

The good news is that this implementation is compatible data available on the existing MAVLINK optical_flow message and has also shown it is possible to estimate height above ground without use of a range sensor (if you have a independent measure of velocity like GPS), or can it can constrain drift when GPS is not available if you have a means of estimating height above ground, eg laser).

The first plot shows the convergence of the focal length scale factor over a flight, optical flow rate innovations and he laser range (in red) superimposed over the optical flow derived height estimate (in blue). The second plot shows the attitude estimate from the filter compared with the PX4 on-board solution.

These are preliminary results and there has been no filter tuning or data quality checking, but it shows the optical flow can provide an estimate of height above ground when laser readings are unavailable and that the optical flow fusion is working correctly. The combination of a laser and optical flow sensor, enables height above ground to be estimated across a larger range of heights.

The next step is to gather some more data sets (i will try to get one recorded from a copter) and do some more work on tuning and data quality checks. The prototype is currently not using the flow quality measure, so that will be one item to look at. Once the improvements will be rolled back into the optflow-wip branch and the testbench is producing reliable results, I can then start an APM implementation

Thank you to Lorenz and the PX4 team for the flight data, 3DR for providing a sensor to experiment with, and Randy for his patch integrating the PX4Flow sensor, which i will try to gather data with this weekend.

Regards,

Paul

[Paul Riseborough]

David,

It should be enough to compile from the ArduCopter directory once you have cloned the PX4Firmware and ardupilot code base. You will need to set EKF_GPS_TYPE to 3 to disable the GPS and set EKF_FALLBACK to 0 to reduce the chance of it reverting to DCM/INAV. At the moment the Copter code will not let you enter position hold mode unless you have a GPS lock, so you will need to do the test outdoors for the time being until we get a specific non-GPS mode sorted out.

You will also need to bridge the I2C bus offset solder pads for bits 0 and 1 on the back of your flow sensor to make is use address 0x45.

For the PX4Flow code, you can compile from its src directory using make clean and make all. You can then use QGroundControl to load the firmware onto the flow sensor and check it is in focus and providing outputs.

If you are using a quad, I can provide a link to my executables. It's now time for me to use that dev build server facility that Tridge has set up.

-Paul

[Jonathan Challinger]

The X axis still looks wrong/different - have you tried your "towel test" on it?

--

[Paul Riseborough]

Sorry, I attached the wrong image. Here is the X axis bench test output using the klt_flow branch.