gyroscopeBiasEstimator.cpp

 
#include "tools/gyroscopeBiasEstimator.h"
#include <algorithm>
 
GyroscopeBiasEstimator::GyroscopeBiasEstimator()
{
  reset();
}
 
GyroscopeBiasEstimator::~GyroscopeBiasEstimator()
{
 
}
 
void GyroscopeBiasEstimator::reset()
{
  smoothedGyroDiff = Vector3d();
  smoothedAccelDiff = Vector3d();
  accelLowPass = LowPassFilter(ACCEL_LOWPASS_FREQ);
  gyroLowPass = LowPassFilter(GYRO_LOWPASS_FREQ);
  gyroBiasLowPass = LowPassFilter(GYRO_BIAS_LOWPASS_FREQ);
  isAccelStatic = IsStaticCounter(IS_STATIC_NUM_FRAMES_THRESHOLD);
  isGyroStatic = IsStaticCounter(IS_STATIC_NUM_FRAMES_THRESHOLD);
}
 
void GyroscopeBiasEstimator::processGyroscope(Vector3d gyro, int64_t sensorTimestampNs)
{
  gyroLowPass.addSample(gyro, sensorTimestampNs);
  Vector3d::sub(gyro, gyroLowPass.getFilteredData(), smoothedGyroDiff);
  
  isGyroStatic.appendFrame(smoothedGyroDiff.length() < GYRO_DIFF_STATIC_THRESHOLD);
 
  if ((isGyroStatic.isRecentlyStatic()) && (isAccelStatic.isRecentlyStatic()))
      updateGyroBias(gyro, sensorTimestampNs);
}
 
void GyroscopeBiasEstimator::processAccelerometer(Vector3d accel, int64_t sensorTimestampNs)
{
  accelLowPass.addSample(accel, sensorTimestampNs);
  Vector3d::sub(accel, accelLowPass.getFilteredData(), smoothedAccelDiff);
 
  isAccelStatic.appendFrame(smoothedAccelDiff.length() < ACCEL_DIFF_STATIC_THRESHOLD);
}
 
void GyroscopeBiasEstimator::getGyroBias(Vector3d &result)
{
  if (gyroBiasLowPass.getNumSamples() < NUM_GYRO_BIAS_SAMPLES_THRESHOLD)
  {
    result.setZero();
  }
  else
  {
   result.set(gyroBiasLowPass.getFilteredData());
   double rampUpRatio = std::min(1.0, double((gyroBiasLowPass.getNumSamples() - NUM_GYRO_BIAS_SAMPLES_THRESHOLD)) / NUM_GYRO_BIAS_SAMPLES_INITIAL_SMOOTHING);
   result.scale(rampUpRatio);
  }
}
 
void GyroscopeBiasEstimator::updateGyroBias(Vector3d gyro, int64_t sensorTimestampNs)
{
  if (gyro.length() >= GYRO_FOR_BIAS_THRESHOLD)
  {
    return;
  }
 
  double updateWeight = std::max(0.0, 1.0 - gyro.length() * 25.0);
  updateWeight *= updateWeight;
  gyroBiasLowPass.addWeightedSample(gyroLowPass.getFilteredData(), sensorTimestampNs, updateWeight);
}