int main() { struct State current_state; initialize_state(¤t_state); kalman_filter(¤t_state); return 0; }
int main(int argc, char **argv) { Args args(argc, argv, "FILE"); const auto path = args.ExpectNextPath(); args.ExpectEnd(); Error error; FileLineReaderA reader(path, error); if (reader.error()) { fprintf(stderr, "%s\n", error.GetMessage()); return EXIT_FAILURE; } KalmanFilter1d kalman_filter(0.0075); unsigned last_t = 0; double last_value; const char *line; while ((line = reader.ReadLine()) != nullptr) { const char *p = line; char *endptr; unsigned t = strtoul(p, &endptr, 10); if (endptr == line) { fprintf(stderr, "Malformed line: %s\n", line); return EXIT_FAILURE; } p = endptr; double value = strtod(p, &endptr); if (endptr == line) { fprintf(stderr, "Malformed line: %s\n", line); return EXIT_FAILURE; } if (last_t > 0 && t > last_t) { auto dt = (t - last_t) / 1000.; kalman_filter.Update(value, 0.05, dt); printf("%u %f %f %f %f\n", t, value, (value - last_value) / dt, kalman_filter.GetXAbs(), kalman_filter.GetXVel()); } last_t = t; last_value = value; } return EXIT_SUCCESS; }
int main(int argc, char **argv) { int ret = 1; const char *input_data = "/dev/random", *output_data = NULL; int count = 0; struct kalman_filter_state k_state; unsigned int *array_in, *array_out; int opt; const struct option options[] = { {"control-input", required_argument, NULL, 'B'}, {"state-transition", required_argument, NULL, 'F'}, {"observation", required_argument, NULL, 'H'}, {"process-noise-cov", required_argument, NULL, 'Q'}, {"meas-noise-cov", required_argument, NULL, 'R'}, {"count", required_argument, NULL, 'c'}, {"input-data", required_argument, NULL, 'i'}, {"output-data", required_argument, NULL, 'o'}, {0, 0, 0, 0} }; memset(&k_state, 0x0, sizeof(k_state)); k_state.I = 1; while ((opt = getopt_long(argc, argv, "B:F:H:Q:R:c:i:o:", options, NULL)) != -1) { switch (opt) { case 'B': k_state.B = atof(optarg); break; case 'F': k_state.F = atof(optarg); break; case 'H': k_state.H = atof(optarg); break; case 'Q': k_state.Q = atof(optarg); break; case 'R': k_state.R = atof(optarg); break; case 'c': count = atoi(optarg); break; case 'i': input_data = optarg; break; case 'o': output_data = optarg; break; case 0: break; default: return usage(argv[0]); } } if (generate_array(&array_in, &count, input_data) == -1) return 1; printf("Elements: %d\n", count); { int i; for (i=0;i<count;++i) array_in[i] %= 100; } printf("Kalman parameters: Q:%f R:%f I:%f F:%f H:%f B:%f\n", k_state.Q, k_state.R, k_state.I, k_state.F, k_state.H, k_state.B); array_out = malloc(sizeof(unsigned int)*count); if (!array_out) { fprintf(stderr, "Unable to allocate %d bytes", count); goto out; } memset(array_out, 0x0, sizeof(unsigned int)*count); { int i; kalman_filter_init(&k_state, 0.1, array_in[0]); for (i=1;i<count;++i) array_out[i] = kalman_filter(&k_state, array_in[i]); } if (output_data) { int i; int d = open(output_data, O_WRONLY|O_CREAT, 0666); if (d == -1) { fprintf(stderr, "Error opening %s: %s", output_data, strerror(errno)); goto out; } ftruncate(d, 0); for (i=0;i<count;++i) { char b[1024] = {0x0, }; int n; n = snprintf(b, sizeof(b), "%d %d %d\n", i, array_in[i], array_out[i]); if (write(d, b, n) == -1) { fprintf(stderr, "Error writing to %s: %s", output_data, strerror(errno)); break; } } close(d); } ret = 0; out: if (array_in) free(array_in); if (array_out) free(array_out); return ret; }