#![allow(non_snake_case)]
#![allow(clippy::many_single_char_names)]
use crate::ahrs::Ahrs;
use core::hash;
use nalgebra::{Matrix4, Matrix6, Quaternion, Scalar, Vector2, Vector3, Vector4, Vector6};
use simba::simd::{SimdRealField, SimdValue};
#[derive(Debug)]
pub struct Madgwick<N: Scalar + SimdValue> {
sample_period: N,
beta: N,
pub quat: Quaternion<N>,
}
impl<N: SimdRealField + Eq> Eq for Madgwick<N> where N::Element: SimdRealField {}
impl<N: SimdRealField> PartialEq for Madgwick<N>
where
N::Element: SimdRealField,
{
fn eq(&self, rhs: &Self) -> bool {
self.sample_period == rhs.sample_period && self.beta == rhs.beta && self.quat == rhs.quat
}
}
impl<N: SimdRealField + hash::Hash> hash::Hash for Madgwick<N> {
fn hash<H: hash::Hasher>(&self, state: &mut H) {
self.sample_period.hash(state);
self.beta.hash(state);
self.quat.hash(state);
}
}
impl<N: Scalar + Copy + SimdValue> Copy for Madgwick<N> {}
impl<N: Scalar + SimdValue> Clone for Madgwick<N> {
#[inline]
fn clone(&self) -> Self {
let sample_period = self.sample_period.clone();
let beta = self.beta.clone();
let quat = self.quat.clone();
Madgwick {
sample_period,
beta,
quat,
}
}
}
impl Default for Madgwick<f64> {
fn default() -> Madgwick<f64> {
Madgwick {
sample_period: (1.0f64) / (256.0),
beta: 0.1f64,
quat: Quaternion::new(1.0f64, 0.0, 0.0, 0.0),
}
}
}
impl<N: Scalar + SimdValue + num_traits::One + num_traits::Zero> Madgwick<N> {
pub fn new(sample_period: N, beta: N) -> Self {
Madgwick::new_with_quat(
sample_period,
beta,
Quaternion::new(N::one(), N::zero(), N::zero(), N::zero()),
)
}
pub fn new_with_quat(sample_period: N, beta: N, quat: Quaternion<N>) -> Self {
Madgwick {
sample_period,
beta,
quat,
}
}
}
#[cfg(feature = "field_access")]
impl<N: Scalar + SimdValue + Copy> Madgwick<N> {
pub fn sample_period(&self) -> N {
self.sample_period
}
pub fn sample_period_mut(&mut self) -> &mut N {
&mut self.sample_period
}
pub fn beta(&self) -> N {
self.beta
}
pub fn beta_mut(&mut self) -> &mut N {
&mut self.beta
}
pub fn quat(&self) -> Quaternion<N> {
self.quat
}
pub fn quat_mut(&mut self) -> &mut Quaternion<N> {
&mut self.quat
}
}
impl<N: simba::scalar::RealField> Ahrs<N> for Madgwick<N> {
fn update(
&mut self,
gyroscope: &Vector3<N>,
accelerometer: &Vector3<N>,
magnetometer: &Vector3<N>,
) -> Result<&Quaternion<N>, &str> {
let q = self.quat;
let zero: N = nalgebra::zero();
let two: N = nalgebra::convert(2.0);
let four: N = nalgebra::convert(4.0);
let half: N = nalgebra::convert(0.5);
let accel = match accelerometer.try_normalize(zero) {
Some(n) => n,
None => return Err("Accelerometer norm divided by zero."),
};
let mag = match magnetometer.try_normalize(zero) {
Some(n) => n,
None => {
return Err("Magnetometer norm divided by zero.");
}
};
let h = q * (Quaternion::from_parts(zero, mag) * q.conjugate());
let b = Quaternion::new(zero, Vector2::new(h[0], h[1]).norm(), zero, h[2]);
#[rustfmt::skip]
let F = Vector6::new(
two*( q[0]*q[2] - q[3]*q[1]) - accel[0],
two*( q[3]*q[0] + q[1]*q[2]) - accel[1],
two*(half - q[0]*q[0] - q[1]*q[1]) - accel[2],
two*b[0]*(half - q[1]*q[1] - q[2]*q[2]) + two*b[2]*(q[0]*q[2] - q[3]*q[1]) - mag[0],
two*b[0]*(q[0]*q[1] - q[3]*q[2]) + two*b[2]*( q[3]*q[0] + q[1]*q[2]) - mag[1],
two*b[0]*(q[3]*q[1] + q[0]*q[2]) + two*b[2]*(half - q[0]*q[0] - q[1]*q[1]) - mag[2]
);
#[rustfmt::skip]
let J_t = Matrix6::new(
-two*q[1], two*q[0], zero, -two*b[2]*q[1], -two*b[0]*q[2]+two*b[2]*q[0], two*b[0]*q[1],
two*q[2], two*q[3], -four*q[0], two*b[2]*q[2], two*b[0]*q[1]+two*b[2]*q[3], two*b[0]*q[2]-four*b[2]*q[0],
-two*q[3], two*q[2], -four*q[1], -four*b[0]*q[1]-two*b[2]*q[3], two*b[0]*q[0]+two*b[2]*q[2], two*b[0]*q[3]-four*b[2]*q[1],
two*q[0], two*q[1], zero, -four*b[0]*q[2]+two*b[2]*q[0], -two*b[0]*q[3]+two*b[2]*q[1], two*b[0]*q[0],
zero, zero, zero, zero, zero, zero,
zero, zero, zero, zero, zero, zero
);
let step = (J_t * F).normalize();
let qDot = q * Quaternion::from_parts(zero, *gyroscope) * half
- Quaternion::new(step[0], step[1], step[2], step[3]) * self.beta;
self.quat = (q + qDot * self.sample_period).normalize();
Ok(&self.quat)
}
fn update_imu(
&mut self,
gyroscope: &Vector3<N>,
accelerometer: &Vector3<N>,
) -> Result<&Quaternion<N>, &str> {
let q = self.quat;
let zero: N = nalgebra::zero();
let two: N = nalgebra::convert(2.0);
let four: N = nalgebra::convert(4.0);
let half: N = nalgebra::convert(0.5);
let accel = match accelerometer.try_normalize(zero) {
Some(n) => n,
None => {
return Err("Accelerator norm divided by zero.");
}
};
#[rustfmt::skip]
let F = Vector4::new(
two*( q[0]*q[2] - q[3]*q[1]) - accel[0],
two*( q[3]*q[0] + q[1]*q[2]) - accel[1],
two*(half - q[0]*q[0] - q[1]*q[1]) - accel[2],
zero
);
#[rustfmt::skip]
let J_t = Matrix4::new(
-two*q[1], two*q[0], zero, zero,
two*q[2], two*q[3], -four*q[0], zero,
-two*q[3], two*q[2], -four*q[1], zero,
two*q[0], two*q[1], zero, zero
);
let step = (J_t * F).normalize();
let qDot = (q * Quaternion::from_parts(zero, *gyroscope)) * half
- Quaternion::new(step[0], step[1], step[2], step[3]) * self.beta;
self.quat = (q + qDot * self.sample_period).normalize();
Ok(&self.quat)
}
}