ImuNoiseModel.h

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2026  Jose Luis Blanco Claraco                       |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <mrpt/math/TPoint3D.h>
#include <mrpt/random.h>
 
namespace mvsim
{
class ImuNoiseModel
{
   public:
    struct ChannelParams
    {
        double white_noise_std = 0.0;
 
        double random_walk_std = 0.0;
    };
 
    ChannelParams gyroscope;
 
    ChannelParams accelerometer;
 
    void reset();
 
    mrpt::math::TVector3D applyGyroscope(const mrpt::math::TVector3D& trueVal, double dt);
 
    mrpt::math::TVector3D applyAccelerometer(const mrpt::math::TVector3D& trueVal, double dt);
 
    const mrpt::math::TVector3D& gyroBias() const { return gyroBias_; }
 
    const mrpt::math::TVector3D& accelBias() const { return accelBias_; }
 
   private:
    mrpt::math::TVector3D applyChannel(
        const mrpt::math::TVector3D& trueVal, double dt, const ChannelParams& params,
        mrpt::math::TVector3D& bias);
 
    mrpt::math::TVector3D gyroBias_{0.0, 0.0, 0.0};
    mrpt::math::TVector3D accelBias_{0.0, 0.0, 0.0};
 
    mrpt::random::CRandomGenerator rng_;
};
 
}  // namespace mvsim