search

PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.47k stars 13.5k forks source link

Slow optical flow drift in SITL #6772

Closed mhkabir closed 7 years ago

mhkabir commented 7 years ago

SITL optical-flow only configs seems to exhibit a slow drift over time, which wasn't this bad before. ~This seems to be the culprit : https://github.com/PX4/Firmware/pull/6527~ It seems to be the switch from the PX4Flow block-matching algorithm to the OpenCV KLT algorithm (https://github.com/PX4/Firmware/pull/6191/) for some reason. I'm not 100% sure yet. Results seem to differ from computer-to-computer.

@ChristophTobler Can you please crosstest?

Steps to repro :

  1. Checkout master
  2. make posix_sitl_default gazebo_iris_opt_flow or make posix_sitl_lpe gazebo_iris_opt_flow -> since both estimators exhibit this, it seems to be an issue in the flow algorithm or calculation.
  3. commander takeoff
ChristophTobler commented 7 years ago

@mhkabir Thanks, I'll have a look at it.

ChristophTobler commented 7 years ago

@mhkabir I don't think it has anything to do with the flow repo. I tried both versions (PX4flow and OpenCV) and both have a small drift. Also if I try a version before the integration of the new flow submodule e.g. 

git checkout `git rev-list -n 1 --before="2016-12-27 13:37" master`

I still get a small drift.

I have observed that even when the iris is slowly moving in one direction, the flow output is 0 for that old version. Maybe it is a bug in Gazebo with updating the image?

ChristophTobler commented 7 years ago

@mhkabir have you tested with different Gazebo versions, since you have observed a different behavior on different computers?

mhkabir commented 7 years ago

@mhkabir I don't think it has anything to do with the flow repo. I tried both versions (PX4flow and OpenCV) and both have a small drift. Also if I try a version before the integration of the new flow submodule e.g.

git checkout git rev-list -n 1 --before="2016-12-27 13:37" master I still get a small drift.

I agree, after having done more testing.

I have observed that even when the iris is slowly moving in one direction, the flow output is 0 for that old version. Maybe it is a bug in Gazebo with updating the image?

That is curious. I can see exactly this. Sometimes it just keeps going in the same direction slowly. It's like a biased drift.

have you tested with different Gazebo versions, since you have observed a different behavior on different computers?

I tested on macOS and Ubuntu, both running Gazebo 7 (same computer). In Ubuntu, the sim runs much faster for some reason, and the drift is lesser.

mhkabir commented 7 years ago

@ChristophTobler Here are 2 logs which kind of make the issue visible. I was testing something else, but you can observe the drift here. LPE also seems to be less drifty (within bounds of what I would expect from OF) while EKF2 is more so.

EKF2 : http://logs.px4.io/plot_app?log=0ac0bb3a-239d-4039-bd65-40409d963a67 LPE : http://review.px4.io/plot_app?log=1ed81db6-8f9b-405b-bc66-9151757657b2 (note that this is a longer flight)

ChristophTobler commented 7 years ago

Seems to be the gyro bias. For EKF it doesn't drift if I don't subtract the gyro bias here https://github.com/PX4/ecl/blob/master/EKF/estimator_interface.cpp#L316-L317 and set the gyro values to zero here https://github.com/PX4/sitl_gazebo/blob/master/src/gazebo_mavlink_interface.cpp#L820-L821. @priseborough FYI

priseborough commented 7 years ago

The flow sensor gyro bias is calculated here: https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L525

It is then used to correct the angular motion and gyro compensated flow rates (flow rates that have already had the flow sensor gyro data subtracted) here: https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L94-L95

The angular motion and gyro bias corrected flow rates are then used to calculate the innovations here: https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L98-L99

Looking at this function and how it is called I do not believe it is correctly averaging the autopilot gyro data across the same time interval as the flow gyro.

priseborough commented 7 years ago

@ChristophTobler Are you able to show me how to replicate your SITL simulation? I have pushed a patch here that I would like to test: https://github.com/PX4/ecl/tree/pr-ekfFlowGyroBias

priseborough commented 7 years ago

See https://github.com/PX4/ecl/pull/325

ChristophTobler commented 7 years ago

fixed in https://github.com/PX4/ecl/pull/325 respectively https://github.com/PX4/Firmware/pull/7966