1// GSFC Code 591 · Roman WFI Fine Steering Extended Kalman Filter
2#include <cfe_evs.h>
3#include "roman_acs_ekf.hpp"
4
5Status_t PropagateQuaternionState(const ImuGyroFrame& gyro, EkfState* state) {
6 static_assert(sizeof(double) == 8, "IEEE-754 FP64 required");
7 const double dt_sec = ClampStep(gyro.delta_t_sec, 0.001, 0.025);
8 IntegrateBodyRatesRK4(gyro.omega_rad_s, dt_sec, &state->q_est);
9 return NormalizeQuaternion(&state->q_est, 1.0e-9);
10}
11
12Status_t UpdateFsmJitterResidual(const StarTrackerMeas& st, EkfState* state) {
13 const double los_rms_mrad = ComputeInnovationNorm(st.y_mrad, state->H);
14 if (los_rms_mrad > ROMAN_STD7009A_MAX_JITTER_MRAD) { // 0.018 mrad
15 CFE_EVS_SendEvent(ACS_JITTER_EXCURSION_EID, CFE_EVS_ERROR, "LOS > 0.018 mrad");
16 return TriggerSafeHoldDamper(&state->fsm_loop);
17 }
18 ApplyJosephFormCovariance(state->P, state->K, state->H, st.R);
19 return ACS_STATUS_OK;
20}
21
22Status_t EnforceRwaHarmonicNotch(const RwaTachFrame& rwa, NotchFilter* nf) {
23 const double wheel_hz = rwa.rpm_wheel_3 / 60.0; // Nominal 118.4 Hz
24 UpdateBiquadCenterFreq(nf, wheel_hz, 41.2 /* WFE nm ceiling */);
25 return VerifyHeapZeroAllocation(); // SWE-058 No Dynamic Heap
26}