From 5c0199cb1a14823320a5d0e300cdb2c1cf0f933f Mon Sep 17 00:00:00 2001 From: peng Date: Wed, 8 May 2024 17:09:17 -0400 Subject: [PATCH] add Fusion add NaiveCF as the default Fusion impl, demonstrated by an example glasses discovery process has more verbose logging add rustfmt --- Cargo.toml | 4 +- examples/sensor_fusion.rs | 25 +++++ rustfmt.toml | 10 ++ src/lib.rs | 110 ++++++++++++++---- src/naive_cf.rs | 229 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 355 insertions(+), 23 deletions(-) create mode 100644 examples/sensor_fusion.rs create mode 100644 rustfmt.toml create mode 100644 src/naive_cf.rs diff --git a/Cargo.toml b/Cargo.toml index 99f0cf5..102fe71 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,13 +22,13 @@ rokid = ["rusb"] [dependencies] bytemuck = { version = "1.13.1", optional = true } byteorder = "1.4" -nalgebra = { version = "0.32.3", default-features=false, features = ["std"]} +nalgebra = { version = "0.32.3", default-features = false, features = ["std"] } rusb = { version = "0.9.2", optional = true } serialport = { version = "4.2", optional = true } tinyjson = { version = "2.5.1", optional = true } [target.'cfg(target_os = "android")'.dependencies] -hidapi = { version = "2.4.1", default-features=false, features = [ "linux-static-libusb" ], optional = true } +hidapi = { version = "2.4.1", default-features = false, features = ["linux-static-libusb"], optional = true } [target.'cfg(not(target_os = "android"))'.dependencies] hidapi = { version = "2.4.1", optional = true } diff --git a/examples/sensor_fusion.rs b/examples/sensor_fusion.rs new file mode 100644 index 0000000..283fa6a --- /dev/null +++ b/examples/sensor_fusion.rs @@ -0,0 +1,25 @@ +// Copyright (C) 2023, Alex Badics +// This file is part of ar-drivers-rs +// Licensed under the MIT license. See LICENSE file in the project root for details. + +use ar_drivers::any_fusion; + +fn main() { + let mut fusion = any_fusion().unwrap(); // Declare conn as mutable + + let serial = fusion.glasses().serial().unwrap(); + println!("Got glasses, serial={}", serial); + + println!(""); + loop { + fusion.update(); + let quaternion = fusion.attitude_quaternion(); + let frd = fusion.attitude_frd_deg(); + let inconsistency = fusion.inconsistency(); + + println!("quaternion:\t{:10.7}", quaternion); + println!("euler:\t{:10.7}", frd.transpose()); + + println!("inconsistency:\t{:10.7}", inconsistency); + } +} diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..d99fb83 --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,10 @@ +format_code_in_doc_comments = true +format_macro_matchers = true +group_imports = "StdExternalCrate" +hex_literal_case = "Lower" +imports_granularity = "Module" +newline_style = "Unix" +normalize_comments = true +normalize_doc_attributes = true +use_field_init_shorthand = true +use_try_shorthand = true \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 94c117e..5f13ab5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -36,10 +36,13 @@ use nalgebra::{Isometry3, Matrix3, UnitQuaternion, Vector2, Vector3}; +use crate::naive_cf::NaiveCF; + #[cfg(feature = "grawoow")] pub mod grawoow; #[cfg(feature = "mad_gaze")] pub mod mad_gaze; +mod naive_cf; #[cfg(feature = "nreal")] pub mod nreal_air; #[cfg(feature = "nreal")] @@ -77,6 +80,58 @@ pub enum Error { type Result = std::result::Result; +/* +high level interface of glasses & state estimation, with the following built-in fusion pipeline: + +(the first version should only use complementary filter for simplicity and sanity test) + +- roll/pitch <= acc + gyro (complementary filter) + - assuming that acc vector always pointed up, spacecraft moving in that direction can create 1G artificial gravity + - TODO: this obviously assumes no steadily accelerating frame, at which point up d_acc has to be used for correction + - TODO: use ESKF (error-state/multiplicatory KF, https://arxiv.org/abs/1711.02508) +- gyro-yaw <= gyro (integrate over time) +- mag-yaw <= mag + roll/pitch (arctan) + - TODO: mag calibration? + (continuous ellipsoid fitting, assuming homogeneous E-M environment & hardpoint-mounted E-M interference) +- yaw <= mag-yaw + gyro-gyro (complementary filter) + - TODO: use EKF + +CAUTION: unlike [[GlassesEvent]], all states & outputs should use FRD reference frame + (forward, right, down, corresponding to roll, pitch, yaw in Euler angles-represented rotation) + +FRD is the standard frame for aerospace, and is also the default frame for NALgebra +*/ +pub trait Fusion: Send { + fn glasses(&mut self) -> &mut Box; + // TODO: only declared mutable as many API of ARGlasses are also mutable + + /// primary estimation output + /// can be used to convert to Euler angles of different conventions + fn attitude_quaternion(&self) -> UnitQuaternion; + + /// use FRD frame as error in Quaternion is multiplicative & is over-defined + fn inconsistency(&self) -> f32; + + fn update(&mut self) -> (); +} + +impl dyn Fusion { + pub fn attitude_frd_rad(&self) -> Vector3 { + let (roll, pitch, yaw) = (self.attitude_quaternion()).euler_angles(); + Vector3::new(roll, pitch, yaw) + } + + pub fn attitude_frd_deg(&self) -> Vector3 { + self.attitude_frd_rad().map(|x| x.to_degrees()) + } +} + +pub fn any_fusion() -> Result> { + // let glasses = any_glasses()?; + let glasses = any_glasses()?; + Ok(Box::new(NaiveCF::new(glasses)?)) +} + impl std::error::Error for Error { fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { match self { @@ -243,30 +298,43 @@ pub struct DisplayMatrices { pub isometry: Isometry3, } +fn upcast(result: Result) -> Result> { + result.map(|glasses| Box::new(glasses) as Box) +} + /// Convenience function to detect and connect to any of the supported glasses #[cfg(not(target_os = "android"))] pub fn any_glasses() -> Result> { - #[cfg(feature = "rokid")] - if let Ok(glasses) = rokid::RokidAir::new() { - return Ok(Box::new(glasses)); - }; - #[cfg(feature = "nreal")] - if let Ok(glasses) = nreal_air::NrealAir::new() { - return Ok(Box::new(glasses)); - }; - #[cfg(feature = "nreal")] - if let Ok(glasses) = nreal_light::NrealLight::new() { - return Ok(Box::new(glasses)); - }; - #[cfg(feature = "grawoow")] - if let Ok(glasses) = grawoow::GrawoowG530::new() { - return Ok(Box::new(glasses)); - }; - #[cfg(feature = "mad_gaze")] - if let Ok(glasses) = mad_gaze::MadGazeGlow::new() { - return Ok(Box::new(glasses)); - }; - Err(Error::NotFound) + let glasses_factories: Vec<(&str, fn() -> Result>)> = vec![ + #[cfg(feature = "rokid")] + ("RokidAir", || upcast(rokid::RokidAir::new())), + #[cfg(feature = "nreal")] + ("NrealAir", || upcast(nreal_air::NrealAir::new())), + #[cfg(feature = "nreal")] + ("NrealLight", || upcast(nreal_light::NrealLight::new())), + #[cfg(feature = "grawoow")] + ("GrawoowG530", || upcast(grawoow::GrawoowG530::new())), + #[cfg(feature = "mad_gaze")] + ("MadGazeGlow", || upcast(mad_gaze::MadGazeGlow::new())), + ]; + + glasses_factories + .into_iter() + .find_map(|(glasses_type, factory)| { + let factory: fn() -> Result> = factory; + + factory() + .map_err(|e| { + // + println!("can't find {}: {}", glasses_type, e) + }) + .ok() + .map(|v| { + println!("found {}", glasses_type); + v + }) + }) + .ok_or(Error::NotFound) } impl From for Error { diff --git a/src/naive_cf.rs b/src/naive_cf.rs new file mode 100644 index 0000000..7c3e639 --- /dev/null +++ b/src/naive_cf.rs @@ -0,0 +1,229 @@ +//! +//! NaiveCF is a simple sensor fusion algorithm that uses a complementary filter. +//! +//! complementary filter with very simple update algorithm: +//! +//! ( +//! assuming: +//! (S, S-) = current & previous estimated state +//! d~S1 = rate sensor reading (e.g. gyro), high frequency, high drift, dead reckoning +//! ~S2 = state sensor reading (e.g. grav/acc, mag), low frequency, high noise, low drift +//! d_t1 = time elapse since last rate sensor sampling +//!) +//! +//!S = S- + ratio * d~S1 * d_t1 + (1-ratio) * (~S2 - S-) +//! = ratio * (S- + d~S1 * d_t1) + (1-ratio) * ~S2 +//! +//!this implies: +//! +//!- the algorithm natively support state sensor(s) with different sampling frequency, incomplete reading, +//!or unreliable reading, as ~S2 variable is merely an optional correction +//!- the interpolation between ~S2 and the first term doesn't need to be linear or additive, +//!e.g. 3D angular interpolation is multiplicative +//!- the ratio can be adjusted based on quality & frequency of state sensor(s) +//! +//!most glasses have acc & grav/acc readings in 1 bundle, but I prefer not using this assumption and still update them independently +//! + +use nalgebra::{UnitQuaternion, Vector3}; + +use crate::{ARGlasses, Error, Fusion, GlassesEvent}; + +#[test] +pub fn __get_correction() { + let acc = Vector3::new(3.0176868, -0.74084723, 9.24847); + let axis = Vector3::new(-0.0015257122, -0.9227901, -0.38530007); + let angle = 2.262818; + + let rotation = UnitQuaternion::from_axis_angle(&Unit::new_normalize(axis), angle); + + let correction = NaiveCF::get_rotation_verified(&acc, &rotation).unwrap(); + + println!( + "compute: {}, {} => {}", + acc.transpose(), + rotation, + correction + ); +} + +type Result = std::result::Result; + +pub struct NaiveCF { + pub glasses: Box, + + //estimation + pub attitude: UnitQuaternion, + + //just old readings + //prevAcc: (Vector3, u64), + pub prev_gyro: (Vector3, u64), //FRD + //prevMag: (Vector3, u64), + pub inconsistency: f32, //roll, pitch. yaw +} + +impl NaiveCF { + pub fn new(glasses: Box) -> Result { + //let attitude = ; + //let prev_gyro = ; + let mut fusion = NaiveCF { + glasses, + attitude: UnitQuaternion::identity(), + // attitude: UnitQuaternion::from_euler_angles(0.0, 0.0, std::f32::consts::PI), // seeing backwards + prev_gyro: (Vector3::zeros(), 0), + inconsistency: 0.0, + }; + + loop { + //wait for the first non-zero gyro reading + let next_event = fusion.next_event(); + match next_event { + GlassesEvent::AccGyro { + accelerometer: _, + gyroscope, + timestamp, + } => { + //if gyroscope != Vector3::zeros() { + fusion.prev_gyro = (gyroscope, timestamp); + return Ok(fusion); + //} + } + _ => {} + } + } + } + + ///read until next valid event. Blocks. + fn next_event(&mut self) -> GlassesEvent { + loop { + match self.glasses.read_event() { + Ok(event) => return event, + Err(e) => { + println!("Error reading event: {}", e); + } + } + } + } + + fn rub_to_frd(v: &Vector3) -> Vector3 { + let result = Vector3::new(-v.z, v.x, -v.y); + result + } + + const BASE_GRAV_RATIO: f32 = 0.005; + + const GYRO_SPEED_IN_TIMESTAMP_FACTOR: f32 = 1000.0 * 1000.0; //microseconds + + const INCONSISTENCY_DECAY: f32 = 0.90; + + const UP_FRD: Vector3 = Vector3::new(0.0, 0.0, -9.81); + //const NORTH_FRD: Vector3 = Vector3::new(0.0, 0.0, -1.0); + + //CAUTION: right-multiplication means rotation, unconventionally + + fn update_gyro_rub(&mut self, gyro_rub: &Vector3, t: u64) -> () { + let gyro = Self::rub_to_frd(gyro_rub); + + let d_t1 = t - self.prev_gyro.1; + let d_t1_f = d_t1 as f32 / Self::GYRO_SPEED_IN_TIMESTAMP_FACTOR; + let d_s1_t1 = d_t1_f * gyro; + + let increment = UnitQuaternion::from_euler_angles(d_s1_t1.x, d_s1_t1.y, d_s1_t1.z); + + // self.attitude = (increment.inverse() * self.attitude.inverse()).inverse(); + self.attitude = self.attitude * increment; + + self.prev_gyro = (gyro, t); + } + + fn update_acc(&mut self, acc_rub: &Vector3, _t: u64) -> () { + let acc = Self::rub_to_frd(acc_rub); + + if acc.norm() < 1.0 { + return; //almost in free fall, or acc disabled, do not correct + } + + let attitude = &self.attitude; + // let acc_inv = Vector3::new(-acc.x, -acc.y, acc.z); + + let correction_opt = Self::get_correction(&acc, &attitude.inverse(), Self::BASE_GRAV_RATIO); + + match correction_opt { + Some(correction_inv) => { + let correction = correction_inv.inverse(); + self.inconsistency = + self.inconsistency * Self::INCONSISTENCY_DECAY + correction.angle(); + + // self.attitude = (correction_inv * attitude.inverse()).inverse(); + self.attitude = attitude * correction; + } + None => { + // opposite direction, no correction + } + } + } + + pub fn get_correction( + acc: &Vector3, + rotation: &UnitQuaternion, + scale: f32, + ) -> Option> { + let uncorrected = rotation * Self::UP_FRD.normalize(); + + let scaled_opt = + UnitQuaternion::scaled_rotation_between(&uncorrected, &acc.normalize(), scale); + + scaled_opt + } + + pub fn get_rotation( + acc: &Vector3, + rotation: &UnitQuaternion, + ) -> Option> { + Self::get_rotation_raw(acc, rotation) + } + + fn get_rotation_raw( + acc: &Vector3, + rotation: &UnitQuaternion, + ) -> Option> { + let uncorrected = rotation * Self::UP_FRD; + let correction_opt = UnitQuaternion::scaled_rotation_between(&uncorrected, &acc, 1.0); + correction_opt + } +} + +//unsafe impl Sync for NaiveCF {} + +impl Fusion for NaiveCF { + fn glasses(&mut self) -> &mut Box { + &mut self.glasses + } + + fn attitude_quaternion(&self) -> UnitQuaternion { + self.attitude + } + + fn inconsistency(&self) -> f32 { + self.inconsistency + } + + fn update(&mut self) -> () { + let event = self.next_event(); + match event { + GlassesEvent::AccGyro { + accelerometer, + gyroscope, + timestamp, + } => { + self.update_gyro_rub(&gyroscope, timestamp); + self.update_acc(&accelerometer, timestamp); + self.attitude.renormalize(); + // self.attitude.renormalize_fast(); // TODO: switch to it after rigorous testing + } + _ => { + //TODO: add magnetometer event etc + } + } + } +}