AllanVarianceNoise.hpp

Go to the documentation of this file.

// Allan Variance Parameterized Noise Model
// Composite IMU noise model using Allan variance parameters
#pragma once
 
#include <Eigen/Core>
#include <vulcan/sensors/BiasInstability.hpp>
#include <vulcan/sensors/GaussianNoise.hpp>
#include <vulcan/sensors/MarkovProcess.hpp>
#include <vulcan/sensors/NoiseTypes.hpp>
#include <vulcan/sensors/RandomWalk.hpp>
 
namespace vulcan::allan {
 
// =============================================================================
// Per-Axis Noise State
// =============================================================================

template <typename Scalar> struct AxisState {
    bias_instability::State<Scalar> bias_state; 
    random_walk::State<Scalar> rrw_state;       
};

template <typename Scalar> AxisState<Scalar> init_axis_state() {
    return AxisState<Scalar>{
        .bias_state = bias_instability::init_state<Scalar>(),
        .rrw_state = random_walk::init_state<Scalar>(),
    };
}
 
// =============================================================================
// Per-Axis Coefficients
// =============================================================================

struct AxisCoeffs {
    double arw_sigma;                     
    bias_instability::Coeffs bias_coeffs; 
    random_walk::Coeffs rrw_coeffs;       
};

inline AxisCoeffs
compute_axis_coeffs(const sensors::AllanParams<double> &params, double dt) {
    return AxisCoeffs{
        .arw_sigma = gaussian::sigma_from_arw(params.N, dt),
        .bias_coeffs =
            bias_instability::compute_coeffs(params.B, params.tau_bias, dt),
        .rrw_coeffs = random_walk::compute_coeffs(params.K, dt),
    };
}
 
// =============================================================================
// Per-Axis Step Function
// =============================================================================

template <typename Scalar>
Scalar step_axis(AxisState<Scalar> &state, const AxisCoeffs &coeffs,
                 const Scalar &noise_arw, const Scalar &noise_bias,
                 const Scalar &noise_rrw) {
    // White noise (angle/velocity random walk)
    Scalar arw = coeffs.arw_sigma * noise_arw;
 
    // Bias instability (Markov process)
    Scalar bias = bias_instability::step(state.bias_state, coeffs.bias_coeffs,
                                         noise_bias);
 
    // Rate random walk (integrated white noise)
    Scalar rrw =
        random_walk::step(state.rrw_state, coeffs.rrw_coeffs, noise_rrw);
 
    return arw + bias + rrw;
}
 
// =============================================================================
// 3-Axis IMU Noise State
// =============================================================================

template <typename Scalar> struct IMUNoiseState {
    AxisState<Scalar> gyro[3];  
    AxisState<Scalar> accel[3]; 
};

template <typename Scalar> IMUNoiseState<Scalar> init_state() {
    IMUNoiseState<Scalar> state;
    for (int i = 0; i < 3; ++i) {
        state.gyro[i] = init_axis_state<Scalar>();
        state.accel[i] = init_axis_state<Scalar>();
    }
    return state;
}
 
// =============================================================================
// 3-Axis IMU Noise Coefficients
// =============================================================================

struct IMUNoiseCoeffs {
    AxisCoeffs gyro;  
    AxisCoeffs accel; 
};

inline IMUNoiseCoeffs
compute_coeffs(const sensors::AllanParams<double> &gyro_params,
               const sensors::AllanParams<double> &accel_params, double dt) {
    return IMUNoiseCoeffs{
        .gyro = compute_axis_coeffs(gyro_params, dt),
        .accel = compute_axis_coeffs(accel_params, dt),
    };
}
 
// =============================================================================
// 3-Axis IMU Step Function
// =============================================================================

template <typename Scalar> struct IMUNoiseInput {
    Eigen::Vector<Scalar, 3> gyro_arw;   
    Eigen::Vector<Scalar, 3> gyro_bias;  
    Eigen::Vector<Scalar, 3> gyro_rrw;   
    Eigen::Vector<Scalar, 3> accel_arw;  
    Eigen::Vector<Scalar, 3> accel_bias; 
    Eigen::Vector<Scalar, 3> accel_rrw;  
};

template <typename Scalar>
sensors::IMUNoiseSample<Scalar> step(IMUNoiseState<Scalar> &state,
                                     const IMUNoiseCoeffs &coeffs,
                                     const IMUNoiseInput<Scalar> &input) {
    sensors::IMUNoiseSample<Scalar> output;
 
    // Step each gyroscope axis
    for (int i = 0; i < 3; ++i) {
        output.gyro(i) =
            step_axis(state.gyro[i], coeffs.gyro, input.gyro_arw(i),
                      input.gyro_bias(i), input.gyro_rrw(i));
    }
 
    // Step each accelerometer axis
    for (int i = 0; i < 3; ++i) {
        output.accel(i) =
            step_axis(state.accel[i], coeffs.accel, input.accel_arw(i),
                      input.accel_bias(i), input.accel_rrw(i));
    }
 
    return output;
}

template <typename Scalar>
sensors::IMUNoiseSample<Scalar>
step_simple(IMUNoiseState<Scalar> &state, const IMUNoiseCoeffs &coeffs,
            const Eigen::Vector<Scalar, 3> &gyro_noise,
            const Eigen::Vector<Scalar, 3> &accel_noise) {
    // Note: This simplified version uses correlated noise between processes
    // For proper simulation, use the full IMUNoiseInput version
    IMUNoiseInput<Scalar> input;
    input.gyro_arw = gyro_noise;
    input.gyro_bias = gyro_noise;
    input.gyro_rrw = gyro_noise;
    input.accel_arw = accel_noise;
    input.accel_bias = accel_noise;
    input.accel_rrw = accel_noise;
 
    return step(state, coeffs, input);
}
 
// =============================================================================
// Grade-Based Factory Functions
// =============================================================================

inline IMUNoiseCoeffs consumer_grade_coeffs(double dt) {
    return compute_coeffs(sensors::imu_grades::consumer_gyro(),
                          sensors::imu_grades::consumer_accel(), dt);
}

inline IMUNoiseCoeffs industrial_grade_coeffs(double dt) {
    return compute_coeffs(sensors::imu_grades::industrial_gyro(),
                          sensors::imu_grades::industrial_accel(), dt);
}

inline IMUNoiseCoeffs tactical_grade_coeffs(double dt) {
    return compute_coeffs(sensors::imu_grades::tactical_gyro(),
                          sensors::imu_grades::tactical_accel(), dt);
}

inline IMUNoiseCoeffs navigation_grade_coeffs(double dt) {
    return compute_coeffs(sensors::imu_grades::navigation_gyro(),
                          sensors::imu_grades::navigation_accel(), dt);
}
 
} // namespace vulcan::allan