testKFilter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00011 #include <stdio.h>
00012 #include <fstream>
00013 #include "RPYKalmanFilter.h"
00014 
00015 // Usage
00016 // 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
00017 
00018 int main(int argc, char *argv[])
00019 {
00020   // Default file names and params
00021   double Q_pos = 0.001;
00022   double Q_vel = 0.003;
00023   double R_pos = 0.005;
00024   double dt = 0.004;
00025   std::string input_file("test.dat");
00026   bool use_gnuplot = false;
00027 
00028   // Parse argument
00029   for (int i = 0; i < argc; ++ i) {
00030       std::string arg(argv[i]);
00031       if ( arg == "--Q-pos" ) { // KF parameters
00032           if (++i < argc) Q_pos = atof(argv[i]);
00033       } else if ( arg == "--Q-vel" ) {
00034           if (++i < argc) Q_vel = atof(argv[i]);
00035       } else if ( arg == "--R-pos" ) {
00036           if (++i < argc) R_pos = atof(argv[i]);
00037       } else if ( arg == "--dt" ) { // sampling time[s]
00038           if (++i < argc) dt = atof(argv[i]);
00039       } else if ( arg == "--input-file" ) { // File path for rate
00040           if (++i < argc) input_file = argv[i];
00041       } else if ( arg == "--use-gnuplot" ) { // Use gnuplot (true or false)
00042           if (++i < argc) use_gnuplot = (std::string(argv[i])=="true"?true:false);
00043       }
00044   }
00045 
00046   // Setup input and output files
00047   std::ifstream inputf(input_file.c_str());
00048   std::cerr << "File : " << input_file << std::endl;
00049   if (!inputf.is_open()) {
00050       std::cerr << "No such " << input_file << std::endl;
00051       return -1;
00052   }
00053   std::string ofname("/tmp/testKalmanFilter.dat");
00054   std::ofstream ofs(ofname.c_str());
00055 
00056   // Test kalman filter
00057   KFilter kf;
00058   //kf.setF(1, -dt, 0, 1);
00059   //kf.setB(dt, 0);
00060   kf.setQ(Q_pos*dt, 0, 0, Q_vel*dt);
00061   kf.setB(0, 0);
00062   kf.setF(1, dt, 0, 1);
00063   kf.setP(0, 0, 0, 0);
00064   kf.setR(R_pos);
00065   //hrp::Vector3 rate, acc, rpy, rpyRaw, baseRpyCurrent, rpyAct;
00066   double time, time2=0.0;
00067   double data;
00068   while(!inputf.eof()){
00069       inputf >> time >> data;
00070       kf.update(0, data);
00071       if (use_gnuplot) {
00072           ofs << time2 << " " << data << " " << kf.getx()[0] << " " << kf.getx()[1] << std::endl;
00073       } else {
00074           std::cout << data << std::endl;
00075       }
00076       time2+=dt;
00077   }
00078   // gnuplot
00079   if (use_gnuplot) {
00080       FILE* gp[2];
00081       std::string titles[2] = {"Pos", "Vel"};
00082       for (size_t ii = 0; ii < 2; ii++) {
00083           gp[ii] = popen("gnuplot", "w");
00084           fprintf(gp[ii], "set title \"%s\"\n", titles[ii].c_str());
00085           fprintf(gp[ii], "set xlabel \"Time [s]\"\n");
00086           fprintf(gp[ii], "set ylabel \"Pos\"\n");
00087           if (ii==0) {
00088               fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Filtered\"\n", ofname.c_str(), 2);
00089               fprintf(gp[ii], "replot \"%s\" using 1:%d with lines title \"Raw\"\n", ofname.c_str(), 3);
00090           } else {
00091               fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Vel\"\n", ofname.c_str(), 4);
00092           }
00093           fflush(gp[ii]);
00094       }
00095       std::cout << "Type any keys + enter to exit." << std::endl;
00096       double tmp;
00097       std::cin >> tmp;
00098       for (size_t j = 0; j < 2; j++) pclose(gp[j]);
00099   }
00100   return 0;
00101 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19