+#define AO_MAX_BARO_HEIGHT 12000
+
+/*
+ * Above this speed, baro measurements are unreliable
+ */
+#define AO_MAX_BARO_SPEED 300
+
+static void
+ao_kalman_predict(void)
+{
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_prev_tick > 5) {
+ ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
+ (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
+ ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10;
+
+ return;
+ }
+#endif
+ ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
+ (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
+ ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100;
+}
+
+static __pdata int16_t ao_error_h;
+static __pdata int16_t ao_raw_alt;
+static __pdata int16_t ao_raw_height;
+static __pdata int16_t ao_error_h_sq_avg;
+
+static void
+ao_kalman_err_height(void)
+{
+ int16_t e;
+ int16_t height_distrust;
+#if HAS_ACCEL
+ int16_t speed_distrust;
+#endif
+
+ ao_error_h = ao_raw_height - (int16_t) (ao_k_height >> 16);
+
+ e = ao_error_h;
+ if (e < 0)
+ e = -e;
+ if (e > 127)
+ e = 127;
+ ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4;
+ ao_error_h_sq_avg += (e * e) >> 4;
+
+ height_distrust = ao_raw_height - AO_MAX_BARO_HEIGHT;
+#if HAS_ACCEL
+ speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> 4;
+ if (speed_distrust <= 0)
+ speed_distrust = 0;
+ else if (speed_distrust > height_distrust)
+ height_distrust = speed_distrust;
+#endif
+ if (height_distrust <= 0)
+ height_distrust = 0;
+
+ if (height_distrust) {
+ if (height_distrust > 0x100)
+ height_distrust = 0x100;
+ ao_error_h = (int16_t) ((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8;
+ }
+}
+
+static void
+ao_kalman_correct_baro(void)
+{
+ ao_kalman_err_height();
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_prev_tick > 5) {
+ ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
+ ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h;
+ ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h;
+ return;
+ }