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;
}
