git.gag.com Git - fw/altos/blob - src/kalman/kalman_micro.5c

   1 #!/usr/bin/env nickle
   2
   3 /*
   4  * Copyright © 2013 Keith Packard <keithp@keithp.com>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful, but
  12  * WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License along
  17  * with this program; if not, write to the Free Software Foundation, Inc.,
  18  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  19  */
  20
  21 autoimport ParseArgs;
  22
  23 load "load_csv.5c"
  24 import load_csv;
  25
  26 load "matrix.5c"
  27 import matrix;
  28
  29 load "kalman_filter.5c"
  30 import kalman;
  31
  32 load "../util/atmosphere.5c"
  33
  34 real    height_scale = 1.0;
  35 real    accel_scale = 1.0;
  36 real    speed_scale = 1.0;
  37
  38 /*
  39  * State:
  40  *
  41  * x[0] = pressure
  42  * x[1] = delta pressure
  43  * x[2] = delta delta pressure
  44  */
  45
  46 /*
  47  * Measurement
  48  *
  49  * z[0] = pressure
  50  */
  51
  52 real default_σ_m = 5;
  53 real default_σ_h = 2.4;        /* pascals */
  54
  55 real[3,3] model_error(t, Φ) = multiply_mat_val ((real[3,3]) {
  56                 { t**5 / 20, t**4 / 8, t**3 / 6 },
  57                 { t**4 /  8, t**3 / 3, t**2 / 2 },
  58                 { t**3 /  6, t**2 / 2, t }
  59         }, Φ);
  60
  61 parameters_t param_baro(real t, real σ_m, real σ_h) {
  62         if (σ_m == 0) {
  63                 printf ("Using default σ_m\n");
  64                 σ_m = default_σ_m;
  65         }
  66         if (σ_h == 0) {
  67                 printf ("Using default σ_h\n");
  68                 σ_h = default_σ_h;
  69         }
  70
  71         σ_m = imprecise(σ_m) * accel_scale;
  72         σ_h = imprecise(σ_h) * height_scale;
  73
  74         t = imprecise(t);
  75         return (parameters_t) {
  76 /*
  77  * Equation computing state k from state k-1
  78  *
  79  * height = height- + velocity- * t + acceleration- * t² / 2
  80  * velocity = velocity- + acceleration- * t
  81  * acceleration = acceleration-
  82  */
  83                 .a = (real[3,3]) {
  84                         { 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
  85                         { 0, 1, t * speed_scale / accel_scale },
  86                         { 0, 0, 1 }
  87                 },
  88 /*
  89  * Model error covariance. The only inaccuracy in the
  90  * model is the assumption that acceleration is constant
  91  */
  92                 .q = model_error (t, σ_m**2),
  93
  94 /*
  95  * Measurement error covariance
  96  * Our sensors are independent, so
  97  * this matrix is zero off-diagonal
  98  */
  99                 .r = (real[1,1]) {
 100                         { σ_h ** 2 },
 101                 },
 102 /*
 103  * Extract measurements from state,
 104  * this just pulls out the height
 105  * values.
 106  */
 107                 .h = (real[1,3]) {
 108                         { 1, 0, 0 },
 109                 },
 110          };
 111 }
 112
 113 bool    just_kalman = true;
 114 real    accel_input_scale = 1;
 115
 116 real    error_avg;
 117
 118 void update_error_avg(real e) {
 119         if (e < 0)
 120                 e = -e;
 121
 122 #       if (e > 1000)
 123 #               e = 1000;
 124
 125         error_avg -= error_avg / 8;
 126         error_avg += (e * e) / 8;
 127 }
 128
 129 void run_flight(string name, file f, bool summary, real σ_m, real σ_h) {
 130         state_t current_both = {
 131                 .x = (real[3]) { 0, 0, 0 },
 132                 .p = (real[3,3]) { { 0 ... } ... },
 133         };
 134         state_t current_accel = current_both;
 135         state_t current_baro = current_both;
 136         real    t;
 137         real    kalman_apogee_time = -1;
 138         real    kalman_apogee = 0;
 139         real    raw_apogee_time_first;
 140         real    raw_apogee_time_last;
 141         real    raw_apogee = 0;
 142         real    speed = 0;
 143         real    prev_acceleration = 0;
 144         real    height, max_height = 0;
 145         state_t apogee_state;
 146         parameters_fast_t       fast_baro;
 147         real                    fast_delta_t = 0;
 148         bool                    fast = true;
 149         int                     speed_lock = 0;
 150
 151         error_avg = 0;
 152         for (;;) {
 153                 record_t        record = parse_record(f, 1.0);
 154                 if (record.done)
 155                         break;
 156                 if (is_uninit(&t))
 157                         t = record.time;
 158                 real delta_t = record.time - t;
 159                 if (delta_t < 0.096)
 160                         continue;
 161 #               delta_t = 0.096;        /* pretend that we're getting micropeak-rate data */
 162 #               record.time = record.time + delta_t;
 163                 t = record.time;
 164                 if (record.height > raw_apogee) {
 165                         raw_apogee_time_first = record.time;
 166                         raw_apogee = record.height;
 167                 }
 168                 if (record.height == raw_apogee)
 169                         raw_apogee_time_last = record.time;
 170
 171                 real    pressure = record.pressure;
 172
 173                 if (current_baro.x[0] == 0)
 174                         current_baro.x[0] = pressure;
 175
 176                 vec_t z_baro = (real[1]) { record.pressure * height_scale };
 177
 178                 real    error_h;
 179
 180                 if (fast) {
 181                         if (delta_t != fast_delta_t) {
 182                                 fast_baro = convert_to_fast(param_baro(delta_t, σ_m, σ_h));
 183                                 fast_delta_t = delta_t;
 184                         }
 185
 186                         current_baro.x = predict_fast(current_baro.x, fast_baro);
 187                         error_h = current_baro.x[0] - pressure;
 188                         current_baro.x = correct_fast(current_baro.x, z_baro, fast_baro);
 189                 } else {
 190                         parameters_t p_baro = param_baro(delta_t, σ_m, σ_h);
 191
 192                         state_t pred_baro = predict(current_baro, p_baro);
 193                         error_h = current_baro.x[0] - pressure;
 194                         state_t next_baro = correct(pred_baro, z_baro, p_baro);
 195                         current_baro = next_baro;
 196                 }
 197
 198                 update_error_avg(error_h);
 199
 200                 /* Don't check for maximum altitude if we're accelerating upwards */
 201                 if (current_baro.x[2] / accel_scale < -2 * σ_m)
 202                         speed_lock = 10;
 203                 else if (speed_lock > 0)
 204                         speed_lock--;
 205
 206                 height = pressure_to_altitude(current_baro.x[0] / height_scale);
 207                 if (speed_lock == 0 && height > max_height)
 208                         max_height = height;
 209
 210                 printf ("%16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %d %d\n",
 211                         record.time,
 212                         record.pressure,
 213                         pressure_to_altitude(record.pressure),
 214                         current_baro.x[0] / height_scale,
 215                         current_baro.x[1] / speed_scale,
 216                         current_baro.x[2] / accel_scale,
 217                         height,
 218                         max_height,
 219                         error_h,
 220                         error_avg,
 221                         error_avg > 50000 ? 0 : 95000,
 222                         speed_lock > 0 ? 0 : 4500);
 223
 224                 if (kalman_apogee_time < 0) {
 225                         if (current_baro.x[1] > 1) {
 226                                 kalman_apogee = current_both.x[0];
 227                                 kalman_apogee_time = record.time;
 228                         }
 229                 }
 230         }
 231         real raw_apogee_time = (raw_apogee_time_last + raw_apogee_time_first) / 2;
 232         if (summary && !just_kalman) {
 233                 printf("%s: kalman (%8.2f m %6.2f s) raw (%8.2f m %6.2f s) error %6.2f s\n",
 234                        name,
 235                        kalman_apogee, kalman_apogee_time,
 236                        raw_apogee, raw_apogee_time,
 237                        kalman_apogee_time - raw_apogee_time);
 238         }
 239 }
 240
 241 void main() {
 242         bool    summary = false;
 243         int     user_argind = 1;
 244         real    time_step = 0.01;
 245         string  compute = "none";
 246         string  prefix = "AO_K";
 247         real    σ_m = default_σ_m;
 248         real    σ_h = default_σ_h;
 249
 250         ParseArgs::argdesc argd = {
 251                 .args = {
 252                         { .var = { .arg_flag = &summary },
 253                           .abbr = 's',
 254                           .name = "summary",
 255                           .desc = "Print a summary of the flight" },
 256                         { .var = { .arg_real = &time_step, },
 257                           .abbr = 't',
 258                           .name = "time",
 259                           .expr_name = "<time-step>",
 260                           .desc = "Set time step for convergence" },
 261                         { .var = { .arg_string = &prefix },
 262                           .abbr = 'p',
 263                           .name = "prefix",
 264                           .expr_name = "<prefix>",
 265                           .desc = "Prefix for compute output" },
 266                         { .var = { .arg_string = &compute },
 267                           .abbr = 'c',
 268                           .name = "compute",
 269                           .expr_name = "{baro,none}",
 270                           .desc = "Compute Kalman factor through convergence" },
 271                         { .var = { .arg_real = &σ_m },
 272                           .abbr = 'M',
 273                           .name = "model",
 274                           .expr_name = "<model-accel-error>",
 275                           .desc = "Model co-variance for acceleration" },
 276                         { .var = { .arg_real = &σ_h },
 277                           .abbr = 'H',
 278                           .name = "height",
 279                           .expr_name = "<measure-height-error>",
 280                           .desc = "Measure co-variance for height" },
 281                 },
 282
 283                 .unknown = &user_argind,
 284         };
 285
 286         ParseArgs::parseargs(&argd, &argv);
 287
 288         if (compute != "none") {
 289                 parameters_t    param;
 290
 291                 printf ("/* Kalman matrix for micro %s\n", compute);
 292                 printf (" *     step = %f\n", time_step);
 293                 printf (" *     σ_m = %f\n", σ_m);
 294                 switch (compute) {
 295                 case "baro":
 296                         printf (" *     σ_h = %f\n", σ_h);
 297                         param = param_baro(time_step, σ_m, σ_h);
 298                         break;
 299                 }
 300                 printf (" */\n\n");
 301                 mat_t k = converge(param);
 302                 int[] d = dims(k);
 303                 int time_inc = floor(1/time_step + 0.5);
 304                 for (int i = 0; i < d[0]; i++)
 305                         for (int j = 0; j < d[1]; j++) {
 306                                 string name;
 307                                 if (d[1] == 1)
 308                                         name = sprintf("%s_K%d_%d", prefix, i, time_inc);
 309                                 else
 310                                         name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
 311                                 printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 312                         }
 313                 printf ("\n");
 314                 exit(0);
 315         }
 316         string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] };
 317
 318         #       height_scale = accel_scale = speed_scale = 1;
 319
 320         if (dim(rest) == 0)
 321                 run_flight("<stdin>", stdin, summary, σ_m, σ_h);
 322         else {
 323                 for (int i = 0; i < dim(rest); i++) {
 324                         twixt(file f = File::open(rest[i], "r"); File::close(f)) {
 325                                 run_flight(rest[i], f, summary, σ_m, σ_h);
 326                         }
 327                 }
 328         }
 329 }
 330 main();