The way I'd approach this is similar to the algorithm used for NTP

Is that Marzullo's algorithm you're talking about?

Not really.

A lot of NTP is about determining propagation delays to find the consensus time, but the part that seems most relevant here is the algorithm used to steer the local clock towards the consensus time. Rather than simply adjust the local clock to correct for any discrepancies, the algorithm aims to control the local clock by skew adjustments so that the discrepancies tend to zero. That seems analogous to the current problem of detecting and correcting long-term errors in the integrated speed/position calculations by comparing them against the low resolution, but believed accurate, barometric height measurements.

I need to make some improvements to reduce oscillating and drifting.

Not to labour the point, but this is precisely the purpose of the D and I terms in PID. Your reluctance to use a conventional PID algorithm still puzzles me; it strikes me as the simplest and most appropriate solution to this problem.