gyroscopeBiasEstimator.h

#ifndef _GYROBIASESTIMATOR_H_
#define _GYROBIASESTIMATOR_H_
 
#include "tools/lowPassFilter.h"
 
#define ACCEL_LOWPASS_FREQ 1.0
#define GYRO_LOWPASS_FREQ 10.0
#define GYRO_BIAS_LOWPASS_FREQ 0.1500000059604645
#define NUM_GYRO_BIAS_SAMPLES_THRESHOLD 30
#define NUM_GYRO_BIAS_SAMPLES_INITIAL_SMOOTHING 100.0
#define ACCEL_DIFF_STATIC_THRESHOLD 0.5
#define GYRO_DIFF_STATIC_THRESHOLD 0.00800000037997961
#define GYRO_FOR_BIAS_THRESHOLD 0.25
#define IS_STATIC_NUM_FRAMES_THRESHOLD 3
 
class IsStaticCounter
{
public:
  IsStaticCounter(){};
 
  IsStaticCounter(int minFrames)
  {
    minStaticFrames = minFrames;
    consecutiveIsStatic = 0;
    lastTimestampNs = 0;
  }
 
  ~IsStaticCounter(){};
 
 
  void appendFrame(bool isStatic)
  {
    if (!isStatic)
      consecutiveIsStatic = 0;
    else
      consecutiveIsStatic += 1;
  }
 
  bool isRecentlyStatic()
  {
    return consecutiveIsStatic >= minStaticFrames;
  }
 
private:
  int minStaticFrames;
  int consecutiveIsStatic;
  int64_t lastTimestampNs;
 
};
 
class GyroscopeBiasEstimator
{
public:
  GyroscopeBiasEstimator();
  ~GyroscopeBiasEstimator();
 
  void reset();
  void processGyroscope(Vector3d gyro, int64_t sensorTimestampNs);
  void processAccelerometer(Vector3d accel, int64_t sensorTimestampNs);
  void getGyroBias(Vector3d &result);
  void updateGyroBias(Vector3d gyro, int64_t sensorTimestampNs);
 
private:
  LowPassFilter accelLowPass;
  LowPassFilter gyroLowPass;
  LowPassFilter gyroBiasLowPass;
  Vector3d smoothedGyroDiff;
  Vector3d smoothedAccelDiff;
  IsStaticCounter isAccelStatic;
  IsStaticCounter isGyroStatic;
};
 
#endif