LPE X-Y Position estimation based on Flow not in correct units

[ecmnet]

Although position hold is working basically, it seems as if the absolute units LPE estimates for local position in XY direction are not correct. Having moved the vehicle by about 1m, LPE shows a movement of about 0.2m.

http://logs.uaventure.com/view/RBzbbkjLCfPtU69py7w5QQ#Local_X_PLOT

To achieve better position hold results, I need to tune PIDs, but I would like to do that based on correct estimations.

[ecmnet]

Note that issue also below: The chart shows a max. positioning error of about 0,15m, while the vehicle moved nearly 1m:

[mhkabir]

There might be a need for a empirical scaling factor as used in inav and EKF-1.

Also, what rangefinder are you using?

[mhkabir]

Also, note that the LocalX and LocalY are NED frame estimates. So, they're decomposed. Are you sure that you moved 1m aligned to e.g North?

[mhkabir]

Estimator seems to be faulting continuously as well. See the last estimator health plot. Are you using the PX4Flow master firmware?

[ecmnet]

@mhkabir ad1: I am using a LaserRangeFinder with 1cm resolution (SF10A), which works perfect. Also altitude hold is ok. ad2: I am aware of that. Manual movements exactly to the North show similar results. ad3: Yes, this is the PX4Flow master. Ground has a good contrast, although this is indoors (btw. I also have had an ADNS3080 working in the same environment which resulted in good flow quality).

[ecmnet]

@mhkabir Strange: When I look to the raw flow quality, it looks like this:

: This should be sufficient and there are no drop-outs.

[ecmnet]

Additionnally, I found out that the scale in Y (EW) direction differs from Y (NS). Think I have to check Px4Flow...

[mhkabir]

Yeah, that can happen. EKF1 had separate FX_SCALER and FY_SCALER params, IIRC.

[ecmnet]

I am just digging into the PX4Flow Firmware. There I saw that the integration of flow is only performed when the sonar has valid measurements. On the other hand, BlockLocalPositionEstimator takes flow as provided by PX4Flow (and does not compensate with published altitude). Am I right?

[ecmnet]

Just removed the sonar compensation in PX4Flow (=assume altitude 1m). Flying in that height approximately gives now correct LPE scales. So for users that don't want to rely on sonar (indoors you have e.g. carpets or reflexions) but on laser range finders, I would suggest

a) add a parameter to PX4Flow to disable sonar compensation (or sonar at all?) b) add a parameter to LPE to compensate flow by altitude within LPE

What do you think?

[mhkabir]

BlockLocalPositionEstimator takes flow as provided by PX4Flow (and does not compensate with published altitude). Am I right?

You are not. https://github.com/PX4/Firmware/pull/3209/files#diff-478580be4572c29aaaa7e5861078259eR905

[mhkabir]

I will add a little something to disable Sonar when a Lidar is present. Currently, both are used in the update loop, which might be causing a bad Z-altitude estimate.

Can you check if your LocalZ matches the distance from ground?

[mhkabir]

add a parameter to PX4Flow to disable sonar compensation (or sonar at all?)

Flow firmware doesn't use the sonar to compensate anything. It just integrates flow measurements if the sonar is within range (which makes sense since the lens has a narrow focus band)

[ecmnet]

You're right. PX4Flow compensates only for speed estimation, not for accumulated flow. Still does it make no sense to me to rely on sonar for integration, when more accurate laser range data is available. I think my issue is, that (in my case) indoors the sonar is not working properly all the time.

I don't think I am facing a wrong Z estimation (although sonar AND Lidar are present) - as soon as Lidar is initialized by LPE, the altitude is perfect.

Regarding LPE flow altitude compensation: I don't get it right now. In correctSonar/Lidar I see clearly Z axis updates on the Kalman, but no flow altitude compensation...

[mhkabir]

no sense to me to rely on sonar for integration,

As I've reiterated, multiple times, sonar DOES NOT affect flow integration on the PX4Flow side. Just that the flow is accumulated if sonar distance > 30 cm. The lens is out of focus closer than that anyway.

I think my issue is, that (in my case) indoors the sonar is not working properly all the time

What makes you think this? Can you try to compare the sonar values and lidar distance values in the logs?

I don't get it right now. In correctSonar/Lidar I see clearly Z axis updates on the Kalman, but no flow altitude compensation..

CorrectSonar()/Lidar is the kalman correction for the altitude estimate (local z; (X_z) ), not Flow. See the correctFlow() for that. I already linked you to the specific line where the flow scaling is done with X_z

[ecmnet]

Just did a test-flight in posHold mode (with disabled distance check in flow): Still it seems to me that position estimation is too low (aprox. by factor 3-4):

Flow quality was good at all time:

Altitude was about 0.7m.

[mhkabir]

Try putting a 1.35x (or whatever) factor multiplier here and try it out : https://github.com/PX4/Firmware/pull/3209/files#diff-478580be4572c29aaaa7e5861078259eR902

You can also try disabling/removing the gyro compensation.

[ecmnet]

Yes, as I've agreed before, Flow does NOT compensate flow integration (though it does for speed estimation, which is not used). But it is clear that the distance check in Flow does affect the flow massively in my surrounding (carpets). So I will stay with disabled check for now.

Could you share some numbers, what is the achievable accuracy in poshold with LPE in your experience?

[mhkabir]

Could you share some numbers, what is the achievable accuracy in poshold with LPE in your experience?

I've had a pretty good experience with LPE sometime back (~2 months) on a small 250-size quadrotor. The gains were properly tuned, and the drift was fairly low, well under 10cm for about 3 mins of flight. I'll see if I can find those logs for you, or do another flight after reassembling the copter.

Something you can do to help would be to stick a GPS unit on the quad and send me the logs of an outdoor flight with flow + GPS. Then I can compare the estimated velocities from GPS and Flow. A VICON or other motion capture environment (if you have access to one) for groundtruth would be even more ideal.

[mhkabir]

Also, can you give me the log to that last flight of yours? Please put it up on LogMuncher.

[ecmnet]

Here is the log:

http://logs.uaventure.com/view/Wz2eijS7JFjtJsxY8UKPLL

As soon as the weather allows outdoor testing, I will try to compare velocities.

[mhkabir]

OK, it's weird that the estimator keeps faulting for optical flow. Check QGC for messages related to what's going on. I believe that the data fusion isn't taking place all the time, leading to underestimation.

[mhkabir]

Remove this line here and try a flight : https://github.com/PX4/Firmware/pull/3209/files#diff-478580be4572c29aaaa7e5861078259eR989

[ecmnet]

Yes, I also noticed that. Ok, I will give that a try tomorrow.

[ecmnet]

First dry-moves show that (as expected), flow updates are now regularly. I'll try a flight by tomorrow. Thank you for your help so far.

[ecmnet]

@mhkabir Did a test-flight: http://logs.uaventure.com/view/zgUWwccUk4zK8tANynHnfA. Estimation seem to be better with disabled flow-validity check, so you assumption is right. But even with this modification, I see an underestimation by aprox. 2. (Those two posHold points shown below were about 1m from each other). The reason for this could be that Loc.Pos.Z does not show absolute altitude above ground in my flight (comparing Sensor distance data with Loc.Pos.Z).

Again flow was valid all the time .

[ecmnet]

Another test-flight, where I ensured that Loc.Pos.Z represents distance to ground. Position estimation looks correct now (did posHold with some manual changes):

This leads for me to two questions:

1. How to ensure that altitude compensation is always done by distance to ground (I know you marked that already as a todo)?
2. Why is LPE assuming that the flow data is invalid?

Thank you again for your help (and sorry for those misunderstandings ).

[mhkabir]

How to ensure that altitude compensation is always done by distance to ground

We need to separate out LocalZ and add a separate terrain state to the filter. Something I've been meaning to get around to doing, but haven't yet. There is some work in the lpe_terrain branch, but it's not flyable yet. Weird things happen to the terrain estimate.

Why is LPE assuming that the flow data is invalid?

See the errors which QGC shows you.

I'm also presently focusing on EKF2 integration, so I'm not sure when I'll get around to that.

[ecmnet]

QGC messages don't help here (saying flow invalid), because that seems not to be correct. (Otherwise the patch wouldn't work).

[mhkabir]

I mean, the exact message. You see this : [lpe] bad flow data ?

That's only based on the quality check, so if that's going below 50, it would consider the data bad. You can set this https://github.com/PX4/Firmware/pull/3209/files#diff-478580be4572c29aaaa7e5861078259eR6 to 0 to disable that check.

[ecmnet]

I disabled the flow-quality check and compensated flow directly by lidar distance (if valid). This solved my issues and posHold accuracy now is about 0.15m (untuned). For now I am closing the issue. Thank you again.