KalmanFilter.hpp

Go to the documentation of this file.

// Kalman Filter - Linear Kalman filter predict/update utilities
// Part of Vulcan - Aerospace Engineering Utilities
#pragma once
 
#include <vulcan/estimation/EstimationTypes.hpp>
 
namespace vulcan::estimation {
 
// =============================================================================
// Linear Kalman Filter Predict
// =============================================================================

template <typename Scalar, int N, int U>
FilterState<Scalar, N> kf_predict(const FilterState<Scalar, N> &state,
                                  const Eigen::Matrix<double, N, N> &F,
                                  const Eigen::Matrix<double, N, U> &B,
                                  const Eigen::Matrix<Scalar, U, 1> &u,
                                  const ProcessNoise<N> &noise) {
    FilterState<Scalar, N> predicted;
 
    // State prediction: x⁻ = F*x + B*u
    predicted.x =
        F.template cast<Scalar>() * state.x + B.template cast<Scalar>() * u;
 
    // Covariance prediction: P⁻ = F*P*Fᵀ + Q
    auto F_scalar = F.template cast<Scalar>();
    predicted.P = F_scalar * state.P * F_scalar.transpose() +
                  noise.Q.template cast<Scalar>();
 
    return predicted;
}

template <typename Scalar, int N>
FilterState<Scalar, N> kf_predict(const FilterState<Scalar, N> &state,
                                  const Eigen::Matrix<double, N, N> &F,
                                  const ProcessNoise<N> &noise) {
    FilterState<Scalar, N> predicted;
 
    // State prediction: x⁻ = F*x
    predicted.x = F.template cast<Scalar>() * state.x;
 
    // Covariance prediction: P⁻ = F*P*Fᵀ + Q
    auto F_scalar = F.template cast<Scalar>();
    predicted.P = F_scalar * state.P * F_scalar.transpose() +
                  noise.Q.template cast<Scalar>();
 
    return predicted;
}
 
// =============================================================================
// Linear Kalman Filter Update
// =============================================================================

template <typename Scalar, int N, int M>
FilterState<Scalar, N> kf_update(const FilterState<Scalar, N> &state,
                                 const Eigen::Matrix<Scalar, M, 1> &z,
                                 const Eigen::Matrix<double, M, N> &H,
                                 const MeasurementNoise<M> &noise) {
    // Cast matrices to Scalar type for mixed operations
    auto H_scalar = H.template cast<Scalar>();
    auto R_scalar = noise.R.template cast<Scalar>();
 
    // Innovation covariance: S = H*P*Hᵀ + R
    Eigen::Matrix<Scalar, M, M> S =
        H_scalar * state.P * H_scalar.transpose() + R_scalar;
 
    // Kalman gain: K = P*Hᵀ*S⁻¹
    Eigen::Matrix<Scalar, N, M> K =
        state.P * H_scalar.transpose() * S.inverse();
 
    // Innovation (measurement residual): y = z - H*x
    Eigen::Matrix<Scalar, M, 1> innovation = z - H_scalar * state.x;
 
    // State update: x̂ = x⁻ + K*y
    FilterState<Scalar, N> updated;
    updated.x = state.x + K * innovation;
 
    // Covariance update: P = (I - K*H)*P⁻
    Eigen::Matrix<Scalar, N, N> I = Eigen::Matrix<Scalar, N, N>::Identity();
    updated.P = (I - K * H_scalar) * state.P;
 
    return updated;
}
 
// =============================================================================
// Combined Predict + Update
// =============================================================================

template <typename Scalar, int N, int U, int M>
FilterState<Scalar, N> kf_step(
    const FilterState<Scalar, N> &state, const Eigen::Matrix<Scalar, U, 1> &u,
    const Eigen::Matrix<Scalar, M, 1> &z, const Eigen::Matrix<double, N, N> &F,
    const Eigen::Matrix<double, N, U> &B, const Eigen::Matrix<double, M, N> &H,
    const ProcessNoise<N> &Q, const MeasurementNoise<M> &R) {
    // Predict
    auto predicted = kf_predict<Scalar, N, U>(state, F, B, u, Q);
 
    // Update
    return kf_update<Scalar, N, M>(predicted, z, H, R);
}

template <typename Scalar, int N, int M>
FilterState<Scalar, N> kf_step(const FilterState<Scalar, N> &state,
                               const Eigen::Matrix<Scalar, M, 1> &z,
                               const Eigen::Matrix<double, N, N> &F,
                               const Eigen::Matrix<double, M, N> &H,
                               const ProcessNoise<N> &Q,
                               const MeasurementNoise<M> &R) {
    // Predict
    auto predicted = kf_predict<Scalar, N>(state, F, Q);
 
    // Update
    return kf_update<Scalar, N, M>(predicted, z, H, R);
}
 
} // namespace vulcan::estimation