+#define AO_MAX_BARO_HEIGHT 8000
+
+/*
+ * 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;
+ 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;
+}
+
+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;
+ }
+#endif
+ ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h;
+ ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h;
+ ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h;
+}
+
+#if HAS_ACCEL
+static __pdata int16_t ao_error_a;
+static __pdata int32_t ao_accel_scale;
+
+static void
+ao_kalman_err_accel(void)
+{
+ int32_t accel;
+
+ accel = (ao_ground_accel - ao_raw_accel) * ao_accel_scale;
+
+ /* Can't use ao_accel here as it is the pre-prediction value still */
+ ao_error_a = (accel - ao_k_accel) >> 16;
+}
+
+static void
+ao_kalman_correct_both(void)
+{
+ ao_kalman_err_height();
+ ao_kalman_err_accel();
+
+#if 0
+ /*
+ * Check to see if things are crazy here --
+ * if the computed height is far above the
+ * measured height, we assume that the flight
+ * trajectory is not vertical, and so ignore
+ * the accelerometer for the remainder of the
+ * flight.
+ */
+ if (ao_error_h_sq_avg > 10)
+ {
+ ao_kalman_correct_baro();
+ return;
+ }
+#endif
+
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_prev_tick > 5) {
+ ao_k_height +=
+ (int32_t) AO_BOTH_K00_10 * ao_error_h +
+ (int32_t) (AO_BOTH_K01_10 >> 4) * ao_error_a;
+ ao_k_speed +=
+ ((int32_t) AO_BOTH_K10_10 << 4) * ao_error_h +
+ (int32_t) AO_BOTH_K11_10 * ao_error_a;
+ ao_k_accel +=
+ ((int32_t) AO_BOTH_K20_10 << 4) * ao_error_h +
+ (int32_t) AO_BOTH_K21_10 * ao_error_a;
+ return;
+ }
+#endif
+ ao_k_height +=
+ (int32_t) AO_BOTH_K00_100 * ao_error_h +
+ (int32_t) AO_BOTH_K01_100 * ao_error_a;
+ ao_k_speed +=
+ (int32_t) AO_BOTH_K10_100 * ao_error_h +
+ (int32_t) AO_BOTH_K11_100 * ao_error_a;
+ ao_k_accel +=
+ (int32_t) AO_BOTH_K20_100 * ao_error_h +
+ (int32_t) AO_BOTH_K21_100 * ao_error_a;
+}
+
+static void
+ao_kalman_correct_accel(void)
+{
+ ao_kalman_err_accel();
+
+#ifdef AO_FLIGHT_TEST
+ if (ao_flight_tick - ao_flight_prev_tick > 5) {
+ ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a;
+ ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a;
+ ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a;
+ return;
+ }
+#endif
+ ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
+ ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
+ ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
+}
+#endif /* HAS_ACCEL */
+
+__xdata int32_t ao_raw_pres_sum;
+
+#ifdef HAS_ACCEL
+__xdata int32_t ao_raw_accel_sum;
+#endif