testKalmanFilterEstimation.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00011 #include <stdio.h>
00012 #include <fstream>
00013 #include "RPYKalmanFilter.h"
00014 #include "EKFilter.h"
00015 
00016 // Usage
00017 // testKalmanFilterEstimation --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
00018 
00019 int main(int argc, char *argv[])
00020 {
00021   // Default file names and params
00022   double Q_angle = 0.001;
00023   double Q_rate = 0.003;
00024   double R_angle = 10;
00025   double dt = 0.002;
00026   std::string pose_file("test.pose"), acc_file("test.acc"), rate_file("test.rate");
00027   bool use_gnuplot = false;
00028 
00029   // Parse argument
00030   for (int i = 0; i < argc; ++ i) {
00031       std::string arg(argv[i]);
00032       if ( arg == "--Q-angle" ) { // KF parameters
00033           if (++i < argc) Q_angle = atof(argv[i]);
00034       } else if ( arg == "--Q-rate" ) {
00035           if (++i < argc) Q_rate = atof(argv[i]);
00036       } else if ( arg == "--R-angle" ) {
00037           if (++i < argc) R_angle = atof(argv[i]);
00038       } else if ( arg == "--dt" ) { // sampling time[s]
00039           if (++i < argc) dt = atof(argv[i]);
00040       } else if ( arg == "--rate-file" ) { // File path for rate
00041           if (++i < argc) rate_file = argv[i];
00042       } else if ( arg == "--acc-file" ) { // File path for acc
00043           if (++i < argc) acc_file = argv[i];
00044       } else if ( arg == "--pose-file" ) { // File path for actual pose
00045           if (++i < argc) pose_file = argv[i];
00046       } else if ( arg == "--use-gnuplot" ) { // Use gnuplot (true or false)
00047           if (++i < argc) use_gnuplot = (std::string(argv[i])=="true"?true:false);
00048       }
00049   }
00050 
00051   // Setup input and output files
00052   std::ifstream ratef(rate_file.c_str()), accf(acc_file.c_str()), posef(pose_file.c_str());
00053   std::cerr << "File : " << rate_file << " " << acc_file << " " << pose_file << std::endl;
00054   if (!ratef.is_open() || !accf.is_open() || !posef.is_open()) {
00055       std::cerr << "No such " << rate_file << " " << acc_file << " " << pose_file << std::endl;
00056       return -1;
00057   }
00058   std::string ofname("/tmp/testKalmanFilterEstimation.dat");
00059   std::ofstream ofs(ofname.c_str());
00060 
00061   // Test kalman filter
00062   RPYKalmanFilter rpy_kf;
00063   rpy_kf.setParam(dt, Q_angle, Q_rate, R_angle);
00064   hrp::Vector3 rate, acc, rpy, rpyRaw, baseRpyCurrent, rpyAct;
00065   double time, time2=0.0;
00066   while(!ratef.eof()){
00067     posef >> time >> time >> time >> time >> rpyAct[0] >> rpyAct[1] >> rpyAct[2]; // Neglect translation in .pose file
00068     ratef >> time >> rate[0] >> rate[1] >> rate[2];
00069     accf >> time >> acc[0] >> acc[1] >> acc[2];
00070     rpy_kf.main_one(rpy, rpyRaw, baseRpyCurrent, acc, rate, 0.0, hrp::Matrix33::Identity());
00071     // rad->deg
00072     rpy*=180/3.14159;
00073     rpyAct*=180/3.14159;
00074     if (use_gnuplot) {
00075         ofs << time2 << " " << rpy[0] << " " << rpy[1] << " " << rpy[2] << " " << rpyAct[0] << " " << rpyAct[1] << " " << rpyAct[2] << std::endl;
00076     } else {
00077         std::cout << rpy[0] << " " << rpy[1] << " " << rpy[2] << " " << rpyAct[0] << " " << rpyAct[1] << " " << rpyAct[2] << std::endl;
00078     }
00079     time2+=dt;
00080   }
00081   // gnuplot
00082   if (use_gnuplot) {
00083       FILE* gp[3];
00084       std::string titles[3] = {"Roll", "Pitch", "Yaw"};
00085       for (size_t ii = 0; ii < 3; ii++) {
00086           gp[ii] = popen("gnuplot", "w");
00087           fprintf(gp[ii], "set title \"%s\"\n", titles[ii].c_str());
00088           fprintf(gp[ii], "set xlabel \"Time [s]\"\n");
00089           fprintf(gp[ii], "set ylabel \"Attitude [rad]\"\n");
00090           fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Estimated\"\n", ofname.c_str(), (2 + ii));
00091           fprintf(gp[ii], "replot \"%s\" using 1:%d with lines title \"Actual\"\n", ofname.c_str(), (2 + ii + 3));
00092           fflush(gp[ii]);
00093       }
00094       std::cout << "Type any keys + enter to exit." << std::endl;
00095       double tmp;
00096       std::cin >> tmp;
00097       for (size_t j = 0; j < 3; j++) pclose(gp[j]);
00098   }
00099   return 0;
00100 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56