4 * Copyright © 2011 Keith Packard <keithp@keithp.com>
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.
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.
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.
29 load "kalman_filter.5c"
33 * AltOS keeps speed and accel scaled
34 * by 4 bits to provide additional precision
36 real height_scale = 1.0;
37 real accel_scale = 16.0;
38 real speed_scale = 16.0;
56 real default_σ_h = 20;
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 }
65 parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) {
73 σ_m = imprecise(σ_m) * accel_scale;
74 σ_h = imprecise(σ_h) * height_scale;
75 σ_a = imprecise(σ_a) * accel_scale;
79 return (parameters_t) {
81 * Equation computing state k from state k-1
83 * height = height- + velocity- * t + acceleration- * t² / 2
84 * velocity = velocity- + acceleration- * t
85 * acceleration = acceleration-
89 t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
90 { 0, 1, t * speed_scale / accel_scale },
94 * Model error covariance. The only inaccuracy in the
95 * model is the assumption that acceleration is constant
97 .q = model_error (t, σ_m**2),
99 * Measurement error covariance
100 * Our sensors are independent, so
101 * this matrix is zero off-diagonal
108 * Extract measurements from state,
109 * this just pulls out the height and acceleration
119 parameters_t param_baro(real t, real σ_m, real σ_h) {
125 σ_m = imprecise(σ_m) * accel_scale;
126 σ_h = imprecise(σ_h) * height_scale;
129 return (parameters_t) {
131 * Equation computing state k from state k-1
133 * height = height- + velocity- * t + acceleration- * t² / 2
134 * velocity = velocity- + acceleration- * t
135 * acceleration = acceleration-
138 { 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
139 { 0, 1, t * speed_scale / accel_scale },
143 * Model error covariance. The only inaccuracy in the
144 * model is the assumption that acceleration is constant
146 .q = model_error (t, σ_m**2),
148 * Measurement error covariance
149 * Our sensors are independent, so
150 * this matrix is zero off-diagonal
156 * Extract measurements from state,
157 * this just pulls out the height
166 parameters_t param_accel(real t, real σ_m, real σ_a) {
172 σ_m = imprecise(σ_m) * accel_scale;
173 σ_a = imprecise(σ_a) * accel_scale;
176 return (parameters_t) {
178 * Equation computing state k from state k-1
180 * height = height- + velocity- * t + acceleration- * t² / 2
181 * velocity = velocity- + acceleration- * t
182 * acceleration = acceleration-
185 { 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale },
186 { 0, 1, t * speed_scale / accel_scale },
190 * Model error covariance. The only inaccuracy in the
191 * model is the assumption that acceleration is constant
193 .q = model_error (t, σ_m**2),
195 * Measurement error covariance
196 * Our sensors are independent, so
197 * this matrix is zero off-diagonal
203 * Extract measurements from state,
204 * this just pulls out the acceleration
213 parameters_t param_vel(real t) {
214 static real σ_m = .1;
215 static real σ_v = imprecise(10);
217 return (parameters_t) {
219 * Equation computing state k from state k-1
221 * height = height- + velocity- * t + acceleration- * t² / 2
222 * velocity = velocity- + acceleration- * t
223 * acceleration = acceleration-
226 { 1, imprecise(t), imprecise((t**2)/2) },
227 { 0, 1, imprecise(t) },
231 * Model error covariance. The only inaccuracy in the
232 * model is the assumption that acceleration is constant
234 .q = model_error (t, σ_m**2),
236 * Measurement error covariance
237 * Our sensors are independent, so
238 * this matrix is zero off-diagonal
244 * Extract measurements from state,
245 * this just pulls out the velocity
254 real max_baro_height = 18000;
256 bool just_kalman = true;
257 real accel_input_scale = 1;
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 ... } ... },
264 state_t current_accel = current_both;
265 state_t current_baro = current_both;
267 real kalman_apogee_time = -1;
268 real kalman_apogee = 0;
269 real raw_apogee_time_first;
270 real raw_apogee_time_last;
272 real default_descent_rate = 20;
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;
283 record_t record = parse_record(f, accel_input_scale);
288 real delta_t = record.time - t;
292 if (record.height > raw_apogee) {
293 raw_apogee_time_first = record.time;
294 raw_apogee = record.height;
296 if (record.height == raw_apogee)
297 raw_apogee_time_last = record.time;
299 real acceleration = record.acceleration;
300 real height = record.height;
302 speed = (speed + (acceleration + prev_acceleration / 2) * delta_t);
303 prev_acceleration = acceleration;
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 };
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;
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);
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);
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);
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);
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;
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",
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;
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",
360 kalman_apogee, kalman_apogee_time,
361 raw_apogee, raw_apogee_time,
362 kalman_apogee_time - raw_apogee_time);
367 bool summary = false;
369 real time_step = 0.01;
370 string compute = "none";
371 string prefix = "AO_K";
376 ParseArgs::argdesc argd = {
378 { .var = { .arg_flag = &summary },
381 .desc = "Print a summary of the flight" },
382 { .var = { .arg_real = &max_baro_height },
385 .expr_name = "height",
386 .desc = "Set maximum usable barometer height" },
387 { .var = { .arg_real = &accel_input_scale, },
390 .expr_name = "<accel-scale>",
391 .desc = "Set accelerometer scale factor" },
392 { .var = { .arg_real = &time_step, },
395 .expr_name = "<time-step>",
396 .desc = "Set time step for convergence" },
397 { .var = { .arg_string = &prefix },
400 .expr_name = "<prefix>",
401 .desc = "Prefix for compute output" },
402 { .var = { .arg_string = &compute },
405 .expr_name = "{both,baro,accel}",
406 .desc = "Compute Kalman factor through convergence" },
407 { .var = { .arg_real = &σ_m },
410 .expr_name = "<model-accel-error>",
411 .desc = "Model co-variance for acceleration" },
412 { .var = { .arg_real = &σ_h },
415 .expr_name = "<measure-height-error>",
416 .desc = "Measure co-variance for height" },
417 { .var = { .arg_real = &σ_a },
420 .expr_name = "<measure-accel-error>",
421 .desc = "Measure co-variance for acceleration" },
424 .unknown = &user_argind,
427 ParseArgs::parseargs(&argd, &argv);
429 if (compute != "none") {
432 printf ("/* Kalman matrix for %s\n", compute);
433 printf (" * step = %f\n", time_step);
434 printf (" * σ_m = %f\n", σ_m);
437 printf (" * σ_h = %f\n", σ_h);
438 printf (" * σ_a = %f\n", σ_a);
439 param = param_both(time_step, σ_m, σ_h, σ_a);
442 printf (" * σ_a = %f\n", σ_a);
443 param = param_accel(time_step, σ_m, σ_a);
446 printf (" * σ_h = %f\n", σ_h);
447 param = param_baro(time_step, σ_m, σ_h);
451 mat_t k = converge(param);
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++) {
458 name = sprintf("%s_K%d_%d", prefix, i, time_inc);
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]);
466 string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] };
468 # height_scale = accel_scale = speed_scale = 1;
471 run_flight("<stdin>", stdin, summary);
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);