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

   1 #!/usr/bin/env nickle
   2
   3 /*
   4  * Copyright © 2011 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 /*
  33  * AltOS keeps speed and accel scaled
  34  * by 4 bits to provide additional precision
  35  */
  36 real    height_scale = 1.0;
  37 real    accel_scale = 16.0;
  38 real    speed_scale = 16.0;
  39
  40 /*
  41  * State:
  42  *
  43  * x[0] = height
  44  * x[1] = velocity
  45  * x[2] = acceleration
  46  */
  47
  48 /*
  49  * Measurement
  50  *
  51  * z[0] = height
  52  * z[1] = acceleration
  53  */
  54
  55 real default_σ_m = 5;
  56 real default_σ_h = 20;
  57 real default_σ_a = 2;
  58
  59 real[3,3] model_error(t, Φ) = multiply_mat_val ((real[3,3]) {
  60                 { t**5 / 20, t**4 / 8, t**3 / 6 },
  61                 { t**4 /  8, t**3 / 3, t**2 / 2 },
  62                 { t**3 /  6, t**2 / 2, t }
  63         }, Φ);
  64
  65 parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
  66         if (σ_m == 0)
  67                 σ_m = default_σ_m;
  68         if (σ_h == 0)
  69                 σ_h = default_σ_h;
  70         if (σ_a == 0)
  71                 σ_a = default_σ_a;
  72
  73         σ_m = imprecise(σ_m) * accel_scale;
  74         σ_h = imprecise(σ_h) * height_scale;
  75         σ_a = imprecise(σ_a) * accel_scale;
  76
  77         t = imprecise(t);
  78
  79         return (parameters_t) {
  80 /*
  81  * Equation computing state k from state k-1
  82  *
  83  * height = height- + velocity- * t + acceleration- * t² / 2
  84  * velocity = velocity- + acceleration- * t
  85  * acceleration = acceleration-
  86  */
  87                 .a = (real[3,3]) {
  88                         { 1,
  89                           t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
  90                         { 0, 1, t * speed_scale / accel_scale },
  91                         { 0, 0, 1 }
  92                 },
  93 /*
  94  * Model error covariance. The only inaccuracy in the
  95  * model is the assumption that acceleration is constant
  96  */
  97                 .q = model_error (t, σ_m**2),
  98 /*
  99  * Measurement error covariance
 100  * Our sensors are independent, so
 101  * this matrix is zero off-diagonal
 102  */
 103                 .r = (real[2,2]) {
 104                         { σ_h ** 2, 0 },
 105                         { 0, σ_a ** 2 },
 106                 },
 107 /*
 108  * Extract measurements from state,
 109  * this just pulls out the height and acceleration
 110  * values.
 111  */
 112                 .h = (real[2,3]) {
 113                         { 1, 0, 0 },
 114                         { 0, 0, 1 },
 115                 },
 116          };
 117 }
 118
 119 parameters_t param_baro(real t, real σ_m, real σ_h) {
 120         if (σ_m == 0)
 121                 σ_m = default_σ_m;
 122         if (σ_h == 0)
 123                 σ_h = default_σ_h;
 124
 125         σ_m = imprecise(σ_m) * accel_scale;
 126         σ_h = imprecise(σ_h) * height_scale;
 127
 128         t = imprecise(t);
 129         return (parameters_t) {
 130 /*
 131  * Equation computing state k from state k-1
 132  *
 133  * height = height- + velocity- * t + acceleration- * t² / 2
 134  * velocity = velocity- + acceleration- * t
 135  * acceleration = acceleration-
 136  */
 137                 .a = (real[3,3]) {
 138                         { 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
 139                         { 0, 1, t * speed_scale / accel_scale },
 140                         { 0, 0, 1 }
 141                 },
 142 /*
 143  * Model error covariance. The only inaccuracy in the
 144  * model is the assumption that acceleration is constant
 145  */
 146                 .q = model_error (t, σ_m**2),
 147 /*
 148  * Measurement error covariance
 149  * Our sensors are independent, so
 150  * this matrix is zero off-diagonal
 151  */
 152                 .r = (real[1,1]) {
 153                         { σ_h ** 2 },
 154                 },
 155 /*
 156  * Extract measurements from state,
 157  * this just pulls out the height
 158  * values.
 159  */
 160                 .h = (real[1,3]) {
 161                         { 1, 0, 0 },
 162                 },
 163          };
 164 }
 165
 166 parameters_t param_accel(real t, real σ_m, real σ_a) {
 167         if (σ_m == 0)
 168                 σ_m = default_σ_m;
 169         if (σ_a == 0)
 170                 σ_a = default_σ_a;
 171
 172         σ_m = imprecise(σ_m) * accel_scale;
 173         σ_a = imprecise(σ_a) * accel_scale;
 174
 175         t = imprecise(t);
 176         return (parameters_t) {
 177 /*
 178  * Equation computing state k from state k-1
 179  *
 180  * height = height- + velocity- * t + acceleration- * t² / 2
 181  * velocity = velocity- + acceleration- * t
 182  * acceleration = acceleration-
 183  */
 184                 .a = (real[3,3]) {
 185                         { 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
 186                         { 0, 1, t * speed_scale / accel_scale },
 187                         { 0, 0, 1 }
 188                 },
 189 /*
 190  * Model error covariance. The only inaccuracy in the
 191  * model is the assumption that acceleration is constant
 192  */
 193                 .q = model_error (t, σ_m**2),
 194 /*
 195  * Measurement error covariance
 196  * Our sensors are independent, so
 197  * this matrix is zero off-diagonal
 198  */
 199                 .r = (real[1,1]) {
 200                         { σ_a ** 2 },
 201                 },
 202 /*
 203  * Extract measurements from state,
 204  * this just pulls out the acceleration
 205  * values.
 206  */
 207                 .h = (real[1,3]) {
 208                         { 0, 0, 1 },
 209                 },
 210          };
 211 }
 212
 213 parameters_t param_vel(real t) {
 214         static real σ_m = .1;
 215         static real σ_v = imprecise(10);
 216
 217         return (parameters_t) {
 218 /*
 219  * Equation computing state k from state k-1
 220  *
 221  * height = height- + velocity- * t + acceleration- * t² / 2
 222  * velocity = velocity- + acceleration- * t
 223  * acceleration = acceleration-
 224  */
 225                 .a = (real[3,3]) {
 226                         { 1, imprecise(t), imprecise((t**2)/2) },
 227                         { 0, 1, imprecise(t) },
 228                         { 0, 0, 1 }
 229                 },
 230 /*
 231  * Model error covariance. The only inaccuracy in the
 232  * model is the assumption that acceleration is constant
 233  */
 234                 .q = model_error (t, σ_m**2),
 235 /*
 236  * Measurement error covariance
 237  * Our sensors are independent, so
 238  * this matrix is zero off-diagonal
 239  */
 240                 .r = (real[1,1]) {
 241                         { σ_v ** 2 },
 242                 },
 243 /*
 244  * Extract measurements from state,
 245  * this just pulls out the velocity
 246  * values.
 247  */
 248                 .h = (real[1,3]) {
 249                         { 0, 1, 0 },
 250                 },
 251          };
 252 }
 253
 254 real    max_baro_height = 18000;
 255
 256 bool    just_kalman = true;
 257 real    accel_input_scale = 1;
 258
 259 void run_flight(string name, file f, bool summary) {
 260         state_t current_both = {
 261                 .x = (real[3]) { 0, 0, 0 },
 262                 .p = (real[3,3]) { { 0 ... } ... },
 263         };
 264         state_t current_accel = current_both;
 265         state_t current_baro = current_both;
 266         real    t;
 267         real    kalman_apogee_time = -1;
 268         real    kalman_apogee = 0;
 269         real    raw_apogee_time_first;
 270         real    raw_apogee_time_last;
 271         real    raw_apogee = 0;
 272         real    default_descent_rate = 20;
 273         real    speed = 0;
 274         real    prev_acceleration = 0;
 275         state_t apogee_state;
 276         parameters_fast_t       fast_both;
 277         parameters_fast_t       fast_baro;
 278         parameters_fast_t       fast_accel;
 279         real                    fast_delta_t = 0;
 280         bool                    fast = true;
 281
 282         for (;;) {
 283                 record_t        record = parse_record(f, accel_input_scale);
 284                 if (record.done)
 285                         break;
 286                 if (is_uninit(&t))
 287                         t = record.time;
 288                 real delta_t = record.time - t;
 289                 if (delta_t <= 0)
 290                         continue;
 291                 t = record.time;
 292                 if (record.height > raw_apogee) {
 293                         raw_apogee_time_first = record.time;
 294                         raw_apogee = record.height;
 295                 }
 296                 if (record.height == raw_apogee)
 297                         raw_apogee_time_last = record.time;
 298
 299                 real    acceleration = record.acceleration;
 300                 real    height = record.height;
 301
 302                 speed = (speed + (acceleration + prev_acceleration / 2) * delta_t);
 303                 prev_acceleration = acceleration;
 304
 305                 vec_t z_both = (real[2]) { record.height * height_scale,  record.acceleration * accel_scale };
 306                 vec_t z_accel = (real[1]) { record.acceleration * accel_scale };
 307                 vec_t z_baro = (real[1]) { record.height * height_scale };
 308
 309
 310                 if (fast) {
 311                         if (delta_t != fast_delta_t) {
 312                                 fast_both = convert_to_fast(param_both(delta_t, 0, 0, 0));
 313                                 fast_accel = convert_to_fast(param_accel(delta_t, 0, 0));
 314                                 fast_baro = convert_to_fast(param_baro(delta_t, 0, 0));
 315                                 fast_delta_t = delta_t;
 316                         }
 317
 318                         current_both.x = predict_fast(current_both.x, fast_both);
 319                         current_accel.x = predict_fast(current_accel.x, fast_accel);
 320                         current_baro.x = predict_fast(current_baro.x, fast_baro);
 321
 322                         current_both.x = correct_fast(current_both.x, z_both, fast_both);
 323                         current_accel.x = correct_fast(current_accel.x, z_accel, fast_accel);
 324                         current_baro.x = correct_fast(current_baro.x, z_baro, fast_baro);
 325                 } else {
 326                         parameters_t p_both = param_both(delta_t, 0, 0, 0);
 327                         parameters_t p_accel = param_accel(delta_t, 0, 0);
 328                         parameters_t p_baro = param_baro(delta_t, 0, 0);
 329
 330                         state_t pred_both = predict(current_both, p_both);
 331                         state_t pred_accel = predict(current_accel, p_accel);
 332                         state_t pred_baro = predict(current_baro, p_baro);
 333
 334                         state_t next_both = correct(pred_both, z_both, p_both);
 335                         state_t next_accel = correct(pred_accel, z_accel, p_accel);
 336                         state_t next_baro = correct(pred_baro, z_baro, p_baro);
 337                         current_both = next_both;
 338                         current_accel = next_accel;
 339                         current_baro = next_baro;
 340                 }
 341
 342                 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",
 343                         record.time,
 344                         record.height, speed, record.acceleration,
 345                         current_both.x[0] / height_scale, current_both.x[1] / speed_scale, current_both.x[2] / accel_scale,
 346                         current_accel.x[0] / height_scale, current_accel.x[1] / speed_scale, current_accel.x[2] / accel_scale,
 347                         current_baro.x[0] / height_scale, current_baro.x[1] / speed_scale, current_baro.x[2] / accel_scale);
 348                 if (kalman_apogee_time < 0) {
 349                         if (current_both.x[1] < -1 && current_accel.x[1] < -1 && current_baro.x[1] < -1) {
 350                                 kalman_apogee = current_both.x[0];
 351                                 kalman_apogee_time = record.time;
 352                                 break;
 353                         }
 354                 }
 355         }
 356         real raw_apogee_time = (raw_apogee_time_last + raw_apogee_time_first) / 2;
 357         if (summary && !just_kalman) {
 358                 printf("%s: kalman (%8.2f m %6.2f s) raw (%8.2f m %6.2f s) error %6.2f s\n",
 359                        name,
 360                        kalman_apogee, kalman_apogee_time,
 361                        raw_apogee, raw_apogee_time,
 362                        kalman_apogee_time - raw_apogee_time);
 363         }
 364 }
 365
 366 void main() {
 367         bool    summary = false;
 368         int     user_argind = 1;
 369         real    time_step = 0.01;
 370         string  compute = "none";
 371         string  prefix = "AO_K";
 372         real    σ_m = 1;
 373         real    σ_h = 4;
 374         real    σ_a = 1;
 375
 376         ParseArgs::argdesc argd = {
 377                 .args = {
 378                         { .var = { .arg_flag = &summary },
 379                           .abbr = 's',
 380                           .name = "summary",
 381                           .desc = "Print a summary of the flight" },
 382                         { .var = { .arg_real = &max_baro_height },
 383                           .abbr = 'm',
 384                           .name = "maxbaro",
 385                           .expr_name = "height",
 386                           .desc = "Set maximum usable barometer height" },
 387                         { .var = { .arg_real = &accel_input_scale, },
 388                           .abbr = 'a',
 389                           .name = "accel",
 390                           .expr_name = "<accel-scale>",
 391                           .desc = "Set accelerometer scale factor" },
 392                         { .var = { .arg_real = &time_step, },
 393                           .abbr = 't',
 394                           .name = "time",
 395                           .expr_name = "<time-step>",
 396                           .desc = "Set time step for convergence" },
 397                         { .var = { .arg_string = &prefix },
 398                           .abbr = 'p',
 399                           .name = "prefix",
 400                           .expr_name = "<prefix>",
 401                           .desc = "Prefix for compute output" },
 402                         { .var = { .arg_string = &compute },
 403                           .abbr = 'c',
 404                           .name = "compute",
 405                           .expr_name = "{both,baro,accel}",
 406                           .desc = "Compute Kalman factor through convergence" },
 407                         { .var = { .arg_real = &σ_m },
 408                           .abbr = 'M',
 409                           .name = "model",
 410                           .expr_name = "<model-accel-error>",
 411                           .desc = "Model co-variance for acceleration" },
 412                         { .var = { .arg_real = &σ_h },
 413                           .abbr = 'H',
 414                           .name = "height",
 415                           .expr_name = "<measure-height-error>",
 416                           .desc = "Measure co-variance for height" },
 417                         { .var = { .arg_real = &σ_a },
 418                           .abbr = 'A',
 419                           .name = "accel",
 420                           .expr_name = "<measure-accel-error>",
 421                           .desc = "Measure co-variance for acceleration" },
 422                 },
 423
 424                 .unknown = &user_argind,
 425         };
 426
 427         ParseArgs::parseargs(&argd, &argv);
 428
 429         if (compute != "none") {
 430                 parameters_t    param;
 431
 432                 printf ("/* Kalman matrix for %s\n", compute);
 433                 printf (" *     step = %f\n", time_step);
 434                 printf (" *     σ_m = %f\n", σ_m);
 435                 switch (compute) {
 436                 case "both":
 437                         printf (" *     σ_h = %f\n", σ_h);
 438                         printf (" *     σ_a = %f\n", σ_a);
 439                         param = param_both(time_step, σ_m, σ_h, σ_a);
 440                         break;
 441                 case "accel":
 442                         printf (" *     σ_a = %f\n", σ_a);
 443                         param = param_accel(time_step, σ_m, σ_a);
 444                         break;
 445                 case "baro":
 446                         printf (" *     σ_h = %f\n", σ_h);
 447                         param = param_baro(time_step, σ_m, σ_h);
 448                         break;
 449                 }
 450                 printf (" */\n\n");
 451                 mat_t k = converge(param);
 452                 int[] d = dims(k);
 453                 int time_inc = floor(1/time_step + 0.5);
 454                 for (int i = 0; i < d[0]; i++)
 455                         for (int j = 0; j < d[1]; j++) {
 456                                 string name;
 457                                 if (d[1] == 1)
 458                                         name = sprintf("%s_K%d_%d", prefix, i, time_inc);
 459                                 else
 460                                         name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
 461                                 printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 462                         }
 463                 printf ("\n");
 464                 exit(0);
 465         }
 466         string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] };
 467
 468         #       height_scale = accel_scale = speed_scale = 1;
 469
 470         if (dim(rest) == 0)
 471                 run_flight("<stdin>", stdin, summary);
 472         else {
 473                 for (int i = 0; i < dim(rest); i++) {
 474                         twixt(file f = File::open(rest[i], "r"); File::close(f)) {
 475                                 run_flight(rest[i], f, summary);
 476                         }
 477                 }
 478         }
 479 }
 480 main();
 481 #kalman(stdin);
 482 #dump(stdin);