vpath %.c ..
vpath %.h ..
vpath make-altitude ..
+vpath make-kalman ..
+vpath kalman.5c ../kalman
+vpath kalman_filter.5c ../kalman
+vpath load_csv.5c ../kalman
+vpath matrix.5c ../kalman
vpath ao-make-product.5c ..
CC=sdcc
ao_pins.h \
cc1111.h \
altitude.h \
+ ao_kalman.h \
25lc1024.h
#
../altitude.h: make-altitude
nickle $< > $@
+../ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c
+ sh $< > $@
+
ao_product.h: ao-make-product.5c ../Version
$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
--- /dev/null
+#!/usr/bin/env nickle
+
+/*
+ * Copyright © 2011 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;
+
+/*
+ * AltOS keeps speed and accel scaled
+ * by 4 bits to provide additional precision
+ */
+real height_scale = 1.0;
+real accel_scale = 16.0;
+real speed_scale = 16.0;
+
+/*
+ * State:
+ *
+ * x[0] = height
+ * x[1] = velocity
+ * x[2] = acceleration
+ */
+
+/*
+ * Measurement
+ *
+ * z[0] = height
+ * z[1] = acceleration
+ */
+
+real default_σ_m = 5;
+real default_σ_h = 20;
+real default_σ_a = 2;
+
+parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
+ if (σ_m == 0)
+ σ_m = default_σ_m;
+ if (σ_h == 0)
+ σ_h = default_σ_h;
+ if (σ_a == 0)
+ σ_a = default_σ_a;
+
+ σ_m = imprecise(σ_m) * accel_scale;
+ σ_h = imprecise(σ_h) * height_scale;
+ σ_a = imprecise(σ_a) * accel_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 = (real[3,3]) {
+ { 0, 0, 0 },
+ { 0, 0, 0 },
+ {.0, 0, σ_m**2 },
+ },
+/*
+ * Measurement error covariance
+ * Our sensors are independent, so
+ * this matrix is zero off-diagonal
+ */
+ .r = (real[2,2]) {
+ { σ_h ** 2, 0 },
+ { 0, σ_a ** 2 },
+ },
+/*
+ * Extract measurements from state,
+ * this just pulls out the height and acceleration
+ * values.
+ */
+ .h = (real[2,3]) {
+ { 1, 0, 0 },
+ { 0, 0, 1 },
+ },
+ };
+}
+
+parameters_t param_baro(real t, real σ_m, real σ_h) {
+ if (σ_m == 0)
+ σ_m = default_σ_m;
+ if (σ_h == 0)
+ σ_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 = (real[3,3]) {
+ { 0, 0, 0 },
+ { 0, 0, 0 },
+ {.0, 0, σ_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 },
+ },
+ };
+}
+
+parameters_t param_accel(real t, real σ_m, real σ_a) {
+ if (σ_m == 0)
+ σ_m = default_σ_m;
+ if (σ_a == 0)
+ σ_a = default_σ_a;
+
+ σ_m = imprecise(σ_m) * accel_scale;
+ σ_a = imprecise(σ_a) * accel_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 = (real[3,3]) {
+ { 0, 0, 0 },
+ { 0, 0, 0 },
+ {.0, 0, σ_m**2 },
+ },
+/*
+ * Measurement error covariance
+ * Our sensors are independent, so
+ * this matrix is zero off-diagonal
+ */
+ .r = (real[1,1]) {
+ { σ_a ** 2 },
+ },
+/*
+ * Extract measurements from state,
+ * this just pulls out the acceleration
+ * values.
+ */
+ .h = (real[1,3]) {
+ { 0, 0, 1 },
+ },
+ };
+}
+
+parameters_t param_vel(real t) {
+ static real σ_m = .1;
+ static real σ_v = imprecise(10);
+
+ 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, imprecise(t), imprecise((t**2)/2) },
+ { 0, 1, imprecise(t) },
+ { 0, 0, 1 }
+ },
+/*
+ * 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 },
+ },
+/*
+ * Measurement error covariance
+ * Our sensors are independent, so
+ * this matrix is zero off-diagonal
+ */
+ .r = (real[1,1]) {
+ { σ_v ** 2 },
+ },
+/*
+ * Extract measurements from state,
+ * this just pulls out the velocity
+ * values.
+ */
+ .h = (real[1,3]) {
+ { 0, 1, 0 },
+ },
+ };
+}
+
+real max_baro_height = 18000;
+
+bool just_kalman = true;
+real accel_input_scale = 1;
+
+void run_flight(string name, file f, bool summary) {
+ 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 default_descent_rate = 20;
+ real speed = 0;
+ real prev_acceleration = 0;
+ state_t apogee_state;
+ parameters_fast_t fast_both;
+ parameters_fast_t fast_baro;
+ parameters_fast_t fast_accel;
+ real fast_delta_t = 0;
+ bool fast = true;
+
+ for (;;) {
+ record_t record = parse_record(f, accel_input_scale);
+ if (record.done)
+ break;
+ if (is_uninit(&t))
+ t = record.time;
+ real delta_t = record.time - t;
+ if (delta_t <= 0)
+ continue;
+ 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 acceleration = record.acceleration;
+ real height = record.height;
+
+ speed = (speed + (acceleration + prev_acceleration / 2) * delta_t);
+ prev_acceleration = acceleration;
+
+ vec_t z_both = (real[2]) { record.height * height_scale, record.acceleration * accel_scale };
+ vec_t z_accel = (real[1]) { record.acceleration * accel_scale };
+ vec_t z_baro = (real[1]) { record.height * height_scale };
+
+
+ if (fast) {
+ if (delta_t != fast_delta_t) {
+ fast_both = convert_to_fast(param_both(delta_t, 0, 0, 0));
+ fast_accel = convert_to_fast(param_accel(delta_t, 0, 0));
+ fast_baro = convert_to_fast(param_baro(delta_t, 0, 0));
+ fast_delta_t = delta_t;
+ }
+
+ current_both.x = predict_fast(current_both.x, fast_both);
+ current_accel.x = predict_fast(current_accel.x, fast_accel);
+ current_baro.x = predict_fast(current_baro.x, fast_baro);
+
+ current_both.x = correct_fast(current_both.x, z_both, fast_both);
+ current_accel.x = correct_fast(current_accel.x, z_accel, fast_accel);
+ current_baro.x = correct_fast(current_baro.x, z_baro, fast_baro);
+ } else {
+ parameters_t p_both = param_both(delta_t, 0, 0, 0);
+ parameters_t p_accel = param_accel(delta_t, 0, 0);
+ parameters_t p_baro = param_baro(delta_t, 0, 0);
+
+ state_t pred_both = predict(current_both, p_both);
+ state_t pred_accel = predict(current_accel, p_accel);
+ state_t pred_baro = predict(current_baro, p_baro);
+
+ state_t next_both = correct(pred_both, z_both, p_both);
+ state_t next_accel = correct(pred_accel, z_accel, p_accel);
+ state_t next_baro = correct(pred_baro, z_baro, p_baro);
+ current_both = next_both;
+ current_accel = next_accel;
+ current_baro = next_baro;
+ }
+
+ printf ("%16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f\n",
+ record.time,
+ record.height, speed, record.acceleration,
+ current_both.x[0] / height_scale, current_both.x[1] / speed_scale, current_both.x[2] / accel_scale,
+ current_accel.x[0] / height_scale, current_accel.x[1] / speed_scale, current_accel.x[2] / accel_scale,
+ current_baro.x[0] / height_scale, current_baro.x[1] / speed_scale, current_baro.x[2] / accel_scale);
+ if (kalman_apogee_time < 0) {
+ if (current_both.x[1] < -1 && current_accel.x[1] < -1 && current_baro.x[1] < -1) {
+ kalman_apogee = current_both.x[0];
+ kalman_apogee_time = record.time;
+ break;
+ }
+ }
+ }
+ 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 = 1;
+ real σ_h = 4;
+ real σ_a = 1;
+
+ ParseArgs::argdesc argd = {
+ .args = {
+ { .var = { .arg_flag = &summary },
+ .abbr = 's',
+ .name = "summary",
+ .desc = "Print a summary of the flight" },
+ { .var = { .arg_real = &max_baro_height },
+ .abbr = 'm',
+ .name = "maxbaro",
+ .expr_name = "height",
+ .desc = "Set maximum usable barometer height" },
+ { .var = { .arg_real = &accel_input_scale, },
+ .abbr = 'a',
+ .name = "accel",
+ .expr_name = "<accel-scale>",
+ .desc = "Set accelerometer scale factor" },
+ { .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 = "{both,baro,accel}",
+ .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" },
+ { .var = { .arg_real = &σ_a },
+ .abbr = 'A',
+ .name = "accel",
+ .expr_name = "<measure-accel-error>",
+ .desc = "Measure co-variance for acceleration" },
+ },
+
+ .unknown = &user_argind,
+ };
+
+ ParseArgs::parseargs(&argd, &argv);
+
+ if (compute != "none") {
+ parameters_t param;
+
+ printf ("/* Kalman matrix for %s\n", compute);
+ printf (" * step = %f\n", time_step);
+ printf (" * σ_m = %f\n", σ_m);
+ switch (compute) {
+ case "both":
+ printf (" * σ_h = %f\n", σ_h);
+ printf (" * σ_a = %f\n", σ_a);
+ param = param_both(time_step, σ_m, σ_h, σ_a);
+ break;
+ case "accel":
+ printf (" * σ_a = %f\n", σ_a);
+ param = param_accel(time_step, σ_m, σ_a);
+ break;
+ 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);
+ if (d[1] == 2) {
+ for (int i = 0; i < d[0]; i++)
+ for (int j = 0; j < d[1]; j++)
+ printf ("#define %s_K%d%d_%d to_fix16(%12.10f)\n",
+ prefix, i, j, time_inc, k[i,j]);
+ } else {
+ for (int i = 0; i < d[0]; i++) {
+ printf ("#define %s_K%d_%d to_fix16(%12.10f)\n",
+ prefix, i, time_inc, k[i,0]);
+ }
+ }
+ printf ("\n");
+ return;
+ }
+ 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);
+ 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);
+ }
+ }
+ }
+}
+main();
+#kalman(stdin);
+#dump(stdin);
--- /dev/null
+load "matrix.5c"
+
+/*
+ * Copyright © 2011 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.
+ */
+
+namespace kalman {
+
+ import matrix;
+
+ public typedef struct {
+ vec_t x; /* state */
+ mat_t p; /* error estimate */
+ mat_t k; /* kalman factor */
+ } state_t;
+
+ public typedef struct {
+ mat_t a; /* model */
+ mat_t q; /* model error covariance */
+ mat_t r; /* measurement error covariance */
+ mat_t h; /* measurement from model */
+ } parameters_t;
+
+ public typedef struct {
+ mat_t a; /* model */
+ mat_t k; /* kalman coefficient */
+ mat_t h; /* measurement from model */
+ } parameters_fast_t;
+
+ vec_t measurement_from_state(vec_t x, mat_t h) {
+ return multiply_mat_vec(h, x);
+ }
+
+
+ void print_state(string name, state_t s) {
+ print_vec(sprintf("%s state", name), s.x);
+ print_mat(sprintf("%s error", name), s.p);
+ }
+
+ public bool debug = false;
+
+ public state_t predict (state_t s, parameters_t p) {
+ state_t n;
+
+ if (debug) {
+ printf ("--------PREDICT--------\n");
+ print_state("current", s);
+ }
+
+ /* Predict state
+ *
+ * x': predicted state
+ * a: model
+ * x: previous state
+ *
+ * x' = a * x;
+ */
+
+ n.x = multiply_mat_vec(p.a, s.x);
+
+ /* t0 = a * p */
+ mat_t t0 = multiply (p.a, s.p);
+ if (debug)
+ print_mat("t0", t0);
+
+ /* t1 = a * p * transpose(a) */
+
+ mat_t t1 = multiply (t0, transpose(p.a));
+
+ /* Predict error
+ *
+ * p': predicted error
+ * a: model
+ * p: previous error
+ * q: model error
+ *
+ * p' = a * p * transpose(a) + q
+ */
+
+ n.p = add(t1, p.q);
+ if (debug)
+ print_state("predict", n);
+ return n;
+ }
+
+ public vec_t predict_fast(vec_t x, parameters_fast_t p) {
+ if (debug) {
+ printf ("--------FAST PREDICT--------\n");
+ print_vec("current", x);
+ }
+ vec_t new = multiply_mat_vec(p.a, x);
+ if (debug)
+ print_vec("predict", new);
+ return new;
+ }
+
+ public vec_t correct_fast(vec_t x, vec_t z, parameters_fast_t p) {
+ if (debug) {
+ printf ("--------FAST CORRECT--------\n");
+ print_vec("measure", z);
+ print_vec("current", x);
+ }
+ vec_t model = multiply_mat_vec(p.h, x);
+ if (debug)
+ print_vec("extract model", model);
+ vec_t diff = vec_subtract(z, model);
+ if (debug)
+ print_vec("difference", diff);
+ vec_t adjust = multiply_mat_vec(p.k, diff);
+ if (debug)
+ print_vec("adjust", adjust);
+
+ vec_t new = vec_add(x,
+ multiply_mat_vec(p.k,
+ vec_subtract(z,
+ multiply_mat_vec(p.h, x))));
+ if (debug)
+ print_vec("correct", new);
+ return new;
+ }
+
+ public state_t correct(state_t s, vec_t z, parameters_t p) {
+ state_t n;
+
+ if (debug) {
+ printf ("--------CORRECT--------\n");
+ print_vec("measure", z);
+ print_state("current", s);
+ }
+
+ /* t0 = p * T(h) */
+
+ /* 3x2 = 3x3 * 3x2 */
+ mat_t t0 = multiply(s.p, transpose(p.h));
+ if (debug)
+ print_mat("t0", t0);
+
+ /* t1 = h * p */
+
+ /* 2x3 = 2x3 * 3x3 */
+ mat_t t1 = multiply(p.h, s.p);
+ if (debug)
+ print_mat("t1", t1);
+
+ /* t2 = h * p * transpose(h) */
+
+ /* 2x2 = 2x3 * 3x2 */
+ mat_t t2 = multiply(t1, transpose(p.h));
+ if (debug)
+ print_mat("t2", t2);
+
+ /* t3 = h * p * transpose(h) + r */
+
+ /* 2x2 = 2x2 + 2x2 */
+ mat_t t3 = add(t2, p.r);
+ if (debug)
+ print_mat("t3", t3);
+
+ /* t4 = inverse(h * p * transpose(h) + r) */
+
+ /* 2x2 = 2x2 */
+ mat_t t4 = inverse(t3);
+ if (debug)
+ print_mat("t4", t4);
+
+ /* Kalman value */
+
+ /* k: Kalman value
+ * p: error estimate
+ * h: state to measurement matrix
+ * r: measurement error covariance
+ *
+ * k = p * transpose(h) * inverse(h * p * transpose(h) + r)
+ *
+ * k = K(p)
+ */
+
+ /* 3x2 = 3x2 * 2x2 */
+ mat_t k = multiply(t0, t4);
+ if (debug)
+ print_mat("k", k);
+ n.k = k;
+
+ /* t5 = h * x */
+
+ /* 2 = 2x3 * 3 */
+ vec_t t5 = multiply_mat_vec(p.h, s.x);
+ if (debug)
+ print_vec("t5", t5);
+
+ /* t6 = z - h * x */
+
+ /* 2 = 2 - 2 */
+ vec_t t6 = vec_subtract(z, t5);
+ if (debug)
+ print_vec("t6", t6);
+
+ /* t7 = k * (z - h * x) */
+
+ /* 3 = 3x2 * 2 */
+ vec_t t7 = multiply_mat_vec(k, t6);
+ if (debug)
+ print_vec("t7", t7);
+
+ /* Correct state
+ *
+ * x: predicted state
+ * k: kalman value
+ * z: measurement
+ * h: state to measurement matrix
+ * x': corrected state
+ *
+ * x' = x + k * (z - h * x)
+ */
+
+ n.x = vec_add(s.x, t7);
+ if (debug)
+ print_vec("n->x", n.x);
+
+ /* t8 = k * h */
+
+ /* 3x3 = 3x2 * 2x3 */
+ mat_t t8 = multiply(k, p.h);
+ if (debug)
+ print_mat("t8", t8);
+
+ /* t9 = 1 - k * h */
+
+ /* 3x3 = 3x3 - 3x3 */
+ mat_t t9 = subtract(identity(dim(s.x)), t8);
+ if (debug)
+ print_mat("t9", t9);
+
+ /* Correct error
+ *
+ * p: predicted error
+ * k: kalman value
+ * h: state to measurement matrix
+ * p': corrected error
+ *
+ * p' = (1 - k * h) * p
+ *
+ * p' = P(k,p)
+ */
+
+ /* 3x3 = 3x3 * 3x3 */
+ n.p = multiply(t9, s.p);
+ if (debug) {
+ print_mat("n->p", n.p);
+# print_state("correct", n);
+ }
+ return n;
+ }
+
+ real distance(mat_t a, mat_t b) {
+ int[2] d = dims(a);
+ int i_max = d[0];
+ int j_max = d[1];
+ real s = 0;
+
+ for (int i = 0; i < i_max; i++)
+ for (int j = 0; j < j_max; j++)
+ s += (a[i,j] - b[i,j]) ** 2;
+ return sqrt(s);
+ }
+
+ public mat_t converge(parameters_t p) {
+ int model = dims(p.a)[0];
+ int measure = dims(p.r)[0];
+ int reps = 0;
+ state_t s = {
+ .x = (real[model]) { 0 ... },
+ .p = (real[model,model]) { { 0 ... } ... },
+ .k = (real[model,measure]) { { 0 ... } ... }
+ };
+
+ vec_t z = (real [measure]) { 0 ... };
+ for (;;) {
+ state_t s_pre = predict(s, p);
+ state_t s_post = correct(s_pre, z, p);
+ real d = distance(s.k, s_post.k);
+ s = s_post;
+ reps++;
+ if (d < 1e-10 && reps > 10)
+ break;
+ }
+ return s.k;
+ }
+
+ public parameters_fast_t convert_to_fast(parameters_t p) {
+ return (parameters_fast_t) {
+ .a = p.a, .k = converge(p), .h = p.h
+ };
+ }
+}
--- /dev/null
+/*
+ * Copyright © 2011 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.
+ */
+
+namespace load_csv {
+ string[*] parse_data(file f) {
+ while (!File::end(f)) {
+ string l = File::fgets(f);
+ if (l[0] == '#')
+ continue;
+ return String::parse_csv(l);
+ }
+ return (string[0]) {};
+ }
+
+ public typedef struct {
+ bool done;
+ real time;
+ real height;
+ real acceleration;
+ } record_t;
+
+ public record_t parse_record(file f, real accel_scale) {
+ string[*] data = parse_data(f);
+ if (dim(data) == 0)
+ return (record_t) { .done = true };
+ int time_off = 4;
+ int height_off = 11;
+ int accel_off = 8;
+ if (string_to_integer(data[0]) == 2) {
+ time_off = 4;
+ accel_off = 9;
+ height_off = 12;
+ }
+ 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) };
+ }
+
+ public void dump(file f) {
+ for (;;) {
+ record_t r = parse_record(f, 1);
+ if (r.done)
+ break;
+ printf ("%f %f %f\n", r.time, r.height, r.acceleration);
+ }
+ }
+}
--- /dev/null
+/*
+ * Copyright © 2011 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.
+ */
+
+namespace matrix {
+ public typedef real[*] vec_t;
+ public typedef real[*,*] mat_t;
+
+ public mat_t transpose(mat_t m) {
+ int[2] d = dims(m);
+ return (real[d[1],d[0]]) { [i,j] = m[j,i] };
+ }
+
+ public void print_mat(string name, mat_t m) {
+ int[2] d = dims(m);
+ printf ("%s {\n", name);
+ for (int y = 0; y < d[0]; y++) {
+ for (int x = 0; x < d[1]; x++)
+ printf (" %14.8f", m[y,x]);
+ printf ("\n");
+ }
+ printf ("}\n");
+ }
+
+ public void print_vec(string name, vec_t v) {
+ int d = dim(v);
+ printf ("%s {", name);
+ for (int x = 0; x < d; x++)
+ printf (" %14.8f", v[x]);
+ printf (" }\n");
+ }
+
+ public mat_t multiply(mat_t a, mat_t b) {
+ int[2] da = dims(a);
+ int[2] db = dims(b);
+
+ assert(da[1] == db[0], "invalid matrix dimensions");
+
+ real dot(int rx, int ry) {
+ real r = 0.0;
+ for (int i = 0; i < da[1]; i++)
+ r += a[ry, i] * b[i, rx];
+ return imprecise(r);
+ }
+
+ mat_t r = (real[da[0], db[1]]) { [ry,rx] = dot(rx,ry) };
+ return r;
+ }
+
+ public mat_t multiply_mat_val(mat_t m, real value) {
+ int[2] d = dims(m);
+ for (int j = 0; j < d[1]; j++)
+ for (int i = 0; i < d[0]; i++)
+ m[i,j] *= value;
+ return m;
+ }
+
+ public mat_t add(mat_t a, mat_t b) {
+ int[2] da = dims(a);
+ int[2] db = dims(b);
+
+ assert(da[0] == db[0] && da[1] == db[1], "mismatching dim in plus");
+ return (real[da[0], da[1]]) { [y,x] = a[y,x] + b[y,x] };
+ }
+
+ public mat_t subtract(mat_t a, mat_t b) {
+ int[2] da = dims(a);
+ int[2] db = dims(b);
+
+ assert(da[0] == db[0] && da[1] == db[1], "mismatching dim in minus");
+ return (real[da[0], da[1]]) { [y,x] = a[y,x] - b[y,x] };
+ }
+
+ public mat_t inverse(mat_t m) {
+ int[2] d = dims(m);
+
+ real[1,1] inverse_1(real[1,1] m) {
+ return (real[1,1]) { { 1/m[0,0] } };
+ }
+
+ if (d[0] == 1 && d[1] == 1)
+ return inverse_1(m);
+
+ real[2,2] inverse_2(real[2,2] m) {
+ real a = m[0,0], b = m[0,1];
+ real c = m[1,0], d = m[1,1];
+ real det = a * d - b * c;
+ return multiply_mat_val((real[2,2]) {
+ { d, -b }, { -c, a } }, 1/det);
+ }
+
+ if (d[0] == 2 && d[1] == 2)
+ return inverse_2(m);
+
+ real[3,3] inverse_3(real[3,3] m) {
+ real a = m[0,0], b = m[0,1], c = m[0, 2];
+ real d = m[1,0], e = m[1,1], f = m[1, 2];
+ real g = m[2,0], h = m[2,1], k = m[2, 2];
+ real Z = a*(e*k-f*h) + b*(f*g - d*k) + c*(d*h-e*g);
+ real A = (e*k-f*h), B = (c*h-b*k), C=(b*f-c*e);
+ real D = (f*g-d*k), E = (a*k-c*g), F=(c*d-a*f);
+ real G = (d*h-e*g), H = (b*g-a*h), K=(a*e-b*d);
+ return multiply_mat_val((real[3,3]) {
+ { A, B, C }, { D, E, F }, { G, H, K }},
+ 1/Z);
+ }
+
+ if (d[0] == 3 && d[1] == 3)
+ return inverse_3(m);
+ assert(false, "cannot invert %v\n", d);
+ return m;
+ }
+
+ public mat_t identity(int d) {
+ return (real[d,d]) { [i,j] = (i == j) ? 1 : 0 };
+ }
+
+ public vec_t vec_subtract(vec_t a, vec_t b) {
+ int da = dim(a);
+ int db = dim(b);
+
+ assert(da == db, "mismatching dim in minus");
+ return (real[da]) { [x] = a[x] - b[x] };
+ }
+
+ public vec_t vec_add(vec_t a, vec_t b) {
+ int da = dim(a);
+ int db = dim(b);
+
+ assert(da == db, "mismatching dim in plus");
+ return (real[da]) { [x] = a[x] + b[x] };
+ }
+
+ public vec_t multiply_vec_mat(vec_t v, mat_t m) {
+ mat_t r2 = matrix::multiply((real[dim(v),1]) { [y,x] = v[y] }, m);
+ return (real[dim(v)]) { [y] = r2[y,0] };
+ }
+
+ public vec_t multiply_mat_vec(mat_t m, vec_t v) {
+ mat_t r2 = matrix::multiply(m, (real[dim(v), 1]) { [y,x] = v[y] });
+ int[2] d = dims(m);
+ return (real[d[0]]) { [y] = r2[y,0] };
+ }
+}
--- /dev/null
+#!/bin/sh
+for i in "$@"; do
+gnuplot -p << EOF
+set title "$i"
+set ylabel "height (m)"
+set y2label "velocity (m/s), acceleration (m/s²)"
+set xlabel "time (s)"
+set xtics border out nomirror
+set ytics border out nomirror
+set y2tics border out nomirror
+plot "$i" using 1:3 with lines lt 1 axes x1y2 title "raw speed",\
+ "$i" using 1:4 with lines lt 1 axes x1y2 title "raw accel",\
+ "$i" using 1:6 with lines lt 2 axes x1y2 title "both speed",\
+ "$i" using 1:7 with lines lt 2 axes x1y2 title "both accel",\
+ "$i" using 1:9 with lines lt 3 axes x1y2 title "accel speed",\
+ "$i" using 1:10 with lines lt 3 axes x1y2 title "accel accel"
+EOF
+done
--- /dev/null
+#!/bin/sh
+for i in "$@"; do
+gnuplot -p << EOF
+set title "$i"
+set ylabel "height (m)"
+set y2label "velocity (m/s), acceleration (m/s²)"
+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 1 axes x1y1 title "raw height",\
+ "$i" using 1:3 with lines lt 1 axes x1y2 title "raw speed",\
+ "$i" using 1:4 with lines lt 1 axes x1y2 title "raw accel",\
+ "$i" using 1:5 with lines lt 2 axes x1y1 title "both height",\
+ "$i" using 1:6 with lines lt 2 axes x1y2 title "both vel",\
+ "$i" using 1:7 with lines lt 2 axes x1y2 title "both accel",\
+ "$i" using 1:8 with lines lt 3 axes x1y1 title "accel height",\
+ "$i" using 1:9 with lines lt 3 axes x1y2 title "accel vel",\
+ "$i" using 1:10 with lines lt 3 axes x1y2 title "accel accel",\
+ "$i" using 1:11 with lines lt 4 axes x1y1 title "baro height",\
+ "$i" using 1:12 with lines lt 4 axes x1y2 title "baro vel",\
+ "$i" using 1:13 with lines lt 4 axes x1y2 title "baro accel"
+EOF
+done
--- /dev/null
+#!/bin/sh
+
+cd ../kalman
+
+SIGMA_ACCEL_MODEL=1
+SIGMA_BARO_MEASURE=8
+SIGMA_ACCEL_MEASURE=4
+
+SIGMA="-M $SIGMA_ACCEL_MODEL -H $SIGMA_BARO_MEASURE -A $SIGMA_ACCEL_MEASURE"
+
+nickle kalman.5c -p AO_BOTH -c both -t 0.01 $SIGMA
+nickle kalman.5c -p AO_BOTH -c both -t 0.1 $SIGMA
+
+nickle kalman.5c -p AO_ACCEL -c accel -t 0.01 $SIGMA
+nickle kalman.5c -p AO_ACCEL -c accel -t 0.1 $SIGMA
+
+nickle kalman.5c -p AO_BARO -c baro -t 0.01 $SIGMA
+nickle kalman.5c -p AO_BARO -c baro -t 0.1 $SIGMA
projects / fw / altos / commitdiff