wrong initialization of accelerometer noise

[iulianfilip]

In this code you initialize the values of Q matrix:

  auto accelNoiseDensity = imuParams["accel_noise_density"].as<std::vector<double>>();
  auto accelZeroGOffset = imuParams["accel_zero_g_offset"].as<double>();
  auto gyroNoiseDensity = imuParams["gyro_noise_density"].as<double>();
  auto gyroZeroRateOffset = imuParams["gyro_zero_rate_offset"].as<double>();

  Eigen::Vector3d sigmaAccelNoise = Eigen::Map<Eigen::Vector3d>(accelNoiseDensity.data()) * GRAVITY_MAGNITUDE * std::sqrt(imuUpdateRate);
  double sigmaGyroNoise = gyroNoiseDensity * std::sqrt(imuUpdateRate) * M_PI / 180.0;
  double sigmaAccelWork = accelZeroGOffset * std::sqrt(imuUpdateRate) * 1e-3 * GRAVITY_MAGNITUDE;
  double sigmaGyroWork = gyroZeroRateOffset * std::sqrt(imuUpdateRate) * M_PI / 180.0;

  sigmaAccelNoise = sigmaAccelNoise.array().square();
  Q_.block<3, 3>(0, 0) = sigmaAccelNoise.asDiagonal();
  Q_.block<3, 3>(3, 3) = std::pow(sigmaGyroNoise, 2.0) * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(6, 6) = std::pow(sigmaAccelWork, 2.0) * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(9, 9) = std::pow(sigmaGyroWork, 2.0) * Eigen::Matrix3d::Identity();

since in the config file the units for accel_noise_density: [105.0, 105.0, 135.0] # micro g / Hz^0.5 is micro g/Hz^0.5 shouldn't you multiply by 10^-6 in this line: Eigen::Vector3d sigmaAccelNoise = Eigen::Map<Eigen::Vector3d>(accelNoiseDensity.data()) * GRAVITY_MAGNITUDE * std::sqrt(imuUpdateRate);