Determining Altitude AGL Using Optical Flow

You are probably familiar with optical flow sensors which provide extremely precise measurement of ground speed (and therefore position) for quadcopters and computer mice.  The PX4FLOW is the most-recent example but there are many others.

How it works

The optical flow sensor provides a 2D measurement of the angular speed of the image that is moving through its field of view, which is perfect for a mouse but by itself not that useful for a free-flying robot. It cannot differentiate between a near thing moving slowly, a far thing moving quickly, or its own camera rotating. Optical flow must be integrated with other kinds of sensor data for it to make sense in 3D space.

For those familiar with trigonometry, the diagram at right provides an intuitively-correct way of understanding how the various sensors are integrated, and how it can be possible to calculate an unknown value from known measurements.

  • Point C represents the center of the optical flow sensor’s view.
  • Length b is the current altitude above ground level (AGL) as reported by sonar.
  • Angle α is the raw optical flow, minus the current pitch rate as reported by the gyro. (Pitching up or down changes the viewpoint of the sensor, adding to the optical flow. We have to subtract this out to get a grounded measurement.)
  • Length a is the ground speed of the craft.

So, given altitude from sonar, pitch rate from the IMU, and optical flow, we can calculate ground speed.

While helpful for visualizing the math, the trigonometric method is only actually correct at α = 0. At α = 90°, a and h hit infinity, which is obviously nonsense. To calculate the actual distance traveled, you must calculate the sum of the movement across point C, which, when you do the calculus, ends up being the exact same formula used to calculate the length of the curved section of a circle segment. I've illustrated this with the second diagram at right.

It might seem that adding that curve to the equation has hopelessly complicated things, but in reality it makes the equation simpler, with no trigonometric functions at all. If I take out the obvious unit conversions, it looks like this:

ground speed = (optical flow - pitch rate) × altitude

A problem, and a solution

In the above formula altitude is a known value directly measured by sonar. Sonar works well in quadcopters and other craft which fly in a hover regime, but tends to be much less reliable in planes, where accurate AGL measurement is probably even more important, especially for landing.

Fortunately, ground speed is not an unknown in planes. GPS and airspeed measurements provide highly-accurate ground speed measurements (there are errors but they’re small compared to the higher speed of the craft). With ground speed solved, we can use basic algebra to move the terms around and solve for a different unknown:

altitude = ground speed / (optical flow - pitch rate)

Thus, we have a reliable and accurate measurement of altitude which works at even greater range than sonar.

Work to be done

The new PX4FLOW has its own sonar altimeter and gyros and performs all sensor integration internally. To add ground speed as an input and altitude as an output, one of the following will need to occur:

  • Autopilot reports ground speed to the PX4FLOW, which then integrates it with its own sensors and sends back the calculated altitude, or
  • PX4FLOW reports raw optical flow rates to autopilot. All other PX4FLOW sensors are turned off. Autopilot integrates optical flow with its own sensor data, providing improved accuracy of altitude, ground speed, position, and winds aloft.

The second option is the winner in my opinion, as it enables drop-in use of less complex optical flow hardware. I really don’t understand why a redundant IMU was placed on the PX4FLOW in the first place.

Please note that I’m not in a position to do this work myself. My own plane does not have any optical flow sensor, and probably won’t for a long time.  I’m OK with hard landings. I’m just offering the math that you’ll need to implement your own.


You may find that your optical flow sensor stops sending intelligible data as the ground gets close. This problem is also encountered with sonar, which has a minimum measurable distance due to the sensor deafening itself while generating the same sound that it needs to listen for. Optical flow fails for a different reason. If we solve for a different unknown we can see why:

optical flow = (ground speed / altitude) + pitch rate

As altitude approaches zero, optical flow will approach infinity. I don’t know the limits of the PX4FLOW (and I don’t have one to test) but I assume the limits are lower than you would want. There are several solutions to mix and match:

  • Install flaps, land at slower speeds.
  • Mount the optical flow board higher, perhaps under a wing instead of the belly.
  • Install a wider-angle lens on the optical flow sensor. The PX4FLOW camera has a 16mm focal length. A shorter lens will widen the view and reduce the detected optical flow. Use simple lenses only, not fisheye/GoPro/FPV, as geometric distortion will cause bad results. Do not use any lens which causes vignetting (black corners) as you will then need to digitally reduce the sample area which will defeat the purpose.
  • Solve it in software: Before landing, collect terrain data with a fly by, continue to measure altitude as long as possible on approach, and complete the landing the using the other available sensors.

Views: 8069

Comment by Rob McDougall on February 14, 2013 at 5:18pm

I love these sensors, they have come full circle! . Ive tested several mouse sensors on a wheeled robot chassis as linear sensors about a year ago. The System On Chip tracks pattern movment based on a pattern from military planes flying over icy mountains, which also looks like paper and any surface on that scale when magnified . contacting a majour sensor manufacturer i was informed 15 to 20%accuracy was realistic. And you can test this , I did, move your desk mouse between fixed points ! It has no need to be better because we compensate.

But that was mice, i havent tried these and I hope they are better, I hope so. Ive come from balancing robots and my experience of flying a quadcopter is in wind ! permanent pitch so the sonar also has to be on a gimble. I hear you say sonar is innifective from defeaning. The sonar device I have is fixed amplitude so that needs to change also,I years ago ! have made a pulser that delivers a single edge with a neon bulb across the TX transducer, US should be able to cover 5m to 10cm Bought a laser rangefinder UT392 before xmas for 42GBP , 2xAA , weight i estimate the optics, 30mm 13mm 20mm 10g, the electronics , 20g, ditch the screen, keyboard, case, batteries. laser rangefinder 30g . For vertical gnd measure you still have to have on double gimble, but then you can spin it round. described as Laser Distance meters .


Comment by Veikko Vierola on February 15, 2013 at 4:56am

Tridge, is there a method how to test the APM dead reckoning. I mean could you perhaps set GPS position calculation off with telemetry during flight. So that you could fly in stabilize mode for some minutes and after the flight you could compare the real GPS track and dead reckoning track side by side.


You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service