diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/Makefile.proto | 9 | ||||
| -rwxr-xr-x | src/kalman/kalman.5c | 493 | ||||
| -rw-r--r-- | src/kalman/kalman_filter.5c | 308 | ||||
| -rw-r--r-- | src/kalman/load_csv.5c | 63 | ||||
| -rw-r--r-- | src/kalman/matrix.5c | 157 | ||||
| -rw-r--r-- | src/kalman/plotaccel | 18 | ||||
| -rwxr-xr-x | src/kalman/plotkalman | 24 | ||||
| -rw-r--r-- | src/make-kalman | 18 |
8 files changed, 1090 insertions, 0 deletions
diff --git a/src/Makefile.proto b/src/Makefile.proto index 68fa4654..85c0c46e 100644 --- a/src/Makefile.proto +++ b/src/Makefile.proto @@ -5,6 +5,11 @@ 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 @@ -25,6 +30,7 @@ INC = \ ao_pins.h \ cc1111.h \ altitude.h \ + ao_kalman.h \ 25lc1024.h # @@ -307,6 +313,9 @@ all: ../$(PROG) ../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) > $@ diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c new file mode 100755 index 00000000..f7347184 --- /dev/null +++ b/src/kalman/kalman.5c @@ -0,0 +1,493 @@ +#!/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); diff --git a/src/kalman/kalman_filter.5c b/src/kalman/kalman_filter.5c new file mode 100644 index 00000000..efbbf1ab --- /dev/null +++ b/src/kalman/kalman_filter.5c @@ -0,0 +1,308 @@ +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 + }; + } +} diff --git a/src/kalman/load_csv.5c b/src/kalman/load_csv.5c new file mode 100644 index 00000000..15e83166 --- /dev/null +++ b/src/kalman/load_csv.5c @@ -0,0 +1,63 @@ +/* + * 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); + } + } +} diff --git a/src/kalman/matrix.5c b/src/kalman/matrix.5c new file mode 100644 index 00000000..667648f5 --- /dev/null +++ b/src/kalman/matrix.5c @@ -0,0 +1,157 @@ +/* + * 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] }; + } +} diff --git a/src/kalman/plotaccel b/src/kalman/plotaccel new file mode 100644 index 00000000..fd540203 --- /dev/null +++ b/src/kalman/plotaccel @@ -0,0 +1,18 @@ +#!/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 diff --git a/src/kalman/plotkalman b/src/kalman/plotkalman new file mode 100755 index 00000000..d2041dfb --- /dev/null +++ b/src/kalman/plotkalman @@ -0,0 +1,24 @@ +#!/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 diff --git a/src/make-kalman b/src/make-kalman new file mode 100644 index 00000000..80157c6b --- /dev/null +++ b/src/make-kalman @@ -0,0 +1,18 @@ +#!/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 |