EstimationTypes.hpp

Go to the documentation of this file.

// Estimation Types - Common types for Kalman filter implementations
// Part of Vulcan - Aerospace Engineering Utilities
#pragma once
 
#include <Eigen/Dense>
#include <janus/janus.hpp>
 
namespace vulcan::estimation {
 
// =============================================================================
// Filter State Container
// =============================================================================

template <typename Scalar, int N> struct FilterState {
    Eigen::Matrix<Scalar, N, 1> x; 
    Eigen::Matrix<Scalar, N, N> P; 

    FilterState()
        : x(Eigen::Matrix<Scalar, N, 1>::Zero()),
          P(Eigen::Matrix<Scalar, N, N>::Identity()) {}

    FilterState(const Eigen::Matrix<Scalar, N, 1> &state,
                const Eigen::Matrix<Scalar, N, N> &covariance)
        : x(state), P(covariance) {}
};
 
// =============================================================================
// Noise Specifications
// =============================================================================

template <int N> struct ProcessNoise {
    Eigen::Matrix<double, N, N> Q; 

    ProcessNoise() : Q(Eigen::Matrix<double, N, N>::Zero()) {}

    explicit ProcessNoise(const Eigen::Matrix<double, N, N> &covariance)
        : Q(covariance) {}

    static ProcessNoise<N>
    diagonal(const Eigen::Matrix<double, N, 1> &variances) {
        ProcessNoise<N> noise;
        noise.Q = variances.asDiagonal();
        return noise;
    }

    static ProcessNoise<N> scalar(double variance) {
        ProcessNoise<N> noise;
        noise.Q = Eigen::Matrix<double, N, N>::Identity() * variance;
        return noise;
    }
};

template <int M> struct MeasurementNoise {
    Eigen::Matrix<double, M, M> R; 

    MeasurementNoise() : R(Eigen::Matrix<double, M, M>::Zero()) {}

    explicit MeasurementNoise(const Eigen::Matrix<double, M, M> &covariance)
        : R(covariance) {}

    static MeasurementNoise<M>
    diagonal(const Eigen::Matrix<double, M, 1> &variances) {
        MeasurementNoise<M> noise;
        noise.R = variances.asDiagonal();
        return noise;
    }

    static MeasurementNoise<M> scalar(double variance) {
        MeasurementNoise<M> noise;
        noise.R = Eigen::Matrix<double, M, M>::Identity() * variance;
        return noise;
    }
};
 
} // namespace vulcan::estimation