From 20427ae4965f756aac0cedc5179a1c45b9a781f2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 21 Mar 2011 19:59:27 +0900 Subject: altos: Add nickle kalman implementation. This generates the constants needed to implement Kalman filtering in the flight firmware. Signed-off-by: Keith Packard --- src/kalman/kalman.5c | 493 ++++++++++++++++++++++++++++++++++++++++++++ src/kalman/kalman_filter.5c | 308 +++++++++++++++++++++++++++ src/kalman/load_csv.5c | 63 ++++++ src/kalman/matrix.5c | 157 ++++++++++++++ src/kalman/plotaccel | 18 ++ src/kalman/plotkalman | 24 +++ 6 files changed, 1063 insertions(+) create mode 100755 src/kalman/kalman.5c create mode 100644 src/kalman/kalman_filter.5c create mode 100644 src/kalman/load_csv.5c create mode 100644 src/kalman/matrix.5c create mode 100644 src/kalman/plotaccel create mode 100755 src/kalman/plotkalman (limited to 'src/kalman') 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 + * + * 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 = "", + .desc = "Set accelerometer scale factor" }, + { .var = { .arg_real = &time_step, }, + .abbr = 't', + .name = "time", + .expr_name = "", + .desc = "Set time step for convergence" }, + { .var = { .arg_string = &prefix }, + .abbr = 'p', + .name = "prefix", + .expr_name = "", + .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 = "", + .desc = "Model co-variance for acceleration" }, + { .var = { .arg_real = &σ_h }, + .abbr = 'H', + .name = "height", + .expr_name = "", + .desc = "Measure co-variance for height" }, + { .var = { .arg_real = &σ_a }, + .abbr = 'A', + .name = "accel", + .expr_name = "", + .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, 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 + * + * 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 + * + * 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 + * + * 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 -- cgit v1.2.3 From 6864e06d88a5b908cffa7c4cd2be8969ff46ce4d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 22 Mar 2011 16:51:04 +0900 Subject: altos/kalman: Kalman terms can be > 1, use 32-bit fixed point Because speed and acceleration are scaled by 16, it's fairly common for the kalman terms to end up larger than 1. Instead of trying to fuss with 16-bit values and shifts, just use 32-bit values. Signed-off-by: Keith Packard --- src/kalman/kalman.5c | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) (limited to 'src/kalman') diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c index f7347184..cfb7abea 100755 --- a/src/kalman/kalman.5c +++ b/src/kalman/kalman.5c @@ -460,19 +460,17 @@ void main() { 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]); + 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"); - return; + exit(0); } string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] }; -- cgit v1.2.3