rust_gnc/filters/
complementary.rs

1//! # Complementary Filter
2//! 
3//! This module implements a first-order Complementary Filter, a computationally 
4//! efficient sensor fusion algorithm used to estimate vehicle attitude.
5//! 
6//! ### Theory of Operation
7//! The filter functions as a combination of a **High-Pass Filter** for the 
8//! gyroscope and a **Low-Pass Filter** for the accelerometer:
9//! 1. **Gyroscope integration** provides high-frequency tracking but suffers 
10//!    from low-frequency bias/drift.
11//! 2. **Accelerometer reference** provides stable low-frequency absolute 
12//!    orientation but is susceptible to high-frequency vibration/noise.
13use crate::filters::{Filter, InertialInput};
14use crate::units::Radians;
15
16/// A first-order Complementary Filter for attitude estimation.
17/// 
18/// The internal state represents the filtered angle, blending integrated 
19/// angular rates with an absolute reference.
20pub struct ComplementaryFilter {
21    alpha: f32, 
22    state: Radians,
23}
24
25impl ComplementaryFilter {
26    /// Alpha is the trust coefficient for the gyroscope measurement. A value of 1.0 means we trust the gyro completely, while 0.0 means we trust the accelerometer completely.
27    /// Typically set to 0.98 for flight controllers.
28    pub fn new(alpha: f32) -> Self {
29        Self {
30            alpha: alpha.clamp(0.0, 1.0),
31            state: Radians(0.0),
32        }
33    }
34
35    /// Dynamically updates the trust coefficient.
36    /// 
37    /// Useful for adaptive filtering where trust in the accelerometer 
38    /// might decrease during high-G maneuvers.
39    pub fn set_alpha(&mut self, alpha: f32) {
40        self.alpha = alpha.clamp(0.0, 1.0);
41    }
42}
43
44impl Filter for ComplementaryFilter {
45    type Input = InertialInput; // (gyro_rate, accel_angle)
46    type Output = Radians; // Estimated angle
47
48    fn new(alpha: f32) -> Self {
49        Self {
50            alpha: alpha.clamp(0.0, 1.0),
51            state: Radians(0.0),
52        }
53    }
54
55    /// Processes a new sensor sample to update the attitude estimate.
56    /// 
57    /// This method implements the discrete-time fusion equation:
58    /// `θ = α * (θ + ω * dt) + (1 - α) * a`
59    /// 
60    /// ### Mathematical Steps
61    /// 1. **Prediction**: Integrate the angular rate (gyro) into the current state.
62    /// 2. **Correction**: Apply the reference angle (accel) to cancel integration drift.
63    fn update(&mut self, input: Self::Input, dt: f32) -> Self::Output {
64        let integrated_rate = self.state.0 + input.rate.0 * dt; // Integrate gyro rate to get angle
65        let filtered_angle = self.alpha * integrated_rate + ((1.0 - self.alpha) * input.reference.0); // Blend with accelerometer angle
66
67        self.state = Radians(filtered_angle);
68        self.state
69    }
70
71    /// Resets the filter's internal state to zero.
72    /// 
73    /// Should be called during the 'Disarm' phase to ensure the next 
74    /// takeoff sequence starts from a clean level-reference.
75    fn reset(&mut self) {
76        self.state = Radians(0.0);
77    }
78}
rust_gnc/filters/complementary.rs

rust_gnc/filters/
complementary.rs
