Adding vertical (Z) velocity to inav estimator

[dyeldandi]

This is needed for high-accuracy GNSS sensors (like Ashtech MB100 we are using). It allows copter to maintain better altitude stability.

Basically copying horizontal code.

Here is the log http://dash.oznet.ch/view/wxpvxAKYstGZjXvkKTSkye

[LorenzMeier]

@DrTon Please join the review. Note that Thomas did suggest a pull request a while back to replace the boiler plate parameter handling code with the block params, e.g. like here:
https://github.com/PX4/Firmware/blob/master/src/modules/navigator/navigator_main.cpp#L140
Our recent discussion sounded like you appreciate now the value of wrapping the params as C++ objects to avoid boilerplate code 8). (PR is here for reference: #755)

However: There is no value in doing a rewrite just to merge this, so this is more of a general architectural remark - we should get this reviewed and in and then iterate on architecture.

@dyeldandi Your log shows that your copter might benefit from a small MC_PITCHRATE_I and MC_ROLLRATE_I value and would show even better position hold / control accuracy with it.

[dyeldandi]

@LorenzMeier
Ah, yes, thank you! We will definitely try that.

[LorenzMeier]

What is the initial tuning value here? Please provide the value you use in your system, but also a safe value that down weights GPS to a small contribution. We want to avoid getting altitude oscillation issues from bad GPS receivers and so start with a low default.

[dyeldandi]

We are using value 1.0 now even without RTK differentials, and I'd guess it would be quite safe to start from 0.5. My understanding is, since most GNSS receivers provide vertical measurements twice as inaccurate as their horizontal measurements, it ought to be 2 times less than INAV_W_XY_GPS_V.

[anatolfilin]

@LorenzMeier

Your log shows that your copter might benefit from a small MC_PITCHRATE_I
and MC_ROLLRATE_I value and would show even better position hold / control accuracy with it.
Lorenz, wondering how did you come to this conclusion? Is it by looking at saw on ATT.ROLLRATE? We constantly analyzing log files and trying to learn as many lessons as possible ...

[LorenzMeier]

Yes, the roll and pitch rates and commanded rates do not match and have a constant offset. If you have a constant offset, you need an integral gain to bring that offset to zero.
I will also add the commanded and actual local position, as this will similarly show how your controller performs. Generally you don't need to tune the outer loop controller for position much, but the rate and attitude responses have to be right for a good total response.

[anatolfilin]

Thanks a lot! Will try to tune inner loops better.

[LorenzMeier]

Could you provide a more detailed analysis / logs if this actually improves flight performance? We would need plots of the GPS velocity and flights with and without. I'm concerned that in noisy GPS reception situations this patch can deteriorate flight performance as GPS altitude is much more affected than position once the signal quality / sky view deteriorates.

[DrTon]

By default the weight for Z velocity can be 0. It needed only for RTK receivers, where altitude accuracy is ~10cm, in this case using velocity may be very useful.

[anatolfilin]

We've done lots of flying with and without this patch. We tested with regular Ublox and with Ashtech GPS board. We also played with various combinations of INAV_W_Z_GPS_P/INAV_W_Z_GPS_V/INAV_W_Z_GPS_BARO. The majority of tests were performed before merging Anton's L1 into trunk on MPC_TRACK branch. We haven't noticed any deterioration of quality. However there wasn't definite result on improving quality either. We just didn't have enough time before we stopped flying due to winter. I have plenty of log files with this patch showing very good vertical precision, but can't isolate effect of this particular patch vs overall quality of algorithm / hardware.

[LorenzMeier]

Ok. Happy to merge with default weight set to zero then.

[dyeldandi]

Ok. Happy to merge with default weight set to zero then.

Default is changed to zero.

[LorenzMeier]

Thanks!

[anatolfilin]

Thanks a lot!