Adding vertical (Z) velocity to inav estimator

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