Merge branch 'master' into telescience-v0.2

14 files changed, 848 insertions, 165 deletions

diff --git a/micropeak/MicroStatsTable.java b/micropeak/MicroStatsTable.java
index f373e25d..cf30fcb7 100644
--- a/micropeak/MicroStatsTable.java
+++ b/micropeak/MicroStatsTable.java
@@ -75,21 +75,21 @@ public class MicroStatsTable extends JComponent {
 	MicroStat	flight_time;
 	
 	public void setStats(MicroStats stats) {
-		max_height.set_values(String.format("%5.0f m", stats.apogee_height),
-				      String.format("%5.0f ft", AltosConvert.meters_to_feet(stats.apogee_height)));
-		max_speed.set_values(String.format("%5.0f m/s", stats.max_speed),
-				     String.format("%5.0f mph", AltosConvert.meters_to_mph(stats.max_speed)),
-				     String.format("Mach %4.1f", AltosConvert.meters_to_mach(stats.max_speed)));
-		max_accel.set_values(String.format("%5.0f m/s²", stats.max_accel),
-				     String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)),
-				     String.format("%5.0f G", AltosConvert.meters_to_g(stats.max_accel)));
-		avg_accel.set_values(String.format("%5.0f m/s²", stats.boost_accel(),
-						   String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())),
-						   String.format("%5.0f G", AltosConvert.meters_to_g(stats.boost_accel()))));
+		max_height.set_values(String.format("%7.1f m", stats.apogee_height),
+				      String.format("%7.1f ft", AltosConvert.meters_to_feet(stats.apogee_height)));
+		max_speed.set_values(String.format("%7.1f m/s", stats.max_speed),
+				     String.format("%7.1f mph", AltosConvert.meters_to_mph(stats.max_speed)),
+				     String.format("Mach %7.3f", AltosConvert.meters_to_mach(stats.max_speed)));
+		max_accel.set_values(String.format("%7.1f m/s²", stats.max_accel),
+				     String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)),
+				     String.format("%7.3f G", AltosConvert.meters_to_g(stats.max_accel)));
+		avg_accel.set_values(String.format("%7.1f m/s²", stats.boost_accel(),
+						   String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())),
+						   String.format("%7.3f G", AltosConvert.meters_to_g(stats.boost_accel()))));
 		boost_duration.set_values(String.format("%6.1f s", stats.boost_duration()));
 		coast_duration.set_values(String.format("%6.1f s", stats.coast_duration()));
-		descent_speed.set_values(String.format("%5.0f m/s", stats.descent_speed()),
-					 String.format("%5.0f ft/s", AltosConvert.meters_to_feet(stats.descent_speed())));
+		descent_speed.set_values(String.format("%7.1f m/s", stats.descent_speed()),
+					 String.format("%7.1f ft/s", AltosConvert.meters_to_feet(stats.descent_speed())));
 		descent_duration.set_values(String.format("%6.1f s", stats.descent_duration()));
 		flight_time.set_values(String.format("%6.1f s", stats.landed_time));
 	}
@@ -104,31 +104,31 @@ public class MicroStatsTable extends JComponent {
 		setLayout(layout);
 		int y = 0;
 		max_height = new MicroStat(layout, y++, "Maximum height",
-					   String.format("%5.0f m", stats.apogee_height),
-					   String.format("%5.0f ft", AltosConvert.meters_to_feet(stats.apogee_height)));
+					   String.format("%7.1f m", stats.apogee_height),
+					   String.format("%7.1f ft", AltosConvert.meters_to_feet(stats.apogee_height)));
 		max_speed = new MicroStat(layout, y++, "Maximum speed",
-					  String.format("%5.0f m/s", stats.max_speed),
-					  String.format("%5.0f mph", AltosConvert.meters_to_mph(stats.max_speed)),
+					  String.format("%7.1f m/s", stats.max_speed),
+					  String.format("%7.1f mph", AltosConvert.meters_to_mph(stats.max_speed)),
 					  String.format("Mach %4.1f", AltosConvert.meters_to_mach(stats.max_speed)));
 		max_accel = new MicroStat(layout, y++, "Maximum boost acceleration",
-					  String.format("%5.0f m/s²", stats.max_accel),
-					  String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)),
-					  String.format("%5.0f G", AltosConvert.meters_to_g(stats.max_accel)));
+					  String.format("%7.1f m/s²", stats.max_accel),
+					  String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)),
+					  String.format("%7.3f G", AltosConvert.meters_to_g(stats.max_accel)));
 		avg_accel = new MicroStat(layout, y++, "Average boost acceleration",
-					  String.format("%5.0f m/s²", stats.boost_accel(),
-							String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())),
-							String.format("%5.0f G", AltosConvert.meters_to_g(stats.boost_accel()))));
+					  String.format("%7.1f m/s²", stats.boost_accel(),
+							String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())),
+							String.format("%7.3f G", AltosConvert.meters_to_g(stats.boost_accel()))));
 		boost_duration = new MicroStat(layout, y++, "Boost duration",
-					       String.format("%6.0f s", stats.boost_duration()));
+					       String.format("%6.1f s", stats.boost_duration()));
 		coast_duration = new MicroStat(layout, y++, "Coast duration",
 					       String.format("%6.1f s", stats.coast_duration()));
 		descent_speed = new MicroStat(layout, y++, "Descent rate",
-					      String.format("%5.0f m/s", stats.descent_speed()),
-					      String.format("%5.0f ft/s", AltosConvert.meters_to_feet(stats.descent_speed())));
+					      String.format("%7.1f m/s", stats.descent_speed()),
+					      String.format("%7.1f ft/s", AltosConvert.meters_to_feet(stats.descent_speed())));
 		descent_duration = new MicroStat(layout, y++, "Descent duration",
 						 String.format("%6.1f s", stats.descent_duration()));
 		flight_time = new MicroStat(layout, y++, "Flight Time",
-					    String.format("%6.0f s", stats.landed_time));
+					    String.format("%6.1f s", stats.landed_time));
 	}
 
 	public MicroStatsTable() {
diff --git a/src/Makefile b/src/Makefile
index a17f51ac..e4b11039 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -8,6 +8,7 @@ vpath make-kalman util
 vpath make-whiten util
 vpath kalman.5c kalman
 vpath kalman_filter.5c kalman
+vpath kalman_micro.5c kalman
 vpath load_csv.5c kalman
 vpath matrix.5c kalman
 
diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c
index cfb7abea..46fa8e1f 100755
--- a/src/kalman/kalman.5c
+++ b/src/kalman/kalman.5c
@@ -55,6 +55,12 @@ real default_σ_m = 5;
 real default_σ_h = 20;
 real default_σ_a = 2;
 
+real[3,3] model_error(t, Φ) = multiply_mat_val ((real[3,3]) {
+		{ t**5 / 20, t**4 / 8, t**3 / 6 },
+		{ t**4 /  8, t**3 / 3, t**2 / 2 },
+		{ t**3 /  6, t**2 / 2, t }
+	}, Φ);
+
 parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
 	if (σ_m == 0)
 		σ_m = default_σ_m;
@@ -87,11 +93,7 @@ parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
  * Model error covariance. The only inaccuracy in the
  * model is the assumption that acceleration is constant
  */
-		.q = (real[3,3]) {
-			{ 0, 0, 0 },
-			{ 0, 0, 0 },
-			{.0, 0, σ_m**2	},
-		},
+		.q = model_error (t, σ_m**2),
 /*
  * Measurement error covariance
  * Our sensors are independent, so
@@ -140,11 +142,7 @@ parameters_t param_baro(real t, real σ_m, real σ_h) {
  * Model error covariance. The only inaccuracy in the
  * model is the assumption that acceleration is constant
  */
-		.q = (real[3,3]) {
-			{ 0, 0, 0 },
-			{ 0, 0, 0 },
-			{.0, 0, σ_m**2	},
-		},
+		.q = model_error (t, σ_m**2),
 /*
  * Measurement error covariance
  * Our sensors are independent, so
@@ -191,11 +189,7 @@ parameters_t param_accel(real t, real σ_m, real σ_a) {
  * Model error covariance. The only inaccuracy in the
  * model is the assumption that acceleration is constant
  */
-		.q = (real[3,3]) {
-			{ 0, 0, 0 },
-			{ 0, 0, 0 },
-			{.0, 0, σ_m**2	},
-		},
+		.q = model_error (t, σ_m**2),
 /*
  * Measurement error covariance
  * Our sensors are independent, so
@@ -236,11 +230,7 @@ parameters_t param_vel(real t) {
  * Model error covariance. The only inaccuracy in the
  * model is the assumption that acceleration is constant
  */
-		.q = (real[3,3]) {
-			{ 0, 0, 0 },
-			{ 0, 0, 0 },
-			{.0, 0, σ_m**2	},
-		},
+		.q = model_error (t, σ_m**2),
 /*
  * Measurement error covariance
  * Our sensors are independent, so
diff --git a/src/kalman/kalman_micro.5c b/src/kalman/kalman_micro.5c
new file mode 100644
index 00000000..266a1182
--- /dev/null
+++ b/src/kalman/kalman_micro.5c
@@ -0,0 +1,329 @@
+#!/usr/bin/env nickle
+
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+autoimport ParseArgs;
+
+load "load_csv.5c"
+import load_csv;
+
+load "matrix.5c"
+import matrix;
+
+load "kalman_filter.5c"
+import kalman;
+
+load "../util/atmosphere.5c"
+
+real	height_scale = 1.0;
+real	accel_scale = 1.0;
+real	speed_scale = 1.0;
+
+/*
+ * State:
+ *
+ * x[0] = pressure
+ * x[1] = delta pressure
+ * x[2] = delta delta pressure
+ */
+
+/*
+ * Measurement
+ *
+ * z[0] = pressure
+ */
+
+real default_σ_m = 5;
+real default_σ_h = 2.4;	/* pascals */
+
+real[3,3] model_error(t, Φ) = multiply_mat_val ((real[3,3]) {
+		{ t**5 / 20, t**4 / 8, t**3 / 6 },
+		{ t**4 /  8, t**3 / 3, t**2 / 2 },
+		{ t**3 /  6, t**2 / 2, t }
+	}, Φ);
+
+parameters_t param_baro(real t, real σ_m, real σ_h) {
+	if (σ_m == 0) {
+		printf ("Using default σ_m\n");
+		σ_m = default_σ_m;
+	}
+	if (σ_h == 0) {
+		printf ("Using default σ_h\n");
+		σ_h = default_σ_h;
+	}
+
+	σ_m = imprecise(σ_m) * accel_scale;
+	σ_h = imprecise(σ_h) * height_scale;
+
+	t = imprecise(t);
+	return (parameters_t) {
+/*
+ * Equation computing state k from state k-1
+ *
+ * height = height- + velocity- * t + acceleration- * t² / 2
+ * velocity = velocity- + acceleration- * t
+ * acceleration = acceleration-
+ */
+		.a = (real[3,3]) {
+			{ 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
+			{ 0, 1, t * speed_scale / accel_scale },
+			{ 0, 0, 1 }
+		},
+/*
+ * Model error covariance. The only inaccuracy in the
+ * model is the assumption that acceleration is constant
+ */
+		.q = model_error (t, σ_m**2),
+
+/*
+ * Measurement error covariance
+ * Our sensors are independent, so
+ * this matrix is zero off-diagonal
+ */
+		.r = (real[1,1]) {
+			{ σ_h ** 2 },
+		},
+/*
+ * Extract measurements from state,
+ * this just pulls out the height
+ * values.
+ */
+		.h = (real[1,3]) {
+			{ 1, 0, 0 },
+		},
+	 };
+}
+
+bool	just_kalman = true;
+real	accel_input_scale = 1;
+
+real	error_avg;
+
+void update_error_avg(real e) {
+	if (e < 0)
+		e = -e;
+
+#	if (e > 1000)
+#		e = 1000;
+
+	error_avg -= error_avg / 8;
+	error_avg += (e * e) / 8;
+}
+
+void run_flight(string name, file f, bool summary, real σ_m, real σ_h) {
+	state_t	current_both = {
+		.x = (real[3]) { 0, 0, 0 },
+		.p = (real[3,3]) { { 0 ... } ... },
+	};
+	state_t current_accel = current_both;
+	state_t current_baro = current_both;
+	real	t;
+	real	kalman_apogee_time = -1;
+	real	kalman_apogee = 0;
+	real	raw_apogee_time_first;
+	real	raw_apogee_time_last;
+	real	raw_apogee = 0;
+	real	speed = 0;
+	real	prev_acceleration = 0;
+	real	height, max_height = 0;
+	state_t	apogee_state;
+	parameters_fast_t	fast_baro;
+	real			fast_delta_t = 0;
+	bool			fast = true;
+	int			speed_lock = 0;
+
+	error_avg = 0;
+	for (;;) {
+		record_t	record = parse_record(f, 1.0);
+		if (record.done)
+			break;
+		if (is_uninit(&t))
+			t = record.time;
+		real delta_t = record.time - t;
+		if (delta_t < 0.096)
+			continue;
+#		delta_t = 0.096;	/* pretend that we're getting micropeak-rate data */
+#		record.time = record.time + delta_t;
+		t = record.time;
+		if (record.height > raw_apogee) {
+			raw_apogee_time_first = record.time;
+			raw_apogee = record.height;
+		}
+		if (record.height == raw_apogee)
+			raw_apogee_time_last = record.time;
+
+		real	pressure = record.pressure;
+
+		if (current_baro.x[0] == 0)
+			current_baro.x[0] = pressure;
+
+		vec_t z_baro = (real[1]) { record.pressure * height_scale };
+
+		real	error_h;
+
+		if (fast) {
+			if (delta_t != fast_delta_t) {
+				fast_baro = convert_to_fast(param_baro(delta_t, σ_m, σ_h));
+				fast_delta_t = delta_t;
+			}
+
+			current_baro.x = predict_fast(current_baro.x, fast_baro);
+			error_h = current_baro.x[0] - pressure;
+			current_baro.x = correct_fast(current_baro.x, z_baro, fast_baro);
+		} else {
+			parameters_t p_baro = param_baro(delta_t, σ_m, σ_h);
+
+			state_t pred_baro = predict(current_baro, p_baro);
+			error_h = current_baro.x[0] - pressure;
+			state_t next_baro = correct(pred_baro, z_baro, p_baro);
+			current_baro = next_baro;
+		}
+
+		update_error_avg(error_h);
+
+		/* Don't check for maximum altitude if we're accelerating upwards */
+		if (current_baro.x[2] / accel_scale < -2 * σ_m)
+			speed_lock = 10;
+		else if (speed_lock > 0)
+			speed_lock--;
+
+		height = pressure_to_altitude(current_baro.x[0] / height_scale);
+		if (speed_lock == 0 && height > max_height)
+			max_height = height;
+
+		printf ("%16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %d %d\n",
+			record.time,
+			record.pressure,
+			pressure_to_altitude(record.pressure),
+			current_baro.x[0] / height_scale,
+			current_baro.x[1] / speed_scale,
+			current_baro.x[2] / accel_scale,
+			height,
+			max_height,
+			error_h,
+			error_avg,
+			error_avg > 50000 ? 0 : 95000,
+			speed_lock > 0 ? 0 : 4500);
+
+		if (kalman_apogee_time < 0) {
+			if (current_baro.x[1] > 1) {
+				kalman_apogee = current_both.x[0];
+				kalman_apogee_time = record.time;
+			}
+		}
+	}
+	real raw_apogee_time = (raw_apogee_time_last + raw_apogee_time_first) / 2;
+	if (summary && !just_kalman) {
+		printf("%s: kalman (%8.2f m %6.2f s) raw (%8.2f m %6.2f s) error %6.2f s\n",
+		       name,
+		       kalman_apogee, kalman_apogee_time,
+		       raw_apogee, raw_apogee_time,
+		       kalman_apogee_time - raw_apogee_time);
+	}
+}
+
+void main() {
+	bool	summary = false;
+	int	user_argind = 1;
+	real	time_step = 0.01;
+	string	compute = "none";
+	string	prefix = "AO_K";
+	real	σ_m = default_σ_m;
+	real	σ_h = default_σ_h;
+
+	ParseArgs::argdesc argd = {
+		.args = {
+			{ .var = { .arg_flag = &summary },
+			  .abbr = 's',
+			  .name = "summary",
+			  .desc = "Print a summary of the flight" },
+			{ .var = { .arg_real = &time_step, },
+			  .abbr = 't',
+			  .name = "time",
+			  .expr_name = "<time-step>",
+			  .desc = "Set time step for convergence" },
+			{ .var = { .arg_string = &prefix },
+			  .abbr = 'p',
+			  .name = "prefix",
+			  .expr_name = "<prefix>",
+			  .desc = "Prefix for compute output" },
+			{ .var = { .arg_string = &compute },
+			  .abbr = 'c',
+			  .name = "compute",
+			  .expr_name = "{baro,none}",
+			  .desc = "Compute Kalman factor through convergence" },
+			{ .var = { .arg_real = &σ_m },
+			  .abbr = 'M',
+			  .name = "model",
+			  .expr_name = "<model-accel-error>",
+			  .desc = "Model co-variance for acceleration" },
+			{ .var = { .arg_real = &σ_h },
+			  .abbr = 'H',
+			  .name = "height",
+			  .expr_name = "<measure-height-error>",
+			  .desc = "Measure co-variance for height" },
+		},
+
+		.unknown = &user_argind,
+	};
+
+	ParseArgs::parseargs(&argd, &argv);
+
+	if (compute != "none") {
+		parameters_t	param;
+
+		printf ("/* Kalman matrix for micro %s\n", compute);
+		printf (" *     step = %f\n", time_step);
+		printf (" *     σ_m = %f\n", σ_m);
+		switch (compute) {
+		case "baro":
+			printf (" *     σ_h = %f\n", σ_h);
+			param = param_baro(time_step, σ_m, σ_h);
+			break;
+		}
+		printf (" */\n\n");
+		mat_t k = converge(param);
+		int[] d = dims(k);
+		int time_inc = floor(1/time_step + 0.5);
+		for (int i = 0; i < d[0]; i++)
+			for (int j = 0; j < d[1]; j++) {
+				string name;
+				if (d[1] == 1)
+					name = sprintf("%s_K%d_%d", prefix, i, time_inc);
+				else
+					name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
+				printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
+			}
+		printf ("\n");
+		exit(0);
+	}
+	string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] };
+
+	#	height_scale = accel_scale = speed_scale = 1;
+
+	if (dim(rest) == 0)
+		run_flight("<stdin>", stdin, summary, σ_m, σ_h);
+	else {
+		for (int i = 0; i < dim(rest); i++) {
+			twixt(file f = File::open(rest[i], "r"); File::close(f)) {
+				run_flight(rest[i], f, summary, σ_m, σ_h);
+			}
+		}
+	}
+}
+main();
diff --git a/src/kalman/load_csv.5c b/src/kalman/load_csv.5c
index 15e83166..0086c6db 100644
--- a/src/kalman/load_csv.5c
+++ b/src/kalman/load_csv.5c
@@ -31,6 +31,7 @@ namespace load_csv {
 		real	time;
 		real	height;
 		real	acceleration;
+		real	pressure;
 	} record_t;
 
 	public record_t parse_record(file f, real accel_scale) {
@@ -40,16 +41,28 @@ namespace load_csv {
 		int time_off = 4;
 		int height_off = 11;
 		int accel_off = 8;
-		if (string_to_integer(data[0]) == 2) {
+		int pres_off = 9;
+		switch (string_to_integer(data[0])) {
+		case 2:
 			time_off = 4;
 			accel_off = 9;
+			pres_off = 10;
 			height_off = 12;
+			break;
+		case 5:
+			time_off = 4;
+			accel_off = 10;
+			pres_off = 11;
+			height_off = 13;
+			break;
 		}
 		return (record_t) {
 			.done = false,
-				.time = string_to_real(data[time_off]),
-				.height = imprecise(string_to_real(data[height_off])),
-				.acceleration = imprecise(string_to_real(data[accel_off]) * accel_scale) };
+			.time = string_to_real(data[time_off]),
+			.height = imprecise(string_to_real(data[height_off])),
+			.acceleration = imprecise(string_to_real(data[accel_off]) * accel_scale),
+			.pressure = imprecise(string_to_real(data[pres_off]))
+		};
 	}
 
 	public void dump(file f) {
diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile
index ff0a4499..315b93f6 100644
--- a/src/micropeak/Makefile
+++ b/src/micropeak/Makefile
@@ -33,7 +33,9 @@ ALTOS_SRC = \
 	ao_eeprom_tiny.c \
 	ao_panic.c \
 	ao_log_micro.c \
-	ao_async.c
+	ao_async.c \
+	ao_microflight.c \
+	ao_microkalman.c
 
 INC=\
 	ao.h \
diff --git a/src/micropeak/ao_microflight.c b/src/micropeak/ao_microflight.c
new file mode 100644
index 00000000..714bb90a
--- /dev/null
+++ b/src/micropeak/ao_microflight.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef AO_FLIGHT_TEST
+#include <ao.h>
+#endif
+#include <ao_micropeak.h>
+#include <ao_log_micro.h>
+
+uint32_t	pa;
+uint32_t	pa_ground;
+uint32_t	pa_min;
+
+static void
+ao_microsample(void)
+{
+	ao_pa_get();
+	ao_microkalman_predict();
+	ao_microkalman_correct();
+}
+
+#define NUM_PA_HIST	16
+
+#define SKIP_PA_HIST(i,j)	(((i) + (j)) & (NUM_PA_HIST - 1))
+
+static uint32_t	pa_hist[NUM_PA_HIST];
+
+void
+ao_microflight(void)
+{
+	int16_t		sample_count;
+	uint16_t	time;
+	uint32_t	pa_interval_min, pa_interval_max;
+	int32_t		pa_diff;
+	uint8_t		h, i;
+	uint8_t		accel_lock = 0;
+	uint32_t	pa_sum = 0;
+
+	/* Wait for motion, averaging values to get ground pressure */
+
+	time = ao_time();
+	ao_pa_get();
+	ao_microkalman_init();
+	pa_ground = pa;
+	sample_count = 0;
+	h = 0;
+	for (;;) {
+		time += SAMPLE_SLEEP;
+		if (sample_count == 0)
+			ao_led_on(AO_LED_REPORT);
+		ao_delay_until(time);
+		ao_microsample();
+		if (sample_count == 0)
+			ao_led_off(AO_LED_REPORT);
+		pa_hist[h] = pa;
+		h = SKIP_PA_HIST(h,1);
+		pa_diff = pa_ground - ao_pa;
+
+		/* Check for a significant pressure change */
+		if (pa_diff > BOOST_DETECT)
+			break;
+
+		if (sample_count < GROUND_AVG * 2) {
+			if (sample_count < GROUND_AVG)
+				pa_sum += pa;
+			++sample_count;
+		} else {
+			pa_ground = pa_sum >> GROUND_AVG_SHIFT;
+			pa_sum = 0;
+			sample_count = 0;
+		}
+	}
+
+	/* Go back and find the first sample a decent interval above the ground */
+	pa_min = pa_ground - LAND_DETECT;
+	for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) {
+		if (pa_hist[i] < pa_min)
+			break;
+	}
+
+	/* Log the remaining samples so we get a complete history since leaving the ground */
+	for (; i != h; i = SKIP_PA_HIST(i,2)) {
+		pa = pa_hist[i];
+		ao_log_micro_data();
+	}
+
+	/* Now sit around until the pressure is stable again and record the max */
+
+	sample_count = 0;
+	pa_min = ao_pa;
+	pa_interval_min = ao_pa;
+	pa_interval_max = ao_pa;
+	for (;;) {
+		time += SAMPLE_SLEEP;
+		ao_delay_until(time);
+		if ((sample_count & 3) == 0)
+			ao_led_on(AO_LED_REPORT);
+		ao_microsample();
+		if ((sample_count & 3) == 0)
+			ao_led_off(AO_LED_REPORT);
+		if (sample_count & 1)
+			ao_log_micro_data();
+
+		/* If accelerating upwards, don't look for min pressure */
+		if (ao_pa_accel < ACCEL_LOCK_PA)
+			accel_lock = ACCEL_LOCK_TIME;
+		else if (accel_lock)
+			--accel_lock;
+		else if (ao_pa < pa_min)
+			pa_min = ao_pa;
+
+		if (sample_count == (GROUND_AVG - 1)) {
+			pa_diff = pa_interval_max - pa_interval_min;
+
+			/* Check to see if the pressure is now stable */
+			if (pa_diff < LAND_DETECT)
+				break;
+			sample_count = 0;
+			pa_interval_min = ao_pa;
+			pa_interval_max = ao_pa;
+		} else {
+			if (ao_pa < pa_interval_min)
+				pa_interval_min = ao_pa;
+			if (ao_pa > pa_interval_max)
+				pa_interval_max = ao_pa;
+			++sample_count;
+		}
+	}
+}
diff --git a/src/micropeak/ao_microkalman.c b/src/micropeak/ao_microkalman.c
new file mode 100644
index 00000000..0684ea2b
--- /dev/null
+++ b/src/micropeak/ao_microkalman.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef AO_FLIGHT_TEST
+#include <ao.h>
+#endif
+#include <ao_micropeak.h>
+
+#define FIX_BITS	16
+
+#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define from_fix8(x)	((x) >> 8)
+#define from_fix(x)	((x) >> 16)
+#define fix8_to_fix16(x)	((x) << 8)
+#define fix16_to_fix8(x)	((x) >> 8)
+
+#include <ao_kalman.h>
+
+/* Basic time step (96ms) */
+#define AO_MK_STEP	to_fix16(0.096)
+/* step ** 2 / 2 */
+#define AO_MK_STEP_2_2	to_fix16(0.004608)
+
+uint32_t	ao_k_pa;		/* 24.8 fixed point */
+int32_t		ao_k_pa_speed;		/* 16.16 fixed point */
+int32_t		ao_k_pa_accel;		/* 16.16 fixed point */
+
+uint32_t	ao_pa;			/* integer portion */
+int16_t		ao_pa_speed;		/* integer portion */
+int16_t		ao_pa_accel;		/* integer portion */
+
+void
+ao_microkalman_init(void)
+{
+	ao_pa = pa;
+	ao_k_pa = pa << 8;
+}	
+
+void
+ao_microkalman_predict(void)
+{
+	ao_k_pa       += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2);
+	ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP;
+}
+
+void
+ao_microkalman_correct(void)
+{
+	int16_t	e;	/* Height error in Pa */
+
+	e = pa - from_fix8(ao_k_pa);
+
+	ao_k_pa       += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10);
+	ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10;
+	ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10;
+	ao_pa = from_fix8(ao_k_pa);
+	ao_pa_speed = from_fix(ao_k_pa_speed);
+	ao_pa_accel = from_fix(ao_k_pa_accel);
+}
diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c
index f361aa26..10f0d192 100644
--- a/src/micropeak/ao_micropeak.c
+++ b/src/micropeak/ao_micropeak.c
@@ -24,16 +24,10 @@
 static struct ao_ms5607_sample	sample;
 static struct ao_ms5607_value	value;
 
-uint32_t	pa;
-uint32_t	pa_avg;
-uint32_t	pa_ground;
-uint32_t	pa_min;
 alt_t		ground_alt, max_alt;
 alt_t		ao_max_height;
 
-static uint32_t	pa_sum;
-
-static void
+void
 ao_pa_get(void)
 {
 	ao_ms5607_sample(&sample);
@@ -60,21 +54,9 @@ ao_pips(void)
 	ao_delay(AO_MS_TO_TICKS(200));
 }
 
-#define NUM_PA_HIST	16
-
-#define SKIP_PA_HIST(i,j)	(((i) + (j)) & (NUM_PA_HIST - 1))
-
-static uint32_t	pa_hist[NUM_PA_HIST];
-
 int
 main(void)
 {
-	int16_t		sample_count;
-	uint16_t	time;
-	uint32_t	pa_interval_min, pa_interval_max;
-	int32_t		pa_diff;
-	uint8_t		h, i;
-
 	ao_led_init(LEDS_AVAILABLE);
 	ao_timer_init();
 
@@ -93,93 +75,9 @@ main(void)
 	ao_log_micro_dump();
 	
 	ao_delay(BOOST_DELAY);
-	/* Wait for motion, averaging values to get ground pressure */
-	time = ao_time();
-	ao_pa_get();
-	pa_avg = pa_ground = pa << FILTER_SHIFT;
-	sample_count = 0;
-	h = 0;
-	for (;;) {
-		time += SAMPLE_SLEEP;
-		if (sample_count == 0)
-			ao_led_on(AO_LED_REPORT);
-		ao_delay_until(time);
-		ao_pa_get();
-		if (sample_count == 0)
-			ao_led_off(AO_LED_REPORT);
-		pa_hist[h] = pa;
-		h = SKIP_PA_HIST(h,1);
-		pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa;
-		pa_diff = pa_ground - pa_avg;
-
-		/* Check for a significant pressure change */
-		if (pa_diff > (BOOST_DETECT << FILTER_SHIFT))
-			break;
-
-		if (sample_count < GROUND_AVG * 2) {
-			if (sample_count < GROUND_AVG)
-				pa_sum += pa;
-			++sample_count;
-		} else {
-			pa_ground = pa_sum >> (GROUND_AVG_SHIFT - FILTER_SHIFT);
-			pa_sum = 0;
-			sample_count = 0;
-		}
-	}
 
-	pa_ground >>= FILTER_SHIFT;
+	ao_microflight();
 
-	/* Go back and find the first sample a decent interval above the ground */
-	pa_min = pa_ground - LAND_DETECT;
-	for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) {
-		if (pa_hist[i] < pa_min)
-			break;
-	}
-
-	/* Log the remaining samples so we get a complete history since leaving the ground */
-	for (; i != h; i = SKIP_PA_HIST(i,2)) {
-		pa = pa_hist[i];
-		ao_log_micro_data();
-	}
-
-	/* Now sit around until the pressure is stable again and record the max */
-
-	sample_count = 0;
-	pa_min = pa_avg;
-	pa_interval_min = pa_avg;
-	pa_interval_max = pa_avg;
-	for (;;) {
-		time += SAMPLE_SLEEP;
-		ao_delay_until(time);
-		if ((sample_count & 3) == 0)
-			ao_led_on(AO_LED_REPORT);
-		ao_pa_get();
-		if ((sample_count & 3) == 0)
-			ao_led_off(AO_LED_REPORT);
-		if (sample_count & 1)
-			ao_log_micro_data();
-		pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa;
-		if (pa_avg < pa_min)
-			pa_min = pa_avg;
-
-		if (sample_count == (GROUND_AVG - 1)) {
-			pa_diff = pa_interval_max - pa_interval_min;
-
-			/* Check to see if the pressure is now stable */
-			if (pa_diff < (LAND_DETECT << FILTER_SHIFT))
-				break;
-			sample_count = 0;
-			pa_interval_min = pa_avg;
-			pa_interval_max = pa_avg;
-		} else {
-			if (pa_avg < pa_interval_min)
-				pa_interval_min = pa_avg;
-			if (pa_avg > pa_interval_max)
-				pa_interval_max = pa_avg;
-			++sample_count;
-		}
-	}
-	pa_min >>= FILTER_SHIFT;
 	ao_log_micro_save();
 	ao_compute_height();
 	ao_report_altitude();
diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h
index e408d7c5..382b98d9 100644
--- a/src/micropeak/ao_micropeak.h
+++ b/src/micropeak/ao_micropeak.h
@@ -18,7 +18,6 @@
 #ifndef _AO_MICROPEAK_H_
 #define _AO_MICROPEAK_H_
 
-#define FILTER_SHIFT		3
 #define SAMPLE_SLEEP		AO_MS_TO_TICKS(96)
 
 /* 16 sample, or about two seconds worth */
@@ -32,14 +31,11 @@
 #define BOOST_DELAY		AO_SEC_TO_TICKS(30)
 
 /* Pressure change (in Pa) to detect landing */
-#define LAND_DETECT		12	/* 1m at sea level, 1.2m at 2000m */
+#define LAND_DETECT		24	/* 2m at sea level, 2.4m at 2000m */
 
 /* Current sensor pressure value */
 extern uint32_t	pa;
 
-/* IIR filtered pressure value */
-extern uint32_t	pa_avg;
-
 /* Average pressure value on ground */
 extern uint32_t	pa_ground;
 
@@ -52,5 +48,31 @@ extern alt_t	ground_alt, max_alt;
 /* max_alt - ground_alt */
 extern alt_t	ao_max_height;
 
+void
+ao_pa_get(void);
+
+void
+ao_microflight(void);
+
+#define ACCEL_LOCK_PA		-20
+#define ACCEL_LOCK_TIME		10
+
+extern uint32_t	ao_k_pa;		/* 24.8 fixed point */
+extern int32_t	ao_k_pa_speed;		/* 16.16 fixed point */
+extern int32_t	ao_k_pa_accel;		/* 16.16 fixed point */
+
+extern uint32_t	ao_pa;			/* integer portion */
+extern int16_t	ao_pa_speed;		/* integer portion */
+extern int16_t	ao_pa_accel;		/* integer portion */
+
+void
+ao_microkalman_init(void);
+
+void
+ao_microkalman_predict(void);
+
+void
+ao_microkalman_correct(void);
+	
 #endif
 
diff --git a/src/test/Makefile b/src/test/Makefile
index 092bf360..87bd70fe 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,14 +1,14 @@
-vpath % ..:../core:../drivers:../util
+vpath % ..:../core:../drivers:../util:../micropeak
 
 PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \
 	ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \
-	ao_aprs_test
+	ao_aprs_test ao_micropeak_test
 
 INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
 
 KALMAN=make-kalman 
 
-CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall
+CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall
 
 all: $(PROGS) ao_aprs_data.wav
 
@@ -60,4 +60,7 @@ ao_aprs_data.wav: ao_aprs_test
 	./ao_aprs_test | sox $(SOX_INPUT_ARGS) - $(SOX_OUTPUT_ARGS) $@
 
 check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests
-	./ao_fec_test && ./run-tests
\ No newline at end of file
+	./ao_fec_test && ./run-tests
+
+ao_micropeak_test: ao_micropeak_test.c ao_microflight.c
+	cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm
\ No newline at end of file
diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c
new file mode 100644
index 00000000..04686402
--- /dev/null
+++ b/src/test/ao_micropeak_test.c
@@ -0,0 +1,192 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define _GNU_SOURCE
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <math.h>
+
+FILE *emulator_in;
+char *emulator_app;
+char *emulator_name;
+char *emulator_info;
+uint8_t ao_flight_debug;
+
+#define AO_FLIGHT_TEST
+
+typedef int32_t alt_t;
+
+#define AO_MS_TO_TICKS(ms)	((ms) / 10)
+
+#define AO_LED_REPORT 0
+
+static void ao_led_on(uint8_t led) {
+}
+
+static void ao_led_off(uint8_t led) {
+}
+
+static void ao_delay_until(uint16_t target) {
+}
+
+static uint16_t ao_time(void) {
+	return 0;
+}
+
+#include "ao_microflight.c"
+#include "ao_microkalman.c"
+#include "ao_convert_pa.c"
+
+uint16_t	now;
+uint8_t		running;
+
+void ao_log_micro_data() {
+	running = 1;
+}
+
+void
+ao_micro_report(void)
+{
+	if (running) {
+		alt_t	ground = ao_pa_to_altitude(pa_ground);
+		printf ("%6.2f %10d %10d %10d\n", now / 100.0,
+			ao_pa_to_altitude(pa) - ground,
+			ao_pa_to_altitude(ao_pa) - ground,
+			ao_pa_to_altitude(pa_min) - ground);
+	}
+}
+
+void
+ao_micro_finish(void)
+{
+	ao_micro_report();
+}
+
+void
+ao_pa_get(void)
+{
+	char	line[4096];
+	char	*toks[128];
+	char	*saveptr;
+	int	t, ntok;
+	static int	time_id;
+	static int	pa_id;
+	double		time;
+	double		pressure;
+	static double	last_time;
+	static int	been_here;
+	static int	start_samples;
+
+	if (been_here && start_samples < 100) {
+		start_samples++;
+		return;
+	}
+	ao_micro_report();
+	for (;;) {
+		if (!fgets(line, sizeof (line), emulator_in))
+			exit(0);
+		for (t = 0; t < 128; t++) {
+			toks[t] = strtok_r(t ? NULL : line, ", ", &saveptr);
+			if (!toks[t])
+				break;
+		}
+		ntok = t;
+		if (toks[0][0] == '#') {
+			if (strcmp(toks[0],"#version") == 0) {
+				for (t = 1; t < ntok; t++) {
+					if (!strcmp(toks[t], "time"))
+						time_id = t;
+					if (!strcmp(toks[t],"pressure"))
+						pa_id = t;
+				}
+			}
+			continue;
+		}
+		time = strtod(toks[time_id],NULL);
+		pressure = strtod(toks[pa_id],NULL);
+		if (been_here && time - last_time < 0.1)
+			continue;
+		been_here = 1;
+		last_time = time;
+		now = floor (time * 100 + 0.5);
+		pa = pressure;
+		break;
+	}
+}
+
+void
+ao_dump_state(void)
+{
+}
+
+static const struct option options[] = {
+	{ .name = "summary", .has_arg = 0, .val = 's' },
+	{ .name = "debug", .has_arg = 0, .val = 'd' },
+	{ .name = "info", .has_arg = 1, .val = 'i' },
+	{ 0, 0, 0, 0},
+};
+
+void run_flight_fixed(char *name, FILE *f, int summary, char *info)
+{
+	emulator_name = name;
+	emulator_in = f;
+	emulator_info = info;
+	ao_microflight();
+	ao_micro_finish();
+}
+
+int
+main (int argc, char **argv)
+{
+	int	summary = 0;
+	int	c;
+	int	i;
+	char	*info = NULL;
+
+	emulator_app="baro";
+	while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) {
+		switch (c) {
+		case 's':
+			summary = 1;
+			break;
+		case 'd':
+			ao_flight_debug = 1;
+			break;
+		case 'i':
+			info = optarg;
+			break;
+		}
+	}
+
+	if (optind == argc)
+		run_flight_fixed("<stdin>", stdin, summary, info);
+	else
+		for (i = optind; i < argc; i++) {
+			FILE	*f = fopen(argv[i], "r");
+			if (!f) {
+				perror(argv[i]);
+				continue;
+			}
+			run_flight_fixed(argv[i], f, summary, info);
+			fclose(f);
+		}
+	exit(0);
+}
diff --git a/src/test/plotmicro b/src/test/plotmicro
new file mode 100755
index 00000000..cdfcc581
--- /dev/null
+++ b/src/test/plotmicro
@@ -0,0 +1,14 @@
+#!/bin/sh
+for i in "$@"; do
+gnuplot -p << EOF &
+set title "$i"
+set ylabel "height (m)"
+set xlabel "time (s)"
+set xtics border out nomirror
+set ytics border out nomirror
+set y2tics border out nomirror
+plot "$i" using 1:2 with lines lt 2 axes x1y1 title "raw height",\
+     "$i" using 1:3 with lines lt 4 axes x1y1 title "kalman height",\
+     "$i" using 1:4 with lines lt 1 axes x1y1 title "max height"
+EOF
+done
diff --git a/src/util/make-kalman b/src/util/make-kalman
index fd33bab0..580a4515 100644
--- a/src/util/make-kalman
+++ b/src/util/make-kalman
@@ -6,6 +6,7 @@ SIGMA_BOTH="-M 2 -H 6 -A 2"
 SIGMA_BARO="-M 2 -H 6 -A 2"
 SIGMA_ACCEL="-M 2 -H 4 -A 4"
 SIGMA_BOTH_NOISY_ACCEL="-M 2 -H 6 -A 3"
+SIGMA_MICRO="-M 10"
 
 echo '#if NOISY_ACCEL'
 echo
@@ -39,3 +40,4 @@ nickle kalman.5c -p AO_BARO -c baro -t 0.01 $SIGMA_BARO
 nickle kalman.5c -p AO_BARO -c baro -t 0.1 $SIGMA_BARO
 nickle kalman.5c -p AO_BARO -c baro -t 1 $SIGMA_BARO
 
+nickle kalman_micro.5c -p AO_MK_BARO -c baro -t 0.096 $SIGMA_MICRO
\ No newline at end of file