Program Listing for File kalman_covariance_provider.h

↰ Return to documentation for file (include/adi_imu/kalman_covariance_provider.h)

/*******************************************************************************
 *   @file   kalman_covariance_provider.h
 *   @brief  Kalman filter-based covariance estimation for IMU data.
 *   @author Tudor Alinei
 *******************************************************************************/
// Copyright 2026 Analog Devices, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ADI_IMU__KALMAN_COVARIANCE_PROVIDER_H_
#define ADI_IMU__KALMAN_COVARIANCE_PROVIDER_H_

#include <cstddef>

#include "adi_imu/imu_covariance_interface.h"
#include "adi_imu/motion_detector.h"

namespace adi_imu
{

class KalmanCovarianceProvider : public ImuCovarianceInterface
{
public:
  // Default Kalman filter covariance algorithm parameters
  inline static constexpr double DEFAULT_PROCESS_NOISE_Q = 1e-6;
  inline static constexpr double DEFAULT_MEASUREMENT_NOISE_R = 1e-4;
  inline static constexpr double DEFAULT_INITIAL_VARIANCE = 1e-4;
  inline static constexpr size_t DEFAULT_WARMUP_SAMPLES = 100;
  inline static constexpr double DEFAULT_MIN_VARIANCE = 1e-9;

  KalmanCovarianceProvider(
    double process_noise_q = DEFAULT_PROCESS_NOISE_Q,
    double measurement_noise_r = DEFAULT_MEASUREMENT_NOISE_R,
    double initial_variance = DEFAULT_INITIAL_VARIANCE,
    size_t warmup_samples = DEFAULT_WARMUP_SAMPLES, double min_variance = DEFAULT_MIN_VARIANCE,
    MotionDetector motion_detector = MotionDetector());

  ~KalmanCovarianceProvider() override = default;

  void addSample(const Vec3 & accel, const Vec3 & gyro) override;
  bool isReady() const override;
  CovarianceMatrix getAccelCovariance() const override;
  CovarianceMatrix getGyroCovariance() const override;
  void reset() override;
  double getCalibrationProgress() const override;

private:
  struct AxisFilter
  {
    double mean;              // Running mean estimate
    double variance;          // Kalman state: estimated variance
    double error_covariance;  // Kalman error covariance (P)

    AxisFilter() : mean(0.0), variance(0.0), error_covariance(1.0) {}
  };

  void updateAxisFilter(AxisFilter & filter, double sample);

  CovarianceMatrix buildCovarianceMatrix(double var_x, double var_y, double var_z) const;

  // Kalman filter parameters
  double m_process_noise_q;      // Q: process noise covariance
  double m_measurement_noise_r;  // R: measurement noise covariance
  double m_initial_variance;     // Initial variance estimate
  double m_min_variance;         // Minimum variance floor

  // Motion detector for stationary filtering
  MotionDetector m_motion_detector;

  // Per-axis filters
  AxisFilter m_accel_x;
  AxisFilter m_accel_y;
  AxisFilter m_accel_z;
  AxisFilter m_gyro_x;
  AxisFilter m_gyro_y;
  AxisFilter m_gyro_z;

  // Sample tracking
  size_t m_warmup_samples;
  size_t m_sample_count;

  // Mean estimation parameters (for computing residuals)
  static constexpr double MEAN_ALPHA = 0.01;  // EMA factor for mean tracking
};

}  // namespace adi_imu

#endif  // ADI_IMU__KALMAN_COVARIANCE_PROVIDER_H_