00001 /* 00002 * IMUCalibrator.hpp 00003 * 00004 * Created on: Aug 23, 2012 00005 * Author: mriedel 00006 */ 00007 00008 #ifndef IMUCALIBRATOR_HPP_ 00009 #define IMUCALIBRATOR_HPP_ 00010 00011 #include <telekyb_defines/telekyb_defines.hpp> 00012 00013 #include <telekyb_base/Options.hpp> 00014 00015 #include "SerialIMUDevice.hpp" 00016 00017 #include <telekyb_base/Time.hpp> 00018 00019 // For statistics 00020 #include <boost/accumulators/accumulators.hpp> 00021 #include <boost/accumulators/statistics/stats.hpp> 00022 #include <boost/accumulators/statistics/variance.hpp> 00023 00024 00025 using namespace boost::accumulators; 00026 00027 namespace TELEKYB_NAMESPACE { 00028 00029 class IMUCalibratorOptions : public OptionContainer { 00030 public: 00031 Option<double>* tDriftEstimTimeout; 00032 Option<double>* tDriftEstimVarianceThreshold; 00033 Option<int>* tDriftEstimWindowSize; 00034 Option<double>* tDriftFilterGain; 00035 IMUCalibratorOptions(); 00036 }; 00037 00038 class IMUCalibrator : public RawImuDataListener { 00039 private: 00040 IMUCalibratorOptions options; 00041 00042 bool driftEstimationActive; 00043 bool driftEstimationDone[3]; 00044 Timer driftEstimationTimer; 00045 00046 Eigen::Vector3d gyroSteadyValues; 00047 00048 accumulator_set<double, stats<tag::variance(lazy)> > driftEstimAccRoll; 00049 accumulator_set<double, stats<tag::variance(lazy)> > driftEstimAccPitch; 00050 accumulator_set<double, stats<tag::variance(lazy)> > driftEstimAccYaw; 00051 00052 00053 public: 00054 IMUCalibrator(); 00055 virtual ~IMUCalibrator(); 00056 00057 00058 bool driftEstimation(Eigen::Vector3d& gyroOffsets); 00059 void processRawIMUData(const RawImuData& data); 00060 }; 00061 00062 } /* namespace telekyb */ 00063 #endif /* IMUCALIBRATOR_HPP_ */
