ahrs/
mahony.rs

1#![allow(non_snake_case)]
2#![allow(clippy::many_single_char_names)]
3
4use crate::ahrs::{Ahrs, AhrsError};
5use core::hash;
6use nalgebra::{Quaternion, Scalar, UnitQuaternion, Vector2, Vector3};
7use simba::simd::{SimdRealField as RealField, SimdRealField, SimdValue};
8
9/// Mahony AHRS implementation.
10///
11/// # Example
12/// ```
13/// # use ahrs::Mahony;
14/// let mut ahrs = Mahony::new(0.002390625f64, 0.5, 0.0);
15/// println!("mahony filter: {:?}", ahrs);
16///
17/// // Can now process IMU data using `Ahrs::update_imu`, etc.
18/// ```
19#[derive(Debug)]
20pub struct Mahony<N: Scalar + SimdValue + Copy> {
21    /// Expected sampling period, in seconds.
22    sample_period: N,
23    /// Proportional filter gain constant.
24    kp: N,
25    /// Integral filter gain constant.
26    ki: N,
27    /// Integral error vector.
28    e_int: Vector3<N>,
29    /// Filter state quaternion.
30    pub quat: UnitQuaternion<N>,
31}
32
33impl<N: SimdRealField + Eq + Copy> Eq for Mahony<N> where N::Element: SimdRealField {}
34
35impl<N: SimdRealField + Copy> PartialEq for Mahony<N>
36where
37    N::Element: SimdRealField,
38{
39    fn eq(&self, rhs: &Self) -> bool {
40        self.sample_period == rhs.sample_period
41            && self.kp == rhs.kp
42            && self.ki == rhs.ki
43            && self.e_int == rhs.e_int
44            && self.quat == rhs.quat
45    }
46}
47
48impl<N: SimdRealField + hash::Hash + Copy> hash::Hash for Mahony<N> {
49    fn hash<H: hash::Hasher>(&self, state: &mut H) {
50        self.sample_period.hash(state);
51        self.kp.hash(state);
52        self.ki.hash(state);
53        self.e_int.hash(state);
54        self.quat.hash(state);
55    }
56}
57
58impl<N: Scalar + Copy + SimdValue> Copy for Mahony<N> {}
59
60impl<N: Scalar + SimdValue + Copy> Clone for Mahony<N> {
61    #[inline]
62    fn clone(&self) -> Self {
63        *self
64    }
65}
66
67impl Default for Mahony<f64> {
68    /// Creates a default `Mahony` AHRS instance with default filter parameters.
69    ///
70    /// ```
71    /// # use ahrs::Mahony;
72    /// # use nalgebra::{Quaternion, Vector4};
73    /// dbg!(Mahony::default());
74    ///
75    /// // prints (roughly):
76    /// //
77    /// // Madgwick {
78    /// //     sample_period: 1.0f64/256.0,
79    /// //     kp: 0.5f64,
80    /// //     ki: 0.0f64,
81    /// //     e_int: Vector3 { x: 0.0f64, y: 0.0, z: 0.0 },
82    /// //     quat: Quaternion { w: 1.0f64, i: 0.0, j: 0.0, k: 0.0 }
83    /// // };
84    /// ```
85    fn default() -> Mahony<f64> {
86        Mahony {
87            sample_period: (1.0f64) / (256.0),
88            kp: 0.5f64,
89            ki: 0.0f64,
90            e_int: Vector3::new(0.0, 0.0, 0.0),
91            quat: UnitQuaternion::new_unchecked(Quaternion::new(1.0f64, 0.0, 0.0, 0.0)),
92        }
93    }
94}
95
96impl<N: RealField + Copy> Mahony<N> {
97    /// Creates a new Mahony AHRS instance with identity quaternion.
98    ///
99    /// # Arguments
100    ///
101    /// * `sample_period` - The expected sensor sampling period in seconds.
102    /// * `kp` - Proportional filter gain constant.
103    /// * `ki` - Integral filter gain constant.
104    pub fn new(sample_period: N, kp: N, ki: N) -> Self {
105        Mahony::new_with_quat(
106            sample_period,
107            kp,
108            ki,
109            UnitQuaternion::new_unchecked(Quaternion::new(
110                N::one(),
111                N::zero(),
112                N::zero(),
113                N::zero(),
114            )),
115        )
116    }
117
118    /// Creates a new Mahony AHRS instance with given quaternion.
119    ///
120    /// # Arguments
121    ///
122    /// * `sample_period` - The expected sensor sampling period in seconds.
123    /// * `kp` - Proportional filter gain constant.
124    /// * `ki` - Integral filter gain constant.
125    /// * `quat` - Existing filter state quaternion.
126    pub fn new_with_quat(sample_period: N, kp: N, ki: N, quat: UnitQuaternion<N>) -> Self {
127        Mahony {
128            sample_period,
129            kp,
130            ki,
131            e_int: nalgebra::zero(),
132            quat,
133        }
134    }
135}
136
137#[cfg(feature = "field_access")]
138impl<N: Scalar + SimdValue + Copy> Mahony<N> {
139    /// Expected sampling period, in seconds.
140    pub fn sample_period(&self) -> N {
141        self.sample_period
142    }
143
144    /// Mutable reference to expected sampling period, in seconds.
145    pub fn sample_period_mut(&mut self) -> &mut N {
146        &mut self.sample_period
147    }
148
149    /// Proportional filter gain constant.
150    pub fn kp(&self) -> N {
151        self.kp
152    }
153
154    /// Mutable reference to proportional filter gain constant.
155    pub fn kp_mut(&mut self) -> &mut N {
156        &mut self.kp
157    }
158
159    /// Integral filter gain constant.
160    pub fn ki(&self) -> N {
161        self.ki
162    }
163
164    /// Mutable reference to integral filter gain constant.
165    pub fn ki_mut(&mut self) -> &mut N {
166        &mut self.ki
167    }
168
169    /// Integral error vector.
170    pub fn e_int(&self) -> Vector3<N> {
171        self.e_int
172    }
173
174    /// Mutable reference to integral error vector.
175    pub fn e_int_mut(&mut self) -> &mut Vector3<N> {
176        &mut self.e_int
177    }
178
179    /// Filter state quaternion.
180    pub fn quat(&self) -> UnitQuaternion<N> {
181        self.quat
182    }
183
184    /// Mutable reference to filter state quaternion.
185    pub fn quat_mut(&mut self) -> &mut UnitQuaternion<N> {
186        &mut self.quat
187    }
188}
189
190impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Mahony<N> {
191    fn update(
192        &mut self,
193        gyroscope: &Vector3<N>,
194        accelerometer: &Vector3<N>,
195        magnetometer: &Vector3<N>,
196    ) -> Result<&UnitQuaternion<N>, AhrsError> {
197        let q = self.quat.as_ref();
198
199        let zero: N = nalgebra::zero();
200        let two: N = nalgebra::convert(2.0);
201        let half: N = nalgebra::convert(0.5);
202
203        // Normalize accelerometer measurement
204        let Some(accel) = accelerometer.try_normalize(zero) else {
205            return Err(AhrsError::AccelerometerNormZero);
206        };
207
208        // Normalize magnetometer measurement
209        let Some(mag) = magnetometer.try_normalize(zero) else {
210            return Err(AhrsError::MagnetometerNormZero);
211        };
212
213        // Reference direction of Earth's magnetic field (Quaternion should still be conj of q)
214        let h = q * (Quaternion::from_parts(zero, mag) * q.conjugate());
215        let b = Quaternion::new(zero, Vector2::new(h[0], h[1]).norm(), zero, h[2]);
216
217        #[rustfmt::skip]
218        let v = Vector3::new(
219            two*( q[0]*q[2] - q[3]*q[1] ),
220            two*( q[3]*q[0] + q[1]*q[2] ),
221            q[3]*q[3] - q[0]*q[0] - q[1]*q[1] + q[2]*q[2]
222        );
223
224        #[rustfmt::skip]
225        let w = Vector3::new(
226            two*b[0]*(half - q[1]*q[1] - q[2]*q[2]) + two*b[2]*(q[0]*q[2] - q[3]*q[1]),
227            two*b[0]*(q[0]*q[1] - q[3]*q[2])        + two*b[2]*(q[3]*q[0] + q[1]*q[2]),
228            two*b[0]*(q[3]*q[1] + q[0]*q[2])        + two*b[2]*(half - q[0]*q[0] - q[1]*q[1])
229        );
230
231        // Error is sum of cross product between estimated direction and measured direction of fields
232        let e: Vector3<N> = accel.cross(&v) + mag.cross(&w);
233
234        // Integrate error
235        self.e_int += e * self.sample_period;
236
237        // Apply feedback terms
238        let gyro = *gyroscope + e * self.kp + self.e_int * self.ki;
239
240        // Compute rate of change of quaternion
241        let qDot = q * Quaternion::from_parts(zero, gyro) * half;
242
243        // Integrate to yield quaternion
244        self.quat = UnitQuaternion::from_quaternion(q + qDot * self.sample_period);
245
246        Ok(&self.quat)
247    }
248
249    fn update_imu(
250        &mut self,
251        gyroscope: &Vector3<N>,
252        accelerometer: &Vector3<N>,
253    ) -> Result<&UnitQuaternion<N>, AhrsError> {
254        let q = self.quat.as_ref();
255
256        let zero: N = nalgebra::zero();
257        let two: N = nalgebra::convert(2.0);
258        let half: N = nalgebra::convert(0.5);
259
260        // Normalize accelerometer measurement
261        let Some(accel) = accelerometer.try_normalize(zero) else {
262            return Err(AhrsError::AccelerometerNormZero);
263        };
264
265        #[rustfmt::skip]
266        let v = Vector3::new(
267            two*( q[0]*q[2] - q[3]*q[1] ),
268            two*( q[3]*q[0] + q[1]*q[2] ),
269            q[3]*q[3] - q[0]*q[0] - q[1]*q[1] + q[2]*q[2]
270        );
271
272        // Error is estimated direction direction of fields
273        let e = accel.cross(&v);
274
275        // Integrate error
276        self.e_int += e * self.sample_period;
277
278        // Apply feedback terms
279        let gyro = *gyroscope + e * self.kp + self.e_int * self.ki;
280
281        // Compute rate of change of quaternion
282        let qDot = q * Quaternion::from_parts(zero, gyro) * half;
283
284        // Integrate to yield quaternion
285        self.quat = UnitQuaternion::from_quaternion(q + qDot * self.sample_period);
286
287        Ok(&self.quat)
288    }
289
290    fn update_gyro(
291        &mut self,
292        gyroscope: &Vector3<N>
293    ) -> &UnitQuaternion<N> {
294        let q = self.quat.as_ref();
295
296        let zero: N = nalgebra::zero();
297        let half: N = nalgebra::convert(0.5);
298
299        // Compute rate of change for quaternion
300        let qDot = q * Quaternion::from_parts(zero, *gyroscope) * half;
301
302        // Integrate to yield quaternion
303        self.quat = UnitQuaternion::from_quaternion(q + qDot * self.sample_period);
304
305        &self.quat
306    }
307}
ahrs/mahony.rs

ahrs/
mahony.rs
