There may be a mistake in calculation of inertial navigation.
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 at the end it returns _buff[14] that is the last sample not the oldest one. The same issue can be seen with correct_with_GPS().
Can anyone in the development team give an explanation?
There’s still no replies at all. I’d like to know at least the viewpoint from anyone in the development team. I don’t know the 3rd order complementary filter algorithm and highly appreciate if anyone can provide the link to that algorithm description.
Now if anyone reads this post can let me know I’m right or wrong as he thinks.
For reporting bugs, please use the bugtracker at Github.
For discussing possible bugs, the drones-discuss mailinglist would be the right place.
Here in the forums, APM:Code would have been the right place (moved the topic there now).
We no longer use peek(14) in master, we now use front(). Same in stable (3.1.2+)