veloxity_core/estimator/
quad.rs

1use super::AttitudeEstimate;
2use super::Estimator;
3use super::EstimatorCtx;
4use crate::comm::messages::messages::ExternalAttitudeMsg;
5use crate::math::FlightFloat;
6use crate::packets;
7use crate::params::{ParamId, ParamValue, Params};
8
9use nalgebra::{Quaternion, SVector as Vector};
10
11fn gravity<R: FlightFloat>() -> R {
12    <R as FlightFloat>::from_f32(9.80665)
13}
14
15#[derive(Debug, Clone, Copy)]
16pub struct AttitudeState<R: FlightFloat> {
17    pub q_hat: Quaternion<R>,
18    pub q_dot: Quaternion<R>,
19    pub body_rate: Vector<R, 3>,
20    pub b_hat: Vector<R, 3>,
21    pub is_healthy: bool,
22}
23
24impl<R: FlightFloat> Default for AttitudeState<R> {
25    fn default() -> Self {
26        Self {
27            q_hat: Quaternion::new(
28                <R as FlightFloat>::from_f32(1.0),
29                <R as FlightFloat>::from_f32(0.0),
30                <R as FlightFloat>::from_f32(0.0),
31                <R as FlightFloat>::from_f32(0.0),
32            ),
33            q_dot: Quaternion::from(Vector::from([<R as FlightFloat>::from_f32(0.0); 4])),
34            body_rate: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
35            b_hat: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
36            is_healthy: false,
37        }
38    }
39}
40
41impl<R: FlightFloat> AttitudeEstimate for AttitudeState<R> {
42    fn q(&self) -> [f32; 4] {
43        [
44            self.q_hat.w.to_f32_lossy(),
45            self.q_hat.i.to_f32_lossy(),
46            self.q_hat.j.to_f32_lossy(),
47            self.q_hat.k.to_f32_lossy(),
48        ]
49    }
50
51    fn q_dot(&self) -> [f32; 4] {
52        [
53            self.q_dot.w.to_f32_lossy(),
54            self.q_dot.i.to_f32_lossy(),
55            self.q_dot.j.to_f32_lossy(),
56            self.q_dot.k.to_f32_lossy(),
57        ]
58    }
59
60    fn is_healthy(&self) -> bool {
61        self.is_healthy
62    }
63}
64
65impl<R: FlightFloat> From<AttitudeState<R>> for Vector<R, 3> {
66    fn from(state: AttitudeState<R>) -> Self {
67        quaternion_to_euler(state.q_hat)
68    }
69}
70
71impl<'a, R: FlightFloat> From<&'a AttitudeState<R>> for Vector<R, 3> {
72    fn from(state: &'a AttitudeState<R>) -> Self {
73        quaternion_to_euler(state.q_hat)
74    }
75}
76
77pub struct QuadEstimator<R: FlightFloat> {
78    k_p: R,
79    k_i: R,
80    k_p_ext: R,
81    q_hat: Quaternion<R>,
82    q_dot: Quaternion<R>,
83    body_rate: Vector<R, 3>,
84    b_hat: Vector<R, 3>,
85    is_initialized: bool, // Track if we've received first IMU packet
86    last_acc_update_us: u64,
87    last_extatt_update_us: u64,
88
89    // Low-pass filter state
90    accel_lpf: Vector<R, 3>, // Filtered accelerometer
91    gyro_lpf: Vector<R, 3>,  // Filtered gyroscope
92    w1: Vector<R, 3>,
93    w2: Vector<R, 3>,
94    q_extatt: Option<Quaternion<R>>,
95
96    // LPF parameters (EMA alpha values) - matching C defaults
97    alpha_acc: R,     // PARAM_ACC_ALPHA = 0.5 in C
98    alpha_gyro_xy: R, // PARAM_GYRO_XY_ALPHA = 0.3 in C
99    alpha_gyro_z: R,  // PARAM_GYRO_Z_ALPHA = 0.3 in C
100
101    // Accelerometer gating
102    accel_margin: R, // PARAM_FILTER_ACCEL_MARGIN = 0.1 in C
103
104    // Adaptive gains during initialization
105    init_time_us: u64,   // PARAM_INIT_TIME = 3000ms = 3,000,000 μs in C
106    first_imu_time: u64, // Track when first IMU arrived
107    use_acc: bool,
108    use_quad_int: bool,
109    use_mat_exp: bool,
110    fixed_wing: bool,
111}
112
113impl<R: FlightFloat> QuadEstimator<R> {
114    pub fn new(k_p: R, k_i: R) -> Self {
115        Self {
116            k_p,
117            k_i,
118            k_p_ext: <R as FlightFloat>::from_f32(1.5),
119            q_hat: Quaternion::new(
120                <R as FlightFloat>::from_f32(1.0),
121                <R as FlightFloat>::from_f32(0.0),
122                <R as FlightFloat>::from_f32(0.0),
123                <R as FlightFloat>::from_f32(0.0),
124            ),
125            q_dot: Quaternion::from(Vector::from([<R as FlightFloat>::from_f32(0.0); 4])),
126            body_rate: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
127            b_hat: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
128            is_initialized: false,
129            last_acc_update_us: 0,
130            last_extatt_update_us: 0,
131
132            // Initialize LPF state - accel starts at gravity pointing down (NED frame)
133            accel_lpf: Vector::from([
134                <R as FlightFloat>::from_f32(0.0),
135                <R as FlightFloat>::from_f32(0.0),
136                -gravity::<R>(),
137            ]),
138            gyro_lpf: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
139            w1: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
140            w2: Vector::from([<R as FlightFloat>::from_f32(0.0); 3]),
141            q_extatt: None,
142
143            // LPF parameters matching C defaults
144            alpha_acc: <R as FlightFloat>::from_f32(0.5),
145            alpha_gyro_xy: <R as FlightFloat>::from_f32(0.3),
146            alpha_gyro_z: <R as FlightFloat>::from_f32(0.3),
147
148            // Accelerometer gating - ±10% around 1g
149            accel_margin: <R as FlightFloat>::from_f32(0.1),
150
151            // Adaptive gains - 3 second initialization period
152            init_time_us: 3_000_000,
153            first_imu_time: 0,
154            use_acc: true,
155            use_quad_int: true,
156            use_mat_exp: true,
157            fixed_wing: false,
158        }
159    }
160
161    /// Update parameters from the parameter server.
162    /// Call this every loop to read fresh parameter values.
163    pub fn update_params(&mut self, params: &Params) {
164        // Read base gains (not the 10× boosted values)
165        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_FILTER_KP_ACC) {
166            self.k_p = <R as FlightFloat>::from_f32(v);
167        }
168        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_FILTER_KI) {
169            self.k_i = <R as FlightFloat>::from_f32(v);
170        }
171
172        // Read LPF alpha values
173        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_ACC_ALPHA) {
174            self.alpha_acc = <R as FlightFloat>::from_f32(v);
175        }
176        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_GYRO_XY_ALPHA) {
177            self.alpha_gyro_xy = <R as FlightFloat>::from_f32(v);
178        }
179        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_GYRO_Z_ALPHA) {
180            self.alpha_gyro_z = <R as FlightFloat>::from_f32(v);
181        }
182
183        // Read accelerometer gating margin
184        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_FILTER_ACCEL_MARGIN) {
185            self.accel_margin = <R as FlightFloat>::from_f32(v);
186        }
187
188        // Read initialization time (convert milliseconds to microseconds)
189        if let ParamValue::Int(v) = params.get_by_id(ParamId::PARAM_INIT_TIME) {
190            self.init_time_us = (v as u64) * 1000;
191        }
192        if let ParamValue::Float(v) = params.get_by_id(ParamId::PARAM_FILTER_KP_EXT) {
193            self.k_p_ext = <R as FlightFloat>::from_f32(v);
194        }
195        if let ParamValue::Int(v) = params.get_by_id(ParamId::PARAM_FILTER_USE_ACC) {
196            self.use_acc = v != 0;
197        }
198        if let ParamValue::Int(v) = params.get_by_id(ParamId::PARAM_FILTER_USE_QUAD_INT) {
199            self.use_quad_int = v != 0;
200        }
201        if let ParamValue::Int(v) = params.get_by_id(ParamId::PARAM_FILTER_USE_MAT_EXP) {
202            self.use_mat_exp = v != 0;
203        }
204        if let ParamValue::Int(v) = params.get_by_id(ParamId::PARAM_FIXED_WING) {
205            self.fixed_wing = v != 0;
206        }
207    }
208}
209
210impl<R: FlightFloat> Default for QuadEstimator<R> {
211    fn default() -> Self {
212        Self::new(
213            <R as FlightFloat>::from_f32(1.5),
214            <R as FlightFloat>::from_f32(0.05),
215        )
216    }
217}
218
219impl<R: FlightFloat> QuadEstimator<R> {
220    pub fn reset_state(&mut self) {
221        self.q_hat = Quaternion::new(
222            <R as FlightFloat>::from_f32(1.0),
223            <R as FlightFloat>::from_f32(0.0),
224            <R as FlightFloat>::from_f32(0.0),
225            <R as FlightFloat>::from_f32(0.0),
226        );
227        self.q_dot = Quaternion::from(Vector::from([<R as FlightFloat>::from_f32(0.0); 4]));
228        self.body_rate = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
229        self.b_hat = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
230        self.accel_lpf = Vector::from([
231            <R as FlightFloat>::from_f32(0.0),
232            <R as FlightFloat>::from_f32(0.0),
233            -gravity::<R>(),
234        ]);
235        self.gyro_lpf = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
236        self.w1 = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
237        self.w2 = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
238        self.q_extatt = None;
239        self.is_initialized = false;
240        self.last_acc_update_us = 0;
241        self.last_extatt_update_us = 0;
242    }
243
244    pub fn reset_adaptive_bias(&mut self) {
245        self.b_hat = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
246    }
247
248    fn set_external_attitude_update(&mut self, external_attitude: ExternalAttitudeMsg) {
249        let mut q = Quaternion::new(
250            <R as FlightFloat>::from_f32(external_attitude.qw),
251            <R as FlightFloat>::from_f32(external_attitude.qx),
252            <R as FlightFloat>::from_f32(external_attitude.qy),
253            <R as FlightFloat>::from_f32(external_attitude.qz),
254        );
255        q.normalize_mut();
256        self.q_extatt = Some(q);
257    }
258
259    fn estimate_packets(
260        &mut self,
261        imu: Option<packets::ImuPacket<R>>,
262        _mag: Option<packets::MagPacket>,
263        _params: &Params,
264        dt: R,
265    ) -> AttitudeState<R> {
266        if dt < <R as FlightFloat>::from_f32(0.0) {
267            return AttitudeState {
268                q_hat: self.q_hat,
269                q_dot: self.q_dot,
270                body_rate: self.body_rate,
271                b_hat: self.b_hat,
272                is_healthy: false,
273            };
274        }
275
276        if let Some(imu_packet) = imu {
277            // Get current timestamp for initialization tracking
278            let current_time = imu_packet.header.timestamp; // microseconds
279
280            // On first call, just initialize timestamp and skip update
281            if !self.is_initialized {
282                self.first_imu_time = current_time;
283                self.last_acc_update_us = current_time;
284                self.last_extatt_update_us = current_time;
285                self.is_initialized = true;
286                return AttitudeState {
287                    q_hat: self.q_hat,
288                    q_dot: self.q_dot,
289                    body_rate: self.body_rate,
290                    b_hat: self.b_hat,
291                    is_healthy: true,
292                };
293            }
294
295            // Apply low-pass filter to raw measurements (EMA filter)
296            let raw_accel = Vector::from(imu_packet.accel);
297            let one = <R as FlightFloat>::from_f32(1.0);
298            self.accel_lpf[0] =
299                (one - self.alpha_acc) * raw_accel[0] + self.alpha_acc * self.accel_lpf[0];
300            self.accel_lpf[1] =
301                (one - self.alpha_acc) * raw_accel[1] + self.alpha_acc * self.accel_lpf[1];
302            self.accel_lpf[2] =
303                (one - self.alpha_acc) * raw_accel[2] + self.alpha_acc * self.accel_lpf[2];
304
305            let raw_gyro = Vector::from(imu_packet.gyro);
306            self.gyro_lpf[0] =
307                (one - self.alpha_gyro_xy) * raw_gyro[0] + self.alpha_gyro_xy * self.gyro_lpf[0];
308            self.gyro_lpf[1] =
309                (one - self.alpha_gyro_xy) * raw_gyro[1] + self.alpha_gyro_xy * self.gyro_lpf[1];
310            self.gyro_lpf[2] =
311                (one - self.alpha_gyro_z) * raw_gyro[2] + self.alpha_gyro_z * self.gyro_lpf[2];
312
313            // Check if accelerometer magnitude is near 1g (gating)
314            let accel_sqrd_norm = self.accel_lpf[0] * self.accel_lpf[0]
315                + self.accel_lpf[1] * self.accel_lpf[1]
316                + self.accel_lpf[2] * self.accel_lpf[2];
317
318            let margin = self.accel_margin;
319            let g = gravity();
320            let lowerbound = (one - margin) * (one - margin) * g * g;
321            let upperbound = (one + margin) * (one + margin) * g * g;
322            let can_use_accel =
323                self.use_acc && accel_sqrd_norm > lowerbound && accel_sqrd_norm < upperbound;
324
325            let mut kp = <R as FlightFloat>::from_f32(0.0);
326            let mut ki = self.k_i;
327            let mut w_err = Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
328
329            if can_use_accel {
330                w_err = accel_correction(self.q_hat, self.accel_lpf);
331                kp = self.k_p;
332                self.last_acc_update_us = current_time;
333            }
334
335            if let Some(q_extatt) = self.q_extatt.take() {
336                w_err = extatt_correction(self.q_hat, q_extatt);
337                kp = self.k_p_ext;
338                let extatt_dt = <R as FlightFloat>::from_u64(
339                    current_time.saturating_sub(self.last_extatt_update_us),
340                ) * <R as FlightFloat>::from_f32(1e-6);
341                let scale_dt = if dt > <R as FlightFloat>::from_f32(0.0) {
342                    extatt_dt / dt
343                } else {
344                    <R as FlightFloat>::from_f32(0.0)
345                };
346                w_err = w_err * scale_dt;
347                self.last_extatt_update_us = current_time;
348            }
349
350            if current_time < self.init_time_us {
351                kp = self.k_p * <R as FlightFloat>::from_f32(10.0);
352                ki = self.k_i * <R as FlightFloat>::from_f32(10.0);
353            }
354
355            self.b_hat -= w_err * (ki * dt);
356
357            let wbar = self.smoothed_gyro_measurement(self.use_quad_int);
358            let wfinal = wbar - self.b_hat + w_err * kp;
359            self.integrate_angular_rate(wfinal, dt, self.use_mat_exp);
360
361            self.body_rate = self.gyro_lpf - self.b_hat;
362
363            let unhealthy_due_to_accel = self.use_acc
364                && current_time > self.last_acc_update_us + 500_000
365                && !self.fixed_wing;
366            if unhealthy_due_to_accel {
367                return AttitudeState {
368                    q_hat: self.q_hat,
369                    q_dot: self.q_dot,
370                    body_rate: self.body_rate,
371                    b_hat: self.b_hat,
372                    is_healthy: false,
373                };
374            }
375        }
376
377        let q = self.q_hat;
378        let is_healthy = q.w.is_finite_value()
379            && q.i.is_finite_value()
380            && q.j.is_finite_value()
381            && q.k.is_finite_value();
382
383        AttitudeState {
384            q_hat: self.q_hat,
385            q_dot: self.q_dot,
386            body_rate: self.body_rate,
387            b_hat: self.b_hat,
388            is_healthy,
389        }
390    }
391
392    fn smoothed_gyro_measurement(&mut self, use_quad_int: bool) -> Vector<R, 3> {
393        if use_quad_int {
394            let wbar = (self.w2 / <R as FlightFloat>::from_f32(-12.0))
395                + self.w1 * <R as FlightFloat>::from_f32(8.0 / 12.0)
396                + self.gyro_lpf * <R as FlightFloat>::from_f32(5.0 / 12.0);
397            self.w2 = self.w1;
398            self.w1 = self.gyro_lpf;
399            wbar
400        } else {
401            self.gyro_lpf
402        }
403    }
404
405    fn integrate_angular_rate(&mut self, omega: Vector<R, 3>, dt: R, use_mat_exp: bool) {
406        let sqrd_norm_w = omega[0] * omega[0] + omega[1] * omega[1] + omega[2] * omega[2];
407        if sqrd_norm_w == <R as FlightFloat>::from_f32(0.0) {
408            self.q_dot = Quaternion::from(Vector::from([<R as FlightFloat>::from_f32(0.0); 4]));
409            return;
410        }
411
412        let p = omega[0];
413        let q = omega[1];
414        let r = omega[2];
415        let current = self.q_hat;
416
417        self.q_dot = Quaternion::new(
418            <R as FlightFloat>::from_f32(0.5) * (-p * current.i - q * current.j - r * current.k),
419            <R as FlightFloat>::from_f32(0.5) * (p * current.w + r * current.j - q * current.k),
420            <R as FlightFloat>::from_f32(0.5) * (q * current.w - r * current.i + p * current.k),
421            <R as FlightFloat>::from_f32(0.5) * (r * current.w + q * current.i - p * current.j),
422        );
423
424        if use_mat_exp {
425            let norm_w = sqrd_norm_w.sqrt();
426            let half_angle = (norm_w * dt) / <R as FlightFloat>::from_f32(2.0);
427            let t1 = half_angle.cos();
428            let t2 = half_angle.sin() / norm_w;
429            self.q_hat = Quaternion::new(
430                t1 * current.w + t2 * (-p * current.i - q * current.j - r * current.k),
431                t1 * current.i + t2 * (p * current.w + r * current.j - q * current.k),
432                t1 * current.j + t2 * (q * current.w - r * current.i + p * current.k),
433                t1 * current.k + t2 * (r * current.w + q * current.i - p * current.j),
434            );
435        } else {
436            self.q_hat = self.q_hat + self.q_dot * dt;
437        }
438        self.q_hat.normalize_mut();
439    }
440}
441
442impl<R: FlightFloat> Estimator<R> for QuadEstimator<R> {
443    type State = AttitudeState<R>;
444
445    fn estimate(&mut self, ctx: EstimatorCtx<'_, R>) -> Self::State {
446        if let Some(external_attitude) = ctx.external_attitude {
447            self.set_external_attitude_update(external_attitude);
448        }
449        self.estimate_packets(ctx.sensors.imu, ctx.sensors.mag, ctx.params, ctx.dt)
450    }
451
452    fn update_params(&mut self, params: &Params) {
453        QuadEstimator::update_params(self, params);
454    }
455
456    fn reset(&mut self) {
457        self.reset_state();
458    }
459
460    fn reset_adaptive_bias(&mut self) {
461        QuadEstimator::reset_adaptive_bias(self);
462    }
463}
464
465fn accel_correction<R: FlightFloat>(
466    attitude: Quaternion<R>,
467    accel_lpf: Vector<R, 3>,
468) -> Vector<R, 3> {
469    let accel_norm =
470        (accel_lpf[0] * accel_lpf[0] + accel_lpf[1] * accel_lpf[1] + accel_lpf[2] * accel_lpf[2])
471            .sqrt();
472    if accel_norm <= <R as FlightFloat>::from_f32(1e-9) {
473        return Vector::from([<R as FlightFloat>::from_f32(0.0); 3]);
474    }
475
476    let ax = accel_lpf[0] / accel_norm;
477    let ay = accel_lpf[1] / accel_norm;
478    let az = accel_lpf[2] / accel_norm;
479
480    let one = <R as FlightFloat>::from_f32(1.0);
481    let mut q_acc_w = one - az;
482    let mut q_acc_x = ay;
483    let mut q_acc_y = -ax;
484    let mut q_acc_z = <R as FlightFloat>::from_f32(0.0);
485    if -az < <R as FlightFloat>::from_f32(-0.999_999) {
486        q_acc_w = <R as FlightFloat>::from_f32(0.0);
487        q_acc_x = one;
488        q_acc_y = <R as FlightFloat>::from_f32(0.0);
489    }
490    let q_acc_norm =
491        (q_acc_w * q_acc_w + q_acc_x * q_acc_x + q_acc_y * q_acc_y + q_acc_z * q_acc_z).sqrt();
492    if q_acc_norm > <R as FlightFloat>::from_f32(0.0) {
493        q_acc_w /= q_acc_norm;
494        q_acc_x /= q_acc_norm;
495        q_acc_y /= q_acc_norm;
496        q_acc_z /= q_acc_norm;
497    }
498
499    let q_tilde_w =
500        q_acc_w * attitude.w - q_acc_x * attitude.i - q_acc_y * attitude.j - q_acc_z * attitude.k;
501    let q_tilde_i =
502        q_acc_w * attitude.i + q_acc_x * attitude.w - q_acc_y * attitude.k + q_acc_z * attitude.j;
503    let q_tilde_j =
504        q_acc_w * attitude.j + q_acc_x * attitude.k + q_acc_y * attitude.w - q_acc_z * attitude.i;
505    Vector::from([
506        <R as FlightFloat>::from_f32(-2.0) * q_tilde_w * q_tilde_i,
507        <R as FlightFloat>::from_f32(-2.0) * q_tilde_w * q_tilde_j,
508        <R as FlightFloat>::from_f32(0.0),
509    ])
510}
511
512fn extatt_correction<R: FlightFloat>(
513    attitude: Quaternion<R>,
514    external: Quaternion<R>,
515) -> Vector<R, 3> {
516    let (xhat, yhat, zhat) = quaternion_to_dcm_rows(attitude);
517    let (xext, yext, zext) = quaternion_to_dcm_rows(external);
518    xext.cross(&xhat) + yext.cross(&yhat) + zext.cross(&zhat)
519}
520
521fn quaternion_to_euler<R: FlightFloat>(q: Quaternion<R>) -> Vector<R, 3> {
522    let two = <R as FlightFloat>::from_f32(2.0);
523    let one = <R as FlightFloat>::from_f32(1.0);
524    let minus_one = <R as FlightFloat>::from_f32(-1.0);
525
526    let sin_roll = two * (q.w * q.i + q.j * q.k);
527    let cos_roll = one - two * (q.i * q.i + q.j * q.j);
528    let roll = sin_roll.atan2(cos_roll);
529
530    let sin_pitch = two * (q.w * q.j - q.k * q.i);
531    let pitch = sin_pitch.clamp(minus_one, one).asin();
532
533    let sin_yaw = two * (q.w * q.k + q.i * q.j);
534    let cos_yaw = one - two * (q.j * q.j + q.k * q.k);
535    let yaw = sin_yaw.atan2(cos_yaw);
536
537    Vector::from([roll, pitch, yaw])
538}
539
540fn quaternion_to_dcm_rows<R: FlightFloat>(
541    q: Quaternion<R>,
542) -> (Vector<R, 3>, Vector<R, 3>, Vector<R, 3>) {
543    let w = q.w;
544    let x = q.i;
545    let y = q.j;
546    let z = q.k;
547    (
548        Vector::from([
549            <R as FlightFloat>::from_f32(1.0) - <R as FlightFloat>::from_f32(2.0) * (y * y + z * z),
550            <R as FlightFloat>::from_f32(2.0) * (x * y - z * w),
551            <R as FlightFloat>::from_f32(2.0) * (x * z + y * w),
552        ]),
553        Vector::from([
554            <R as FlightFloat>::from_f32(2.0) * (x * y + z * w),
555            <R as FlightFloat>::from_f32(1.0) - <R as FlightFloat>::from_f32(2.0) * (x * x + z * z),
556            <R as FlightFloat>::from_f32(2.0) * (y * z - x * w),
557        ]),
558        Vector::from([
559            <R as FlightFloat>::from_f32(2.0) * (x * z - y * w),
560            <R as FlightFloat>::from_f32(2.0) * (y * z + x * w),
561            <R as FlightFloat>::from_f32(1.0) - <R as FlightFloat>::from_f32(2.0) * (x * x + y * y),
562        ]),
563    )
564}
565
566#[cfg(test)]
567mod tests {
568    use super::*;
569    use crate::{
570        comm::messages::messages::ExternalAttitudeMsg,
571        estimator::Estimator,
572        packets::{ImuPacket, RosflightPacketHeader},
573        sensors::ProcessedSensors,
574    };
575
576    #[test]
577    fn accel_correction_matches_rosflight_turbomath_convention_with_coupled_attitude() {
578        // Upstream turbomath uses the opposite quaternion cross-term convention
579        // from nalgebra. This coupled yaw/tilt case exercises all terms that
580        // differ; axis-aligned tests cannot detect the convention mismatch.
581        let attitude = Quaternion::new(
582            0.923_380_516_9_f64,
583            0.102_597_835_2,
584            -0.153_896_752_8,
585            0.307_793_505_6,
586        );
587
588        let correction = accel_correction(attitude, Vector::from([2.0, -1.0, -9.5]));
589
590        assert!((correction[0] - -0.156_011_43).abs() < 1e-8);
591        assert!((correction[1] - 0.478_690_38).abs() < 1e-8);
592        assert_eq!(correction[2], 0.0);
593    }
594
595    fn estimate(
596        estimator: &mut QuadEstimator<f64>,
597        sensors: &ProcessedSensors<f64>,
598        params: &Params,
599        dt: f64,
600    ) -> AttitudeState<f64> {
601        estimator.update_params(params);
602        estimator.estimate(EstimatorCtx {
603            sensors,
604            params,
605            dt,
606            external_attitude: None,
607        })
608    }
609
610    fn estimate_with_external_attitude(
611        estimator: &mut QuadEstimator<f64>,
612        sensors: &ProcessedSensors<f64>,
613        params: &Params,
614        dt: f64,
615        external_attitude: ExternalAttitudeMsg,
616    ) -> AttitudeState<f64> {
617        estimator.update_params(params);
618        estimator.estimate(EstimatorCtx {
619            sensors,
620            params,
621            dt,
622            external_attitude: Some(external_attitude),
623        })
624    }
625
626    #[test]
627    fn estimator_applies_external_attitude_as_correction_not_replacement() {
628        let mut params = Params::new();
629        params.set_by_id(ParamId::PARAM_FILTER_USE_ACC, ParamValue::Int(0));
630        params.set_by_id(ParamId::PARAM_FILTER_USE_QUAD_INT, ParamValue::Int(0));
631        params.set_by_id(ParamId::PARAM_FILTER_USE_MAT_EXP, ParamValue::Int(0));
632        params.set_by_id(ParamId::PARAM_INIT_TIME, ParamValue::Int(0));
633        params.set_by_id(ParamId::PARAM_FILTER_KP_EXT, ParamValue::Float(1.5));
634        let mut sensors = ProcessedSensors::<f64>::default();
635        sensors.imu = Some(ImuPacket {
636            header: RosflightPacketHeader {
637                timestamp: 1_000,
638                status: 0,
639            },
640            accel: [0.0, 0.0, -9.80665],
641            gyro: [0.0, 0.0, 0.0],
642            temperature: 25.0,
643            seq: 1,
644        });
645
646        let mut estimator = QuadEstimator::default();
647        let _ = estimate(&mut estimator, &sensors, &params, 1.0 / 400.0);
648        sensors.imu.as_mut().unwrap().header.timestamp = 3_000;
649        let state = estimate_with_external_attitude(
650            &mut estimator,
651            &sensors,
652            &params,
653            1.0 / 400.0,
654            ExternalAttitudeMsg {
655                qw: core::f32::consts::FRAC_1_SQRT_2,
656                qx: core::f32::consts::FRAC_1_SQRT_2,
657                qy: 0.0,
658                qz: 0.0,
659            },
660        );
661
662        assert_ne!(state.q(), [0.0, 1.0, 0.0, 0.0]);
663        assert!(state.q()[0] < 1.0);
664        assert!(state.q()[1] > 0.0);
665        assert!(state.is_healthy());
666    }
667
668    #[test]
669    fn estimator_reports_unhealthy_after_accel_correction_timeout() {
670        let mut params = Params::new();
671        params.set_by_id(ParamId::PARAM_FILTER_USE_ACC, ParamValue::Int(1));
672        params.set_by_id(ParamId::PARAM_FIXED_WING, ParamValue::Int(0));
673        params.set_by_id(ParamId::PARAM_INIT_TIME, ParamValue::Int(0));
674        let mut estimator = QuadEstimator::default();
675        let mut sensors = ProcessedSensors::<f64>::default();
676        sensors.imu = Some(ImuPacket {
677            header: RosflightPacketHeader {
678                timestamp: 1_000,
679                status: 0,
680            },
681            accel: [0.0, 0.0, -9.80665],
682            gyro: [0.0, 0.0, 0.0],
683            ..Default::default()
684        });
685
686        let _ = estimate(&mut estimator, &sensors, &params, 0.002);
687        sensors.imu = Some(ImuPacket {
688            header: RosflightPacketHeader {
689                timestamp: 601_001,
690                status: 0,
691            },
692            accel: [20.0, 0.0, 0.0],
693            gyro: [0.0, 0.0, 0.0],
694            ..Default::default()
695        });
696
697        let state = estimate(&mut estimator, &sensors, &params, 0.002);
698
699        assert!(!state.is_healthy());
700    }
701
702    #[test]
703    fn fixedwing_flag_keeps_attitude_estimator_healthy_on_accel_correction_timeout() {
704        let mut params = Params::new();
705        params.set_by_id(ParamId::PARAM_FILTER_USE_ACC, ParamValue::Int(1));
706        params.set_by_id(ParamId::PARAM_FIXED_WING, ParamValue::Int(1));
707        params.set_by_id(ParamId::PARAM_INIT_TIME, ParamValue::Int(0));
708        let mut estimator = QuadEstimator::default();
709        let mut sensors = ProcessedSensors::<f64>::default();
710        sensors.imu = Some(ImuPacket {
711            header: RosflightPacketHeader {
712                timestamp: 1_000,
713                status: 0,
714            },
715            accel: [0.0, 0.0, -9.80665],
716            gyro: [0.0, 0.0, 0.0],
717            ..Default::default()
718        });
719
720        let _ = estimate(&mut estimator, &sensors, &params, 0.002);
721        sensors.imu = Some(ImuPacket {
722            header: RosflightPacketHeader {
723                timestamp: 601_001,
724                status: 0,
725            },
726            accel: [20.0, 0.0, 0.0],
727            gyro: [0.0, 0.0, 0.0],
728            ..Default::default()
729        });
730
731        let state = estimate(&mut estimator, &sensors, &params, 0.002);
732
733        assert!(state.is_healthy());
734    }
735
736    #[test]
737    fn reset_reinitializes_attitude_from_the_next_imu_sample() {
738        let mut params = Params::new();
739        params.set_by_id(ParamId::PARAM_FILTER_USE_ACC, ParamValue::Int(0));
740        params.set_by_id(ParamId::PARAM_FILTER_USE_QUAD_INT, ParamValue::Int(0));
741        params.set_by_id(ParamId::PARAM_FILTER_USE_MAT_EXP, ParamValue::Int(1));
742        params.set_by_id(ParamId::PARAM_INIT_TIME, ParamValue::Int(0));
743        params.set_by_id(ParamId::PARAM_GYRO_Z_ALPHA, ParamValue::Float(0.0));
744        let mut estimator = QuadEstimator::default();
745        let mut sensors = ProcessedSensors::<f64>::default();
746        sensors.imu = Some(ImuPacket {
747            header: RosflightPacketHeader {
748                timestamp: 1_000,
749                status: 0,
750            },
751            accel: [0.0, 0.0, -9.80665],
752            gyro: [0.0, 0.0, 1.0],
753            ..Default::default()
754        });
755
756        let _ = estimate(&mut estimator, &sensors, &params, 0.1);
757        sensors.imu.as_mut().unwrap().header.timestamp = 101_000;
758        let drifted = estimate(&mut estimator, &sensors, &params, 0.1);
759        assert!(drifted.q()[3] > 0.0);
760
761        estimator.reset();
762        sensors.imu.as_mut().unwrap().header.timestamp = 201_000;
763        let reset = estimate(&mut estimator, &sensors, &params, 0.1);
764
765        assert_eq!(reset.q(), [1.0, 0.0, 0.0, 0.0]);
766        assert!(reset.is_healthy());
767    }
768
769    #[test]
770    fn quadratic_interpolation_delays_gyro_rate_like_rosflight() {
771        let mut params = Params::new();
772        params.set_by_id(ParamId::PARAM_FILTER_USE_ACC, ParamValue::Int(0));
773        params.set_by_id(ParamId::PARAM_FILTER_USE_QUAD_INT, ParamValue::Int(1));
774        params.set_by_id(ParamId::PARAM_FILTER_USE_MAT_EXP, ParamValue::Int(0));
775        params.set_by_id(ParamId::PARAM_INIT_TIME, ParamValue::Int(0));
776        let mut estimator = QuadEstimator::default();
777        let mut sensors = ProcessedSensors::<f64>::default();
778        sensors.imu = Some(ImuPacket {
779            header: RosflightPacketHeader {
780                timestamp: 1_000,
781                status: 0,
782            },
783            accel: [0.0, 0.0, -9.80665],
784            gyro: [0.0, 0.0, 1.0],
785            ..Default::default()
786        });
787
788        let _ = estimate(&mut estimator, &sensors, &params, 0.002);
789        sensors.imu.as_mut().unwrap().header.timestamp = 3_000;
790        let state = estimate(&mut estimator, &sensors, &params, 0.002);
791
792        assert!(state.q()[3] > 0.0);
793        assert!(state.q()[3] < 0.001);
794    }
795
796    #[test]
797    fn matrix_exponential_integration_matches_constant_yaw_rate() {
798        let mut params = Params::new();
799        params.set_by_id(ParamId::PARAM_FILTER_USE_ACC, ParamValue::Int(0));
800        params.set_by_id(ParamId::PARAM_FILTER_USE_QUAD_INT, ParamValue::Int(0));
801        params.set_by_id(ParamId::PARAM_FILTER_USE_MAT_EXP, ParamValue::Int(1));
802        params.set_by_id(ParamId::PARAM_INIT_TIME, ParamValue::Int(0));
803        params.set_by_id(ParamId::PARAM_GYRO_Z_ALPHA, ParamValue::Float(0.0));
804        let mut estimator = QuadEstimator::default();
805        let mut sensors = ProcessedSensors::<f64>::default();
806        sensors.imu = Some(ImuPacket {
807            header: RosflightPacketHeader {
808                timestamp: 1_000,
809                status: 0,
810            },
811            accel: [0.0, 0.0, -9.80665],
812            gyro: [0.0, 0.0, 1.0],
813            ..Default::default()
814        });
815
816        let _ = estimate(&mut estimator, &sensors, &params, 0.1);
817        sensors.imu.as_mut().unwrap().header.timestamp = 101_000;
818        let state = estimate(&mut estimator, &sensors, &params, 0.1);
819
820        assert!((state.q()[0] as f64 - 0.05_f64.cos()).abs() < 1e-6);
821        assert!((state.q()[3] as f64 - 0.05_f64.sin()).abs() < 1e-6);
822    }
823}
veloxity_core/estimator/quad.rs

veloxity_core/estimator/
quad.rs
