Make LPE Default Position Estimator

[jgoppert]

Generally performance of LPE has been much better than INAV. This log shows an example where that may not be the case. The accelerometer noise is set very low, to the data sheet spec. This results in us trusting the integration scheme to a level never tested previously. This is creating some new issues: For instance, here are two flights, one with INAV, and one with LPE:

INAV

LPE

Notice that with LPE we maintain an offset relative to the GPS which eventually goes away at the end of the flight. This could be two things:

1. The estimator could be working better than before and we are now able to counter GPS drift. In that case we need to figure out why it paused at each waypoint and did not continue smoothly.

   Remedy Figure out why we paused.

2. Accelerometer bias isn't being adequately accounted for, maybe bias random process noise level is set too low and we have a constant accel bias pulling the vehicle to the right.

   Remedy Increase accelerometer noise level or increase bias random walk noise level.

Here is a plot of the accelerometer bias estimation for the flight. It isn't clear that the y-bias had a large offset. So again, maybe are are doing well here and just need to figure out why it paused when the GPS estimate was significantly different than the position estimate.

Flight Logs

• INAV http://logs.uaventure.com/view/Abwwn6Zozrzd792USGhrih
• LPE http://logs.uaventure.com/view/VxsZcUFeeXutLeNqaa6dkH

[LorenzMeier]

@jgoppert It did not pause, but instead it overshot, which is against the design of the system. How fast is LPE publishing? I can see the sawtooth in the estimate clearly.

[jgoppert]

Update rate is controlled by sensor combined topic, so around 100-200 Hz. The saw tooth I think is from the GPS and inertial positions fighting. If we increase the accel noise, the integrator would give up and start trusting GPS more.

[mhkabir]

@LorenzMeier @jgoppert With respect to the sawtoothing, I was thinking about the low-pass filter introduced on the controllers to smooth the estimate. Shouldn't any sort of smoothing be done on the estimator side, since it would benefit all downstream consumers of the position estimate?

[scott-eddy]

I would think that running the output of a Kalman filter through a low pass filter is really not a proper solution here. Ideally we should be able to adjust the tuning knobs of the KF (noise variances, adding states such as accel bias, etc.) in order to get better estimator performance. Filtering the output of one filter with another makes overall tuning more difficult and is not really addressing the problems inherit in the first filter.

Just my 2-cents

[jgoppert]

@mhkabir , @scott-eddy I agree that we have to address the saw-toothing. This was more of an edge case. I flew about 5 times with that code and only saw this once. As @scott-eddy suggested, if we increase the accel noise, it will converge to the GPS trajectory and you won't see any more saw toothing, so this would likely be better than a low pass filter. When a kalman filter saw tooths, two things are fighting, in this case GPS and accel and we don't let either be dominant.

[jgoppert]

@mhkabir, @LorenzMeier, @scott-eddy

After the latest fix to the bias estimation I'm now getting excellent results in several live flight tests:

The next item on the list to address should be GPS delays. Here is a minimization of the error between the estimated and GPS signal. The minimum represents the best fit.

The GPS xy position are lagging about 150 milliseconds while the altitude and velocity components are lagging by about 250 milliseconds.

Flight log: http://logs.uaventure.com/view/L373uGfDQH5iuSA9XiQgGS

[LorenzMeier]

Hi James,

Awesome! That looks much more like it.

The interpretation of that data should be that the GPS is always lagging 250 ms but the baro is already lagging 100 ms behind the accel, and needs similar compensation. The accel lag itself is about 30 ms with the low passed values you're currently using and more in the 10 ms range with the delta velocities I added recently.

[LorenzMeier]

Looking at this I also realised that we should be publishing navigation_capabilities from the controller and announcing our min switch radius, which would be current velocity * brake time, where brake time would be max deceleration integrated to hit a value of zero. E.g. for the plot above setting the brake time to 1s would have resulted in 2-5m waypoint switch radius. https://github.com/PX4/Firmware/blob/master/msg/navigation_capabilities.msg

Set turn distance to current velocity * 1s. The navigator picks max(wp_switch_dist, navigator_switch_dist). That would have led to a smoother trajectory here, and is an easy and quick fix.

[scott-eddy]

This is looking much better. Any chance of it flying with optical flow indoors any time soon? It seems that GPS denied estimation has understandably been put on the back burner, but ideally won't be forgotten. Hopefully with the bias fix we would see improvement in the indoor case as well.

Can anyone work on this? I am getting a quad with a flow up and running now but it won't be flying for a month probably.

[mhkabir]

@scott-eddy Working on it. I just came back from vacation. I'll fly and upload a video of pure-flow flight with LPE soon. I have some fixes here on my side which will need to be sent in for things to start working properly again (#3067) :)

[LorenzMeier]

@jgoppert What is holding this off is the question of the influence of GPS and baro delays on the gyro offset estimate. I would be thrilled to switch to your attitude EKF as well, but I want to first be sure that we fix the root cause with the right solution.

Its clear that a 200 ms phase delay has an influence on the gyro offset estimation, and as long that's not addressed in the position estimator and validated I don't want to start messing with the attitude estimate.

[LorenzMeier]

Closing this due to lack of activity.

[jgoppert]

Reopening this to start work to wrap it up.

Goals are now:

1. Analyze GPS delay effect on attitude gyro bias estimation vs. INAV.
2. Determine accuracy of accel bias estimation using fixed offsets in simulation.
3. Test indoor flight with flow.

@LorenzMeier Please feel free to add to this list.

@TSC21, @BMP-TECH, @mhkabir If you guys can help me address these let me know. I'd like to finish this quickly.

[mhkabir]

@jgoppert @LorenzMeier

3209 has a load of changes implementing number 3 and fixing miscellaneous issues. It would be ideal to have that merged in first, before any other changes in order to avoid painful rebases.

A large amount of time was sunk into it, and has had solid cross testing from myself (and MANY others!)

[jgoppert]

@mhkabir Sounds good. I think we need to get some plots of performance vs. INAV as a base line for the flow that it is robust enough for main stream.

[jgoppert]

@LorenzMeier let me know what else is holding this back.

[LorenzMeier]

Closing in favour of #4483.