test/src/RunKalmanFilter1d.cpp

   1 /*
   2 Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2013 The XCSoar Project
   6   A detailed list of copyright holders can be found in the file "AUTHORS".
   7
   8   This program is free software; you can redistribute it and/or
   9   modify it under the terms of the GNU General Public License
  10   as published by the Free Software Foundation; either version 2
  11   of the License, or (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  21 }
  22 */
  23
  24 #include "OS/Args.hpp"
  25 #include "IO/FileLineReader.hpp"
  26 #include "Math/KalmanFilter1d.hpp"
  27
  28 #include <stdio.h>
  29
  30 int main(int argc, char **argv)
  31 {
  32   Args args(argc, argv, "FILE");
  33   tstring path = args.ExpectNextT();
  34   args.ExpectEnd();
  35
  36   FileLineReaderA reader(path.c_str());
  37   if (reader.error()) {
  38     _ftprintf(stderr, _T("Failed to open %s\n"), path.c_str());
  39     return EXIT_FAILURE;
  40   }
  41
  42   KalmanFilter1d kalman_filter(fixed(0.0075));
  43
  44   unsigned last_t = 0;
  45   double last_value;
  46
  47   const char *line;
  48   while ((line = reader.ReadLine()) != nullptr) {
  49     const char *p = line;
  50     char *endptr;
  51     unsigned t = strtoul(p, &endptr, 10);
  52     if (endptr == line) {
  53       fprintf(stderr, "Malformed line: %s\n", line);
  54       return EXIT_FAILURE;
  55     }
  56
  57     p = endptr;
  58     double value = strtod(p, &endptr);
  59     if (endptr == line) {
  60       fprintf(stderr, "Malformed line: %s\n", line);
  61       return EXIT_FAILURE;
  62     }
  63
  64     if (last_t > 0 && t > last_t) {
  65       fixed dt = fixed(t - last_t) / 1000;
  66
  67       kalman_filter.Update(fixed(value), fixed(0.05), dt);
  68
  69       printf("%u %f %f %f %f\n", t,
  70              value, double(fixed(value - last_value) / dt),
  71              (double)kalman_filter.GetXAbs(), (double)kalman_filter.GetXVel());
  72     }
  73
  74     last_t = t;
  75     last_value = value;
  76   }
  77
  78   return EXIT_SUCCESS;
  79 }
  80