There may be a mistake in calculation of inertial navigation.
This issue I've posted on ardupilot.com forum on APM:Copter>>3.1, but so far there is still no replies, so I post it again here with the hope that someone can give me an explanation.
In AP_InertialNav::correct_with_baro() because of the delay of 150 ms from baro we should calculate error using historical estimates about 15 samples ago (exactly the delay then is something between 140 and 149 ms). With the cyclic buffer of 15 samples it means the oldest sample or the sample at the _head index.
However, I read in the code that hist_position_base_z = _hist_position_estimate_z.peek(14).
In AP_Buffer.cpp there is j = _head+position; here position =14. For simplicity we can imagine that if _head = 0 then j =14 and in the end it returns _buff, that is the latest sample not the oldest one. The same issue can be seen with correct_with_GPS().
Can anyone in the development team give an explanation?