EstimationUtils.hpp

Go to the documentation of this file.

// Estimation Utilities - Helper functions for Kalman filtering
// Part of Vulcan - Aerospace Engineering Utilities
#pragma once
 
#include <Eigen/Dense>
#include <janus/janus.hpp>
 
namespace vulcan::estimation {
 
// =============================================================================
// Innovation Statistics
// =============================================================================

template <typename Scalar, int M>
Scalar
normalized_innovation_squared(const Eigen::Matrix<Scalar, M, 1> &innovation,
                              const Eigen::Matrix<Scalar, M, M> &S) {
    return (innovation.transpose() * S.inverse() * innovation)(0, 0);
}
 
// =============================================================================
// Measurement Gating
// =============================================================================

template <typename Scalar, int M>
bool passes_gate(const Eigen::Matrix<Scalar, M, 1> &innovation,
                 const Eigen::Matrix<Scalar, M, M> &S, double threshold) {
    Scalar nis = normalized_innovation_squared<Scalar, M>(innovation, S);
    return static_cast<double>(nis) <= threshold;
}

template <typename Scalar, int M>
bool measurement_passes_gate(const Eigen::Matrix<Scalar, M, 1> &z,
                             const Eigen::Matrix<Scalar, M, 1> &z_pred,
                             const Eigen::Matrix<Scalar, M, M> &S,
                             double threshold) {
    Eigen::Matrix<Scalar, M, 1> innovation = z - z_pred;
    return passes_gate<Scalar, M>(innovation, S, threshold);
}
 
// =============================================================================
// Joseph Form Covariance Update
// =============================================================================

template <int N, int M>
Eigen::Matrix<double, N, N>
joseph_update(const Eigen::Matrix<double, N, N> &P_prior,
              const Eigen::Matrix<double, N, M> &K,
              const Eigen::Matrix<double, M, N> &H,
              const Eigen::Matrix<double, M, M> &R) {
    Eigen::Matrix<double, N, N> I = Eigen::Matrix<double, N, N>::Identity();
    Eigen::Matrix<double, N, N> IKH = I - K * H;
 
    // Joseph form: P = (I-KH)*P*(I-KH)ᵀ + K*R*Kᵀ
    return IKH * P_prior * IKH.transpose() + K * R * K.transpose();
}
 
// =============================================================================
// Common Chi-Squared Thresholds
// =============================================================================

inline double chi_squared_threshold(int dof, double confidence = 0.95) {
    // Pre-computed values for common cases
    if (confidence >= 0.99) {
        switch (dof) {
        case 1:
            return 6.635;
        case 2:
            return 9.210;
        case 3:
            return 11.345;
        case 4:
            return 13.277;
        case 5:
            return 15.086;
        case 6:
            return 16.812;
        default:
            // Approximation for larger dof at 99%
            return dof + 2.33 * std::sqrt(2.0 * dof);
        }
    } else {
        // 95% confidence
        switch (dof) {
        case 1:
            return 3.841;
        case 2:
            return 5.991;
        case 3:
            return 7.815;
        case 4:
            return 9.488;
        case 5:
            return 11.070;
        case 6:
            return 12.592;
        default:
            // Approximation for larger dof at 95%
            return dof + 1.645 * std::sqrt(2.0 * dof);
        }
    }
}
 
// =============================================================================
// Covariance Symmetry Enforcement
// =============================================================================

template <int N>
Eigen::Matrix<double, N, N> symmetrize(const Eigen::Matrix<double, N, N> &P) {
    return 0.5 * (P + P.transpose());
}
 
} // namespace vulcan::estimation