testKFilter.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
11 #include <stdio.h>
12 #include <fstream>
13 #include "RPYKalmanFilter.h"
14 
15 // Usage
16 // testKalmanFilter --acc-file /tmp/kftest/test.acc --rate-file /tmp/kftest/test.rate --pose-file /tmp/kftest/test.pose --Q-angle 1e-3 --Q-rate 1e-10 --dt 0.005 --R-angle 1 --use-gnuplot true
17 
18 int main(int argc, char *argv[])
19 {
20  // Default file names and params
21  double Q_pos = 0.001;
22  double Q_vel = 0.003;
23  double R_pos = 0.005;
24  double dt = 0.004;
25  std::string input_file("test.dat");
26  bool use_gnuplot = false;
27 
28  // Parse argument
29  for (int i = 0; i < argc; ++ i) {
30  std::string arg(argv[i]);
31  if ( arg == "--Q-pos" ) { // KF parameters
32  if (++i < argc) Q_pos = atof(argv[i]);
33  } else if ( arg == "--Q-vel" ) {
34  if (++i < argc) Q_vel = atof(argv[i]);
35  } else if ( arg == "--R-pos" ) {
36  if (++i < argc) R_pos = atof(argv[i]);
37  } else if ( arg == "--dt" ) { // sampling time[s]
38  if (++i < argc) dt = atof(argv[i]);
39  } else if ( arg == "--input-file" ) { // File path for rate
40  if (++i < argc) input_file = argv[i];
41  } else if ( arg == "--use-gnuplot" ) { // Use gnuplot (true or false)
42  if (++i < argc) use_gnuplot = (std::string(argv[i])=="true"?true:false);
43  }
44  }
45 
46  // Setup input and output files
47  std::ifstream inputf(input_file.c_str());
48  std::cerr << "File : " << input_file << std::endl;
49  if (!inputf.is_open()) {
50  std::cerr << "No such " << input_file << std::endl;
51  return -1;
52  }
53  std::string ofname("/tmp/testKalmanFilter.dat");
54  std::ofstream ofs(ofname.c_str());
55 
56  // Test kalman filter
57  KFilter kf;
58  //kf.setF(1, -dt, 0, 1);
59  //kf.setB(dt, 0);
60  kf.setQ(Q_pos*dt, 0, 0, Q_vel*dt);
61  kf.setB(0, 0);
62  kf.setF(1, dt, 0, 1);
63  kf.setP(0, 0, 0, 0);
64  kf.setR(R_pos);
65  //hrp::Vector3 rate, acc, rpy, rpyRaw, baseRpyCurrent, rpyAct;
66  double time, time2=0.0;
67  double data;
68  while(!inputf.eof()){
69  inputf >> time >> data;
70  kf.update(0, data);
71  if (use_gnuplot) {
72  ofs << time2 << " " << data << " " << kf.getx()[0] << " " << kf.getx()[1] << std::endl;
73  } else {
74  std::cout << data << std::endl;
75  }
76  time2+=dt;
77  }
78  // gnuplot
79  if (use_gnuplot) {
80  FILE* gp[2];
81  std::string titles[2] = {"Pos", "Vel"};
82  for (size_t ii = 0; ii < 2; ii++) {
83  gp[ii] = popen("gnuplot", "w");
84  fprintf(gp[ii], "set title \"%s\"\n", titles[ii].c_str());
85  fprintf(gp[ii], "set xlabel \"Time [s]\"\n");
86  fprintf(gp[ii], "set ylabel \"Pos\"\n");
87  if (ii==0) {
88  fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Filtered\"\n", ofname.c_str(), 2);
89  fprintf(gp[ii], "replot \"%s\" using 1:%d with lines title \"Raw\"\n", ofname.c_str(), 3);
90  } else {
91  fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Vel\"\n", ofname.c_str(), 4);
92  }
93  fflush(gp[ii]);
94  }
95  std::cout << "Type any keys + enter to exit." << std::endl;
96  double tmp;
97  std::cin >> tmp;
98  for (size_t j = 0; j < 2; j++) pclose(gp[j]);
99  }
100  return 0;
101 }
int main(int argc, char *argv[])
Definition: testKFilter.cpp:18
png_uint_32 i
def j(str, encoding="cp932")
FILE * popen(const char *cmd, const char *mode)
void setQ(const double _q0, const double _q1, const double _q2, const double _q3)
JSAMPIMAGE data
char * arg
void pclose(FILE *fd)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51