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