diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..5e3db18 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,5 @@ +[submodule "tests/data"] + path = tests/data + url = https://github.com/sunsided/serial-sensors-test-data.git + shallow = true + branch = main diff --git a/Cargo.toml b/Cargo.toml index 7436eaf..0159f0c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,17 +11,29 @@ categories = ["aerospace", "algorithms", "science::robotics", "no-std", "embedde edition = "2021" [features] +default = ["coordinate-frame"] std = ["minikalman/std", "num-traits/std"] unsafe = [] +coordinate-frame = ["dep:coordinate-frame"] [dependencies] +coordinate-frame = { version = "0.4.0", optional = true, features = ["num-traits", "nalgebra"] } minikalman = { version = "0.6.0", default-features = false } num-traits = { version = "0.2.19", default-features = false } +uniform-array-derive = "0.1.0" [dev-dependencies] ensure-uniform-type = "0.1.1" paste = "1.0.15" minikalman = { version = "0.6.0", features = ["std"] } +csv = "1.3.0" +serde = { version = "1.0.204", features = ["derive"] } +kiss3d = "0.35.0" + +[[example]] +name = "simulation" +path = "examples/simulation.rs" +required-features = ["std", "coordinate-frame"] [package.metadata.docs.rs] all-features = true diff --git a/examples/data/fonts/firamono/FiraMono-Regular.ttf b/examples/data/fonts/firamono/FiraMono-Regular.ttf new file mode 100644 index 0000000..67bbd42 Binary files /dev/null and b/examples/data/fonts/firamono/FiraMono-Regular.ttf differ diff --git a/examples/data/fonts/firamono/OFL.txt b/examples/data/fonts/firamono/OFL.txt new file mode 100644 index 0000000..029b036 --- /dev/null +++ b/examples/data/fonts/firamono/OFL.txt @@ -0,0 +1,93 @@ +Copyright (c) 2012-2013, The Mozilla Corporation and Telefonica S.A. + +This Font Software is licensed under the SIL Open Font License, Version 1.1. +This license is copied below, and is also available with a FAQ at: +https://openfontlicense.org + + +----------------------------------------------------------- +SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007 +----------------------------------------------------------- + +PREAMBLE +The goals of the Open Font License (OFL) are to stimulate worldwide +development of collaborative font projects, to support the font creation +efforts of academic and linguistic communities, and to provide a free and +open framework in which fonts may be shared and improved in partnership +with others. + +The OFL allows the licensed fonts to be used, studied, modified and +redistributed freely as long as they are not sold by themselves. The +fonts, including any derivative works, can be bundled, embedded, +redistributed and/or sold with any software provided that any reserved +names are not used by derivative works. The fonts and derivatives, +however, cannot be released under any other type of license. The +requirement for fonts to remain under this license does not apply +to any document created using the fonts or their derivatives. + +DEFINITIONS +"Font Software" refers to the set of files released by the Copyright +Holder(s) under this license and clearly marked as such. This may +include source files, build scripts and documentation. + +"Reserved Font Name" refers to any names specified as such after the +copyright statement(s). + +"Original Version" refers to the collection of Font Software components as +distributed by the Copyright Holder(s). + +"Modified Version" refers to any derivative made by adding to, deleting, +or substituting -- in part or in whole -- any of the components of the +Original Version, by changing formats or by porting the Font Software to a +new environment. + +"Author" refers to any designer, engineer, programmer, technical +writer or other person who contributed to the Font Software. + +PERMISSION & CONDITIONS +Permission is hereby granted, free of charge, to any person obtaining +a copy of the Font Software, to use, study, copy, merge, embed, modify, +redistribute, and sell modified and unmodified copies of the Font +Software, subject to the following conditions: + +1) Neither the Font Software nor any of its individual components, +in Original or Modified Versions, may be sold by itself. + +2) Original or Modified Versions of the Font Software may be bundled, +redistributed and/or sold with any software, provided that each copy +contains the above copyright notice and this license. These can be +included either as stand-alone text files, human-readable headers or +in the appropriate machine-readable metadata fields within text or +binary files as long as those fields can be easily viewed by the user. + +3) No Modified Version of the Font Software may use the Reserved Font +Name(s) unless explicit written permission is granted by the corresponding +Copyright Holder. This restriction only applies to the primary font name as +presented to the users. + +4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font +Software shall not be used to promote, endorse or advertise any +Modified Version, except to acknowledge the contribution(s) of the +Copyright Holder(s) and the Author(s) or with their explicit written +permission. + +5) The Font Software, modified or unmodified, in part or in whole, +must be distributed entirely under this license, and must not be +distributed under any other license. The requirement for fonts to +remain under this license does not apply to any document created +using the Font Software. + +TERMINATION +This license becomes null and void if any of the above conditions are +not met. + +DISCLAIMER +THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT +OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE +COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM +OTHER DEALINGS IN THE FONT SOFTWARE. diff --git a/examples/simulation.rs b/examples/simulation.rs new file mode 100644 index 0000000..7ef456b --- /dev/null +++ b/examples/simulation.rs @@ -0,0 +1,856 @@ +use std::cell::RefCell; +use std::error::Error; +use std::ops::Deref; +use std::path::Path; +use std::rc::Rc; +use std::time::{Duration, Instant}; + +use coordinate_frame::{EastNorthUp, NorthEastDown, NorthWestDown, SouthEastUp, WestUpNorth}; +use csv::ReaderBuilder; +use kiss3d::event::{Action, Key, WindowEvent}; +use kiss3d::light::Light; +use kiss3d::nalgebra::{Matrix3, Point2, Point3, Scalar, Translation3, UnitQuaternion, Vector3}; +use kiss3d::resource::Mesh; +use kiss3d::scene::SceneNode; +use kiss3d::text::Font; +use kiss3d::window::Window; +use serde::de::DeserializeOwned; +use serde::Deserialize; + +use marg_orientation::gyro_free::{MagneticReference, OwnedOrientationEstimator}; +use marg_orientation::{ + AccelerometerNoise, AccelerometerReading, GyroscopeReading, MagnetometerNoise, + MagnetometerReading, +}; + +// const DATASET: &str = "2024-07-10/stm32f3discovery/stationary"; +const DATASET: &str = "2024-07-10/stm32f3discovery/x-forward-rotate-around-up-ccw"; +// const DATASET: &str = "2024-07-10/stm32f3discovery/x-forward-tilt-top-east"; +// const DATASET: &str = "2024-07-10/stm32f3discovery/x-forward-tilt-nose-up"; +// const DATASET: &str = "2024-07-06/stm32f3discovery"; + +/// Kiss3d uses a West, Up, North system by default. +type Kiss3DCoordinates = WestUpNorth; + +/// LSM303DLHC accelerometer readings. +#[derive(Debug, Deserialize)] +struct LSM303DLHCAccelerometer { + /// The sample time, in seconds, relative to the Unix epoch. + #[serde(rename = "host_time")] + time: f64, + /// Accelerometer reading on the x-axis. + #[serde(rename = "x")] + acc_x: f32, + /// Accelerometer reading on the y-axis. + #[serde(rename = "y")] + acc_y: f32, + /// Accelerometer reading on the z-axis. + #[serde(rename = "z")] + acc_z: f32, +} + +/// L3GD20 gyroscope readings. +#[derive(Debug, Deserialize)] +struct L3GD20Gyro { + /// The sample time, in seconds, relative to the Unix epoch. + #[serde(rename = "host_time")] + time: f64, + /// Gyroscope reading on the x-axis. + #[serde(rename = "x")] + gyro_x: f32, + /// Gyroscope reading on the y-axis. + #[serde(rename = "y")] + gyro_y: f32, + /// Gyroscope reading on the z-axis. + #[serde(rename = "z")] + gyro_z: f32, +} + +/// LSM303DLHC magnetometer readings. +#[derive(Debug, Deserialize)] +struct LSM303DLHCMagnetometer { + /// The sample time, in seconds, relative to the Unix epoch. + #[serde(rename = "host_time")] + time: f64, + /// Magnetometer reading on the x-axis. + #[serde(rename = "x")] + compass_x: f32, + /// Magnetometer reading on the y-axis. + #[serde(rename = "y")] + compass_y: f32, + /// Magnetometer reading on the z-axis. + #[serde(rename = "z")] + compass_z: f32, +} + +pub trait Time { + /// Gets the sample time. + fn time(&self) -> f64; +} + +impl Time for L3GD20Gyro { + fn time(&self) -> f64 { + self.time + } +} + +impl Time for LSM303DLHCAccelerometer { + fn time(&self) -> f64 { + self.time + } +} + +impl Time for LSM303DLHCMagnetometer { + fn time(&self) -> f64 { + self.time + } +} + +impl From<&LSM303DLHCAccelerometer> for AccelerometerReading { + fn from(value: &LSM303DLHCAccelerometer) -> Self { + // The HMC303DLHC's accelerometer on the STM32F3 Discovery board measures North, West, Down. + let frame = NorthWestDown::new(value.acc_x, value.acc_y, value.acc_z); + // Normalize by the sensor value range. + let frame = frame / 16384.0; + AccelerometerReading::north_east_down(frame) + } +} + +impl From for AccelerometerReading { + fn from(value: LSM303DLHCAccelerometer) -> Self { + Self::from(&value) + } +} + +impl From<&LSM303DLHCMagnetometer> for MagnetometerReading { + fn from(value: &LSM303DLHCMagnetometer) -> Self { + // The HMC303DLHC's magnetometer on the STM32F3 Discovery board measures South, East, Up. + let frame = SouthEastUp::new(value.compass_x, value.compass_y, value.compass_z); + // Normalize by the sensor value range. + let frame = frame / 1100.0; + MagnetometerReading::north_east_down(frame) + } +} + +impl From for MagnetometerReading { + fn from(value: LSM303DLHCMagnetometer) -> Self { + Self::from(&value) + } +} + +impl From<&L3GD20Gyro> for GyroscopeReading { + fn from(value: &L3GD20Gyro) -> Self { + // The L3GD20 gyroscope on the STM32F3 Discovery board measures East, North, Up. + let frame = EastNorthUp::new(value.gyro_x, value.gyro_y, value.gyro_z); + // Normalize by the sensor value range. + let frame = frame / 5.714285; + GyroscopeReading::north_east_down(frame) + } +} + +impl From for GyroscopeReading { + fn from(value: L3GD20Gyro) -> Self { + Self::from(&value) + } +} + +/// A timed reading. +pub struct Timed { + pub time: f64, + pub reading: R, +} + +impl Timed { + pub fn with_time_offset(mut self, value: f64) -> Self { + self.time -= value; + self + } +} + +impl Time for Timed { + fn time(&self) -> f64 { + self.time + } +} + +impl From for Timed> { + fn from(value: LSM303DLHCAccelerometer) -> Self { + Self { + time: value.time, + reading: value.into(), + } + } +} + +impl From for Timed> { + fn from(value: LSM303DLHCMagnetometer) -> Self { + Self { + time: value.time, + reading: value.into(), + } + } +} + +impl From for Timed> { + fn from(value: L3GD20Gyro) -> Self { + Self { + time: value.time, + reading: value.into(), + } + } +} + +impl Deref for Timed { + type Target = R; + + fn deref(&self) -> &Self::Target { + &self.reading + } +} + +fn main() -> Result<(), Box> { + let gyro = + read_csv::(format!("tests/data/{DATASET}/106-gyro-i16-x1.csv").as_str())?; + let compass = read_csv::( + format!("tests/data/{DATASET}/30-mag-i16-x3.csv").as_str(), + )?; + let accel = read_csv::( + format!("tests/data/{DATASET}/25-acc-i16-x3.csv").as_str(), + )?; + + // Obtain the offset times. + let gyro_t = gyro[0].time; + let accel_t = accel[0].time; + let compass_t = compass[0].time; + let time_offset = gyro_t.min(accel_t).min(compass_t); + + // Convert the readings into normalized frames. + let gyro: Vec>> = gyro + .into_iter() + .map(Timed::from) + .map(|t| t.with_time_offset(time_offset)) + .collect(); + let compass: Vec>> = compass + .into_iter() + .map(Timed::from) + .map(|t| t.with_time_offset(time_offset)) + .collect(); + let accel: Vec>> = accel + .into_iter() + .map(Timed::from) + .map(|t| t.with_time_offset(time_offset)) + .collect(); + + // Determine sample rates. + let (gyro_sample_rate, _) = determine_sampling(&gyro); + let (accel_sample_rate, _) = determine_sampling(&accel); + let (compass_sample_rate, _) = determine_sampling(&compass); + + println!("Average sample rates:"); + println!("- Accelerometer readings: {accel_sample_rate} Hz (expected 400 Hz)"); + println!("- Magnetometer readings: {compass_sample_rate} Hz (expected 75 Hz)"); + println!("- Gyroscope readings: {gyro_sample_rate} Hz (expected 400 Hz)"); + + // Magnetic field reference for Berlin, Germany expressed in North, East, Down. + let reference = MagneticReference::new(18.0, 1.5, 47.0); + + // Create the estimator. + let mut estimator = OwnedOrientationEstimator::::new( + AccelerometerNoise::new(0.07, 0.07, 0.07), + MagnetometerNoise::new(0.18, 0.11, 0.34), + reference, + 0.001, + ); + + let mut gyro_x_estimator = + marg_orientation::gyro_drift::GyroscopeAxisEstimator::::new(0.00003, 5.0, 0.001); + + // Prepare some basics for the simulation. + let font = Font::new(Path::new( + "examples/data/fonts/firamono/FiraMono-Regular.ttf", + )) + .expect("failed to load font"); + + let mut window = Window::new("MPU6050 and HMC8533L simulation"); + window.set_framerate_limit(Some(30)); + window.set_light(Light::StickToCamera); + + // Create a box at the coordinate origin. + let mut c = window.add_cube(0.02, 0.02, 0.02); + c.set_color(1.0, 1.0, 1.0); + + // Create the arrow indicating orientation. + let mut arrows = create_arrow(&mut window); + + // Some colors. + let red = Point3::new(1.0, 0.0, 0.0); + let green = Point3::new(0.0, 1.0, 0.0); + let blue = Point3::new(0.0, 0.0, 1.0); + let dark_red = red * 0.5; + let dark_green = green * 0.5; + let dark_blue = blue * 0.5; + + // Walk through the simulation data. + let mut accel_index = 0; + let mut compass_index = 0; + let mut gyro_index = 0; + + let mut last_time = Instant::now(); + let mut simulation_time = Duration::default(); + let mut is_paused = false; + let mut reset_times = false; + let mut display_reference = true; + let mut display_body_frame = false; + let mut display_sensors = true; + + 'render: while window.render() { + // Obtain the current render timestamp. + let now = Instant::now(); + let elapsed_time = now - last_time; + last_time = now; + + if reset_times { + reset_times = false; + simulation_time = Duration::default(); + accel_index = 0; + gyro_index = 0; + compass_index = 0; + } + + // Handle window events to check for key presses + for event in window.events().iter() { + if let WindowEvent::Key(key, Action::Press, _) = event.value { + if key == Key::Space { + is_paused = !is_paused; + } else if key == Key::R { + reset_times = true; + continue; + } else if key == Key::C { + display_reference = !display_reference; + continue; + } else if key == Key::B { + display_body_frame = !display_body_frame; + continue; + } else if key == Key::S { + display_sensors = !display_sensors; + continue; + } + } + } + + if !is_paused { + simulation_time += elapsed_time; + window.set_background_color(0.118, 0.122, 0.149); + } else { + window.set_background_color(0.149, 0.122, 0.118); + } + + // Enable updates when we receive new data. + let mut acc_should_update = false; + let mut mag_should_update = false; + let mut gyro_should_update = false; + + // Increment simulation index. + if !is_paused { + while accel[accel_index].time < simulation_time.as_secs_f64() { + accel_index += 1; + acc_should_update = true; + if accel_index >= accel.len() { + reset_times = true; + continue 'render; + } + } + while gyro[gyro_index].time < simulation_time.as_secs_f64() { + gyro_index += 1; + gyro_should_update = true; + if accel_index >= gyro.len() { + reset_times = true; + continue 'render; + } + } + while compass[compass_index].time < simulation_time.as_secs_f64() { + compass_index += 1; + mag_should_update = true; + if compass_index >= compass.len() { + reset_times = true; + continue 'render; + } + } + } + + let accel_meas = &accel[accel_index]; + let gyro_meas = &gyro[gyro_index]; + let compass_meas = &compass[compass_index]; + + // Calculate the angle between the magnetic field vector and the down vector. + let angle_mag_accel = calculate_angle_acc_mag(accel_meas, compass_meas); + + // Calculated heading. + let heading_degrees = { + let vec = Vector3::new(compass_meas.x, compass_meas.y, compass_meas.z).normalize(); + let heading = vec[1].atan2(vec[0]).to_degrees(); + if heading >= 360.0 { + heading - 360.0 + } else if heading <= 0.0 { + heading + 360.0 + } else { + heading + } + }; + + // Run a prediction. + if !is_paused { + estimator.predict(); + gyro_x_estimator.predict(elapsed_time.as_secs_f32()); + } + + let estimated_angles = estimator.estimated_angles(); + if estimated_angles.yaw_psi.is_nan() + || estimated_angles.pitch_theta.is_nan() + || estimated_angles.roll_phi.is_nan() + { + todo!("nan before correction"); + } + + // Update the filter when needed. + if acc_should_update { + estimator.correct_accelerometer(&accel_meas.reading); + } + if mag_should_update { + estimator.correct_magnetometer(&compass_meas.reading); + } + if gyro_should_update { + gyro_x_estimator.correct(gyro_meas.reading.omega_x); + } + + let estimated_angles = estimator.estimated_angles(); + if estimated_angles.yaw_psi.is_nan() + || estimated_angles.pitch_theta.is_nan() + || estimated_angles.roll_phi.is_nan() + { + todo!("nan after correction"); + } + + // Obtain a rotation matrix from the estimated angles. + let north = estimator.rotate_vector_world(marg_orientation::Vector3::new(1.0, 0.0, 0.0)); + let east = estimator.rotate_vector_world(marg_orientation::Vector3::new(0.0, 1.0, 0.0)); + let down = estimator.rotate_vector_world(marg_orientation::Vector3::new(0.0, 0.0, 1.0)); + let filter_x = kiss3d_point(NorthEastDown::new(north.x, north.y, north.z)); + let filter_y = kiss3d_point(NorthEastDown::new(east.x, east.y, east.z)); + let filter_z = kiss3d_point(NorthEastDown::new(down.x, down.y, down.z)); + + // Update the arrow according to the estimations. + update_arrow_orientation(&mut arrows, filter_x, filter_y, filter_z); + + // Display default coordinate system. + if display_reference { + let x_axis = NorthEastDown::new(1.0, 0.0, 0.0); + let y_axis = NorthEastDown::new(0.0, 1.0, 0.0); + let z_axis = NorthEastDown::new(0.0, 0.0, 1.0); + + window.draw_line(&Point3::default(), &kiss3d_point(x_axis), &dark_red); + window.draw_line(&Point3::default(), &kiss3d_point(y_axis), &dark_green); + window.draw_line(&Point3::default(), &kiss3d_point(z_axis), &dark_blue); + } + + // Convert estimations. + if display_body_frame { + // Display estimated orientation. + window.draw_line(&Point3::default(), &filter_x, &red); + window.draw_line(&Point3::default(), &filter_y, &green); + window.draw_line(&Point3::default(), &filter_z, &blue); + } + + // Display the accelerometer reading. + if display_sensors { + let am = NorthEastDown::new(accel_meas.x, accel_meas.y, accel_meas.z); + let p1 = Point3::new(0.0, 0.0, 0.0); + window.draw_line(&p1, &kiss3d_point(am), &Point3::new(0.5, 0.0, 1.0)); + } + + // Display the compass reading. + if display_sensors { + let mm = NorthEastDown::new(compass_meas.x, compass_meas.y, compass_meas.z); + let p1 = Point3::new(0.0, 0.0, 0.0); + window.draw_line(&p1, &kiss3d_point(mm), &Point3::new(1.0, 0.0, 0.5)); + } + + display_times_and_indexes( + &gyro, + &compass, + &accel, + &font, + &mut window, + accel_index, + compass_index, + gyro_index, + simulation_time, + elapsed_time, + ); + + // Display angle between measured accelerometer and magnetometer. + let info = format!("cos⁻¹(acc·mag) = {:+0.02}°", angle_mag_accel.to_degrees()); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 18.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display angle between measured accelerometer and magnetometer. + let info = format!("heading = {:+0.02}°", heading_degrees); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 17.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display magnetometer. + let info = format!("Bx = {:+0.02} Gs", compass_meas.x); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 15.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display magnetometer. + let info = format!("By = {:+0.02} Gs", compass_meas.y); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 14.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display magnetometer. + let info = format!("Bz = {:+0.02} Gs", compass_meas.z); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 13.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display acceleration. + let info = format!("ax = {:+0.02} G", accel_meas.x); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 11.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display acceleration. + let info = format!("ay = {:+0.02} G", accel_meas.y); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 10.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display acceleration. + let info = format!("az = {:+0.02} G", accel_meas.z); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 9.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display gyro roll rates. + let estimated_omega_x = gyro_x_estimator.angular_velocity(); + let estimated_x_bias = gyro_x_estimator.bias(); + let info = format!( + "ωx = {:+0.02} rad/s ({:+0.02}°/s) - {:+0.02} rad/s ± {:+0.02}rad/s", + gyro_meas.omega_x, + gyro_meas.omega_x * 180.0 / std::f32::consts::PI, + estimated_omega_x, + estimated_x_bias + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 7.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display gyro roll rates. + let info = format!( + "ωy = {:+0.02} rad/s ({:+0.02}°/s)", + gyro_meas.omega_y, + gyro_meas.omega_y * 180.0 / std::f32::consts::PI + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 6.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display gyro roll rates. + let info = format!( + "ωz = {:+0.02} rad/s ({:+0.02}°/s)", + gyro_meas.omega_z, + gyro_meas.omega_z * 180.0 / std::f32::consts::PI + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 5.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display estimated angles. + let info = format!( + "φ = {:+0.02} ± {:+0.02} rad ({:+0.02}°)", + estimated_angles.roll_phi, + estimator.roll_variance().sqrt(), + estimated_angles.roll_phi * 180.0 / std::f32::consts::PI, + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 3.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display estimated angles. + let info = format!( + "θ = {:+0.02} ± {:+0.02} rad ({:+0.02}°)", + estimated_angles.pitch_theta, + estimator.pitch_variance().sqrt(), + estimated_angles.pitch_theta * 180.0 / std::f32::consts::PI, + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 2.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display estimated angles. + let info = format!( + "ψ = {:+0.02} ± {:+0.02} rad ({:+0.02}°)", + estimated_angles.yaw_psi, + estimator.yaw_variance().sqrt(), + estimated_angles.yaw_psi * 180.0 / std::f32::consts::PI, + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + } + + Ok(()) +} + +fn display_times_and_indexes( + gyro: &[Timed>], + compass: &[Timed>], + accel: &[Timed>], + font: &Rc, + window: &mut Window, + accel_index: usize, + compass_index: usize, + gyro_index: usize, + simulation_time: Duration, + elapsed_time: Duration, +) { + // Display elapsed time since last frame. + let info = format!( + "ΔT = {:.4} s ({:.2}) Hz", + elapsed_time.as_secs_f32(), + elapsed_time.as_secs_f32().recip() + ); + window.draw_text( + &info, + &Point2::new(0.0, 0.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation time. + let info = format!(" t = {:.2} s", simulation_time.as_secs_f32()); + window.draw_text( + &info, + &Point2::new(0.0, 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation indexes. + let info = format!("ta = {:.2} s (#{})", accel[accel_index].time, accel_index); + window.draw_text( + &info, + &Point2::new(0.0, 32.0 + 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation indexes. + let info = format!( + "tm = {:.2} s (#{})", + compass[compass_index].time, compass_index + ); + window.draw_text( + &info, + &Point2::new(0.0, 32.0 + 2.0 * 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation indexes. + let info = format!("tg = {:.2} s (#{})", gyro[gyro_index].time, gyro_index); + window.draw_text( + &info, + &Point2::new(0.0, 32.0 + 3.0 * 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); +} + +fn create_arrow(window: &mut Window) -> SceneNode { + // Define the vertices for the arrow, based on a rectangle (shaft) and triangles (head). + let arrow_vertices = vec![ + // Rectangle vertices (shaft) on x-z plane, half as long + Point3::new(-0.20, 0.0, -0.05), // Bottom-left + Point3::new(0.10, 0.0, -0.05), // Bottom-right + Point3::new(0.10, 0.0, 0.05), // Top-right + Point3::new(-0.20, 0.0, 0.05), // Top-left + // Triangle vertices (head) on x-z plane, half as long + Point3::new(0.10, 0.0, -0.1), // Bottom + Point3::new(0.30, 0.0, 0.0), // Tip + Point3::new(0.10, 0.0, 0.1), // Top + ]; + + // Define the indices for the arrow. + let arrow_indices = vec![ + // Rectangle (shaft) + Point3::new(0u16, 1, 2), + Point3::new(0, 2, 3), + // Triangle (head) + Point3::new(4, 5, 6), + ]; + + // Create the mesh from vertices and indices + let arrow_mesh = Mesh::new(arrow_vertices, arrow_indices, None, None, false); + let arrow_mesh = Rc::new(RefCell::new(arrow_mesh)); + + // Quaternion to flip the arrow around once. + let flip_quaternion = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::PI); + + // Add the mesh to the window + let mut arrows = window.add_group(); + + // The top-side of the arrow (red). + let mut top_arrow = arrows.add_mesh(arrow_mesh.clone(), Vector3::new(1.0, 1.0, 1.0)); + top_arrow.set_color(0.8, 0.165, 0.212); + top_arrow.set_local_translation(Translation3::new(0.0, -0.005, 0.0)); + + // Flipped version to work around backface culling. + let mut top_arrow = arrows.add_mesh(arrow_mesh.clone(), Vector3::new(1.0, 1.0, 1.0)); + top_arrow.set_color(0.545, 0.114, 0.145); + top_arrow.set_local_translation(Translation3::new(0.0, -0.005, 0.0)); + top_arrow.set_local_rotation(flip_quaternion); + + // The bottom-side of the arrow (white). + let mut bottom_arrow = arrows.add_mesh(arrow_mesh.clone(), Vector3::new(1.0, 1.0, 1.0)); + bottom_arrow.set_color(1.0, 1.0, 1.0); + bottom_arrow.set_local_translation(Translation3::new(0.0, 0.005, 0.0)); + + // Flipped version to work around backface culling. + let mut bottom_arrow = arrows.add_mesh(arrow_mesh, Vector3::new(1.0, 1.0, 1.0)); + bottom_arrow.set_color(1.0, 1.0, 1.0); + bottom_arrow.set_local_translation(Translation3::new(0.0, 0.005, 0.0)); + bottom_arrow.set_local_rotation(flip_quaternion); + arrows +} + +fn calculate_angle_acc_mag( + accel_meas: &Timed>, + compass_meas: &Timed>, +) -> f32 { + let accel_vec: Vector3<_> = Vector3::new(accel_meas.x, accel_meas.y, accel_meas.z).normalize(); + let mag_vec: Vector3<_> = + Vector3::new(compass_meas.x, compass_meas.y, compass_meas.z).normalize(); + accel_vec.dot(&mag_vec).acos() +} + +fn determine_sampling(data: &[T]) -> (f64, f64) { + let (total_diff, count) = data.windows(2).fold((0.0, 0), |(sum, cnt), window| { + let diff = window[1].time() - window[0].time(); + (sum + diff, cnt + 1) + }); + let sample_time = total_diff / (count as f64); + let sample_rate = 1.0 / sample_time; + (sample_rate, sample_time) +} + +fn read_csv(file_path: &str) -> Result, Box> { + let mut rdr = ReaderBuilder::new().from_path(file_path)?; + let mut data = Vec::new(); + + for result in rdr.deserialize() { + let record: T = result?; + data.push(record); + } + + Ok(data) +} + +fn kiss3d_point(vector: C) -> Point3 +where + C: Into>, + T: Scalar, +{ + let vector = vector.into(); + Point3::new(vector.x(), vector.y(), vector.z()) +} + +// Function to update the arrow's orientation based on new basis vectors +fn update_arrow_orientation( + arrow: &mut SceneNode, + x_basis: Point3, + y_basis: Point3, + z_basis: Point3, +) { + let x_basis = Vector3::new(x_basis[0], x_basis[1], x_basis[2]); + let y_basis = Vector3::new(y_basis[0], y_basis[1], y_basis[2]); + let z_basis = Vector3::new(z_basis[0], z_basis[1], z_basis[2]); + + // Create a rotation matrix from the orthonormal basis vectors + let rotation_matrix = Matrix3::from_columns(&[x_basis, y_basis, z_basis]); + + // Convert the rotation matrix to a UnitQuaternion + let rotation = UnitQuaternion::from_matrix(&rotation_matrix); + let flip = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::FRAC_PI_2); + + let rotation = rotation * flip; + + // Apply the rotation to the arrow + arrow.set_local_rotation(rotation); +} diff --git a/math/quaternion_jacobian.m b/math/quaternion_jacobian.m new file mode 100644 index 0000000..159ab9d --- /dev/null +++ b/math/quaternion_jacobian.m @@ -0,0 +1,42 @@ +% Define symbolic variables +syms q0 q1 q2 q3 real % quaternion components +syms x y z real % components of the 3D vector + +% Define the quaternion +q = [q0 q1 q2 q3]; + +% Define the 3D vector +v = [x; y; z]; + +% Down vector. +% v(1) = 0; +% v(2) = 0; +% v(3) = 1; + +% Quaternion multiplication (q*v*q') +q_conj = [q0, -q1, -q2, -q3]; % Conjugate of the quaternion + +% Quaternion-vector multiplication q*v +v_quat = [0; v]; % Extend vector to quaternion form +qv = quat_mult(q, v_quat); % Quaternion multiplication q*v + +% Quaternion-vector multiplication (q*v)*q' +v_rot_quat = quat_mult(qv, q_conj); + +% Extract rotated vector +v_rot = v_rot_quat(2:4); + +% Compute the Jacobian of the rotated vector with respect to the quaternion +J = jacobian(v_rot, q); + +% Display the Jacobian +disp('Jacobian of the rotated vector with respect to the quaternion:'); +disp(J); + +function res = quat_mult(q1, q2) + % Quaternion multiplication: q1 * q2 + res = [q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4); + q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3); + q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2); + q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1)]; +end diff --git a/src/accelerometer_noise.rs b/src/accelerometer_noise.rs index 2ff5c25..5b3ba77 100644 --- a/src/accelerometer_noise.rs +++ b/src/accelerometer_noise.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct AccelerometerNoise { @@ -19,12 +21,6 @@ impl AccelerometerNoise { pub const fn new(x: T, y: T, z: T) -> Self { Self { x, y, z } } - - /// Returns the length of the [`AccelerometerNoise`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 - } } impl Default for AccelerometerNoise @@ -78,34 +74,6 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for AccelerometerNoise { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for AccelerometerNoise { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } - } -} - impl_standard_traits!(AccelerometerNoise, T); #[cfg(test)] diff --git a/src/accelerometer_reading.rs b/src/accelerometer_reading.rs index f18ead1..5da998a 100644 --- a/src/accelerometer_reading.rs +++ b/src/accelerometer_reading.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct AccelerometerReading { @@ -20,10 +22,20 @@ impl AccelerometerReading { Self { x, y, z } } - /// Returns the length of the [`AccelerometerReading`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 + /// Constructs a new [`AccelerometerReading`] instance from a reading in a given coordinate frame. + #[cfg(feature = "coordinate-frame")] + #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] + pub fn north_east_down(coordinate: C) -> Self + where + C: Into>, + T: Clone, + { + let coordinate = coordinate.into(); + Self { + x: coordinate.x(), + y: coordinate.y(), + z: coordinate.z(), + } } } @@ -78,31 +90,15 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for AccelerometerReading { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for AccelerometerReading { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } +#[cfg(feature = "coordinate-frame")] +#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] +impl From for AccelerometerReading +where + C: coordinate_frame::CoordinateFrame, + T: Copy + coordinate_frame::SaturatingNeg, +{ + fn from(value: C) -> Self { + Self::north_east_down(value.to_ned()) } } diff --git a/src/euler_angles.rs b/src/euler_angles.rs index 7f08902..b68cf6b 100644 --- a/src/euler_angles.rs +++ b/src/euler_angles.rs @@ -1,6 +1,8 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct EulerAngles { @@ -22,13 +24,6 @@ impl EulerAngles { yaw_psi, } } - - /// Returns the length of the [`EulerAngles`] vector. - #[inline(always)] - #[allow(unused)] - pub const fn len(&self) -> usize { - 3 - } } impl Default for EulerAngles @@ -67,36 +62,6 @@ where } } -#[cfg_attr(docsrs, doc(cfg(not(feature = "unsafe"))))] -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for EulerAngles { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.roll_phi, - 1 => &self.pitch_theta, - 2 => &self.yaw_psi, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg_attr(docsrs, doc(cfg(not(feature = "unsafe"))))] -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for EulerAngles { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.roll_phi, - 1 => &mut self.pitch_theta, - 2 => &mut self.yaw_psi, - _ => panic!("Index out of bounds"), - } - } -} - impl_standard_traits!(EulerAngles, T); #[cfg(test)] diff --git a/src/gyro_drift.rs b/src/gyro_drift.rs new file mode 100644 index 0000000..03d6a17 --- /dev/null +++ b/src/gyro_drift.rs @@ -0,0 +1,6 @@ +//! Estimations for gyroscope measurements. + +mod filter; +mod types; + +pub use filter::*; diff --git a/src/gyro_drift/filter.rs b/src/gyro_drift/filter.rs new file mode 100644 index 0000000..7150064 --- /dev/null +++ b/src/gyro_drift/filter.rs @@ -0,0 +1,290 @@ +use minikalman::buffers::types::*; +use minikalman::matrix::MatrixDataType; +use minikalman::prelude::*; +use minikalman::regular::{RegularKalmanBuilder, RegularObservationBuilder}; + +use crate::gyro_drift::types::*; + +/// An estimator for gyroscope axis-specific angular velocity and bias. +pub struct GyroscopeAxisEstimator { + filter: OwnedKalmanFilter, + measurement: OwnedObservation, +} + +impl GyroscopeAxisEstimator { + /// Initializes a new instance of the [`GyroscopeAxisEstimator`] struct. + /// + /// ## Arguments + /// * `drift_estimate` - The initial estimate for the axis drift value. + /// * `gyro_noise` - The gyroscope noise values (sigma-squared) for the axis. + /// * `process_noise` - A process noise value. + pub fn new(drift_estimate: T, gyro_noise: T, process_noise: T) -> Self + where + T: MatrixDataType + Default, + { + let filter = Self::build_filter(drift_estimate, gyro_noise, process_noise); + let measurement = Self::build_measurement(gyro_noise); + + Self { + filter, + measurement, + } + } +} + +impl GyroscopeAxisEstimator { + /// Performs a prediction step to obtain new gyroscope estimates. + pub fn predict(&mut self, delta_t: T) + where + T: MatrixDataType, + { + self.filter.state_transition_mut().set(0, 1, delta_t); + self.filter.predict(); + } + + /// Performs a correction step using accelerometer and magnetometer readings. + /// + /// ## Arguments + /// * `accelerometer` - The accelerometer reading. + pub fn correct(&mut self, angular_velocity: T) + where + T: MatrixDataType, + { + self.measurement.measurement_vector_mut().apply(|vec| { + vec.set_row(0, -angular_velocity); + }); + + // Perform the update step. + self.filter.correct(&mut self.measurement); + } + + /// Gets the estimated angular velocity. + pub fn angular_velocity(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(0) + } + + /// Gets the estimated bias term. + pub fn bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(1) + } +} + +impl GyroscopeAxisEstimator { + /// Builds the Kalman filter used for prediction. + fn build_filter( + drift_estimate: T, + sensor_noise: T, + process_noise_value: T, + ) -> OwnedKalmanFilter + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // State vector. + let mut state_vec = + StateVectorBuffer::::new(MatrixData::new_array::( + [zero; STATES], + )); + state_vec.apply(|vec| { + vec.set_row(0, T::zero()); + vec.set_row(1, drift_estimate); + }); + + // State transition matrix. + let mut state_transition = + StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + state_transition.make_identity(); + + // Estimate covariance matrix. + let mut estimate_covariance = + EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + estimate_covariance.make_scalar(sensor_noise); + + // Process noise matrix. + let mut process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * STATES }], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Predicted state vector. + let predicted_state = + PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< + STATES, + 1, + STATES, + T, + >([zero; STATES])); + + // Temporary estimate covariance matrix. + let temp_state_matrix = + TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularKalmanBuilder::new::( + state_transition, + state_vec, + estimate_covariance, + process_noise, + predicted_state, + temp_state_matrix, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_measurement(axis_noise: T) -> OwnedObservation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + observation_matrix.apply(|mat| { + mat.set_col(0, T::one()); + mat.set_col(1, T::zero()); + }); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >([zero; { OBSERVATIONS * OBSERVATIONS }]), + ); + noise_covariance.apply(|mat| { + mat.set_at(0, 0, axis_noise); + }); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >( + [zero; { OBSERVATIONS * OBSERVATIONS }], + )); + + // Kalman Gain matrix + let kalman_gain = + KalmanGainMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary residual covariance inverted matrix + let temp_sinv = TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { OBSERVATIONS * OBSERVATIONS }], + ), + ); + + // Temporary H×P matrix + let temp_hp = + TemporaryHPMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + + // Temporary P×Hᵀ matrix + let temp_pht = + TemporaryPHTMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } +} diff --git a/src/gyro_drift/types.rs b/src/gyro_drift/types.rs new file mode 100644 index 0000000..5a30bc9 --- /dev/null +++ b/src/gyro_drift/types.rs @@ -0,0 +1,79 @@ +use minikalman::buffers::types::*; +use minikalman::prelude::*; +use minikalman::regular::{RegularKalman, RegularObservation}; + +pub const STATES: usize = 2; // angular velocity, bias +pub const OBSERVATIONS: usize = 1; // angular velocity + +// A Kalman filter of four states, using owned buffers. +pub type OwnedKalmanFilter = RegularKalman< + STATES, + T, + StateTransitionMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + StateVectorBuffer>, + EstimateCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + >, + DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + PredictedStateEstimateVectorBuffer>, + TemporaryStateMatrixBuffer>, +>; + +/// On observation of three states, using owned buffers. +pub type OwnedObservation = RegularObservation< + STATES, + OBSERVATIONS, + T, + ObservationMatrixMutBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + MeasurementVectorBuffer>, + MeasurementNoiseCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + InnovationVectorBuffer>, + InnovationCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + KalmanGainMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryResidualCovarianceInvertedMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryHPMatrixBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + TemporaryPHTMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryKHPMatrixBuffer>, +>; diff --git a/src/gyro_free.rs b/src/gyro_free.rs new file mode 100644 index 0000000..756a1a0 --- /dev/null +++ b/src/gyro_free.rs @@ -0,0 +1,4 @@ +mod filter; +mod types; + +pub use crate::gyro_free::filter::*; diff --git a/src/gyro_free/filter.rs b/src/gyro_free/filter.rs new file mode 100644 index 0000000..a351504 --- /dev/null +++ b/src/gyro_free/filter.rs @@ -0,0 +1,752 @@ +use crate::gyro_free::types::*; +use crate::vector3::Vector3; +use crate::{ + Abs, AccelerometerNoise, AccelerometerReading, ArcSin, ArcTan, EulerAngles, IsNaN, + MagnetometerNoise, MagnetometerReading, NormalizeAngle, +}; +use minikalman::buffers::types::*; +use minikalman::extended::{ExtendedKalmanBuilder, ExtendedObservationBuilder}; +use minikalman::matrix::MatrixDataType; +use minikalman::prelude::*; +use num_traits::Zero; + +/// A magnetic field reference vector. +pub type MagneticReference = MagnetometerReading; + +/// A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator with generic type `T`. +pub struct OwnedOrientationEstimator { + filter: OwnedKalmanFilter, + /// Magnetometer measurements. + mag_measurement: OwnedVector3Observation, + /// Accelerometer measurements. + acc_measurement: OwnedVector3Observation, + /// Magnetic field reference vector for the current location. + magnetic_field_ref: Vector3, +} + +impl OwnedOrientationEstimator { + /// Initializes a new instance of the [`OwnedOrientationEstimator`] struct. + /// + /// ## Arguments + /// * `accelerometer_noise` - The accelerometer noise values (sigma-squared) for each axis. + /// * `magnetometer_noise` - The magnetometer noise values (sigma-squared) for each axis. + /// * `magnetic_field_ref` - The magnetic field reference vector for the current location. + /// * `process_noise` - A process noise value. + /// * `epsilon` - A small bias term to avoid divisions by zero. Set to e.g. `1e-6`. + pub fn new( + accelerometer_noise: AccelerometerNoise, + magnetometer_noise: MagnetometerNoise, + magnetic_field_ref: MagneticReference, + process_noise: T, + ) -> Self + where + T: MatrixDataType + Default, + { + let filter = Self::build_filter(process_noise); + let mag_measurement = Self::build_mag_measurement(&magnetometer_noise); + let acc_measurement = Self::build_accel_measurement(&accelerometer_noise); + + let magnetic_field_ref = Vector3::new( + magnetic_field_ref.x, + magnetic_field_ref.y, + magnetic_field_ref.z, + ) + .normalized(); + + Self { + filter, + mag_measurement, + acc_measurement, + magnetic_field_ref, + } + } +} + +impl OwnedOrientationEstimator { + /// Performs a prediction step to obtain new orientation estimates. + pub fn predict(&mut self) + where + T: MatrixDataType + IsNaN, + { + // Perform a regular Kalman Filter prediction step. + self.filter + .predict_nonlinear(|current, next| current.copy(next)); + self.panic_if_nan(); + } + + fn apply_normalized_vector(vec: V, measurement: &mut M) + where + M: RowVectorMut<3, T>, + V: Into>, + T: MatrixDataType, + { + let vec: Vector3 = vec.into().normalized(); + measurement.set_row(0, vec.x); + measurement.set_row(1, vec.y); + measurement.set_row(2, vec.z); + } + + /// Performs a correction step using accelerometer and magnetometer readings. + /// + /// ## Arguments + /// * `accelerometer` - The accelerometer reading. + pub fn correct_accelerometer(&mut self, accelerometer: &AccelerometerReading) + where + T: MatrixDataType + IsNaN, + { + // Normalize the vectors. + self.acc_measurement.measurement_vector_mut().apply(|vec| { + Self::apply_normalized_vector(accelerometer, vec); + }); + + // Update the Jacobian. + let two = T::one() + T::one(); + let (q0, q1, q2, q3) = self.estimated_quaternion(); + + self.acc_measurement + .observation_jacobian_matrix_mut() + .apply(|mat| { + let two_q0 = q0 * two; + let two_q1 = q1 * two; + let two_q2 = q2 * two; + let two_q3 = q3 * two; + + mat.set_at(0, 0, two_q2); + mat.set_at(0, 1, two_q3); + mat.set_at(0, 2, two_q0); + mat.set_at(0, 3, two_q1); + + mat.set_at(1, 0, -two_q1); + mat.set_at(1, 1, -two_q0); + mat.set_at(1, 2, two_q3); + mat.set_at(1, 3, two_q2); + + mat.set_at(2, 0, two_q0); + mat.set_at(2, 1, -two_q1); + mat.set_at(2, 2, -two_q2); + mat.set_at(2, 3, two_q3); + }); + + // Perform the update step. + self.filter + .correct_nonlinear(&mut self.acc_measurement, |state, measurement| { + let down = Vector3::new(T::zero(), T::zero(), T::one()); + let rotated = Self::rotate_vector_internal(state, down); + measurement.set_row(0, rotated.x); + measurement.set_row(1, rotated.y); + measurement.set_row(2, rotated.z); + + measurement.set_row(0, two * (q1 * q3 + q0 * q2)); + measurement.set_row(1, two * (q2 * q3 - q0 * q1)); + measurement.set_row(2, T::one() - two * (q1 * q1 + q2 * q2)); + }); + + self.normalize_state_quaternion(); + self.panic_if_nan(); + } + + /// Performs a correction step using accelerometer readings. + /// + /// ## Arguments + /// * `magnetometer` - The magnetometer reading. + pub fn correct_magnetometer(&mut self, magnetometer: &MagnetometerReading) + where + T: MatrixDataType + IsNaN, + { + // Normalize the vector. + self.mag_measurement.measurement_vector_mut().apply(|vec| { + Self::apply_normalized_vector(magnetometer, vec); + }); + + // Update the Jacobian. + let one = T::one(); + let two = one + one; + let (q0, q1, q2, q3) = self.estimated_quaternion(); + let (mx, my, mz) = self.magnetic_field_ref.into(); + self.mag_measurement + .observation_jacobian_matrix_mut() + .apply(|mat| { + mat.set_at(0, 0, two * (q0 * mx - q3 * my + q2 * mz)); + mat.set_at(0, 1, two * (q1 * mx + q2 * my + q3 * mz)); + mat.set_at(0, 2, two * (q1 * my - q2 * mx + q0 * mz)); + mat.set_at(0, 3, two * (q1 * mz - q0 * my - q3 * mx)); + + mat.set_at(1, 0, two * (q3 * mx + q0 * my - q1 * mz)); + mat.set_at(1, 1, two * (q2 * mx - q1 * my - q0 * mz)); + mat.set_at(1, 2, two * (q1 * mx + q2 * my + q3 * mz)); + mat.set_at(1, 3, two * (q0 * mx - q3 * my + q2 * mz)); + + mat.set_at(2, 0, two * (q1 * my - q2 * mx + q0 * mz)); + mat.set_at(2, 1, two * (q3 * mx + q0 * my - q1 * mz)); + mat.set_at(2, 2, two * (q3 * my - q0 * mx - q2 * mz)); + mat.set_at(2, 3, two * (q1 * mx + q2 * my + q3 * mz)); + }); + + // Perform the update step. + self.filter + .correct_nonlinear(&mut self.mag_measurement, |state, measurement| { + let reference = self.magnetic_field_ref; + let rotated = Self::rotate_vector_internal(state, reference); + measurement.set_row(0, rotated.x); + measurement.set_row(1, rotated.y); + measurement.set_row(2, rotated.z); + }); + + self.normalize_state_quaternion(); + self.panic_if_nan(); + } + + fn rotate_vector_internal(state: &V, vec: Vector3) -> Vector3 + where + V: RowVectorMut, + T: MatrixDataType, + { + let q0 = state.get_row(0); + let q1 = state.get_row(1); + let q2 = state.get_row(2); + let q3 = state.get_row(3); + + let one = T::one(); + let two = one + one; + let x = vec.x * (one - two * (q2 * q2 + q3 * q3)) + + vec.y * two * (q1 * q2 - q0 * q3) + + vec.z * two * (q1 * q3 + q0 * q2); + + let y = vec.x * two * (q1 * q2 + q0 * q3) + + vec.y * (one - two * (q1 * q1 + q3 * q3)) + + vec.z * two * (q2 * q3 - q0 * q1); + + let z = vec.x * two * (q1 * q3 - q0 * q2) + + vec.y * two * (q2 * q3 + q0 * q1) + + vec.z * (one - two * (q1 * q1 + q2 * q2)); + + Vector3::new(x, y, z) + } + + /// Rotate a vector in the body frame. + /// + /// For display purposes you likely want to use [`rotate_vector_world`](Self::rotate_vector_world) instead. + pub fn rotate_vector_body(&self, vector: Vector3) -> Vector3 + where + T: MatrixDataType, + { + Self::rotate_vector_internal(self.filter.state_vector(), vector) + } + + /// Rotates a vector in the world frame. + pub fn rotate_vector_world(&self, vec: Vector3) -> Vector3 + where + T: MatrixDataType, + { + let state = self.filter.state_vector(); + let q0 = state.get_row(0); + let q1 = state.get_row(1); + let q2 = state.get_row(2); + let q3 = state.get_row(3); + + let one = T::one(); + let two = one + one; + let x = vec.x * (one - two * (q2 * q2 + q3 * q3)) + + vec.y * two * (q1 * q2 + q0 * q3) + + vec.z * two * (q1 * q3 - q0 * q2); + + let y = vec.x * two * (q1 * q2 - q0 * q3) + + vec.y * (one - two * (q1 * q1 + q3 * q3)) + + vec.z * two * (q2 * q3 + q0 * q1); + + let z = vec.x * two * (q1 * q3 + q0 * q2) + + vec.y * two * (q2 * q3 - q0 * q1) + + vec.z * (one - two * (q1 * q1 + q2 * q2)); + + Vector3::new(x, y, z) + } + + fn normalize_state_quaternion(&mut self) + where + T: MatrixDataType, + { + self.filter.state_vector_mut().apply(|vec| { + let w = vec.get_row(0); + let x = vec.get_row(1); + let y = vec.get_row(2); + let z = vec.get_row(3); + + let (w, x, y, z) = if w >= T::zero() { + (w, x, y, z) + } else { + (-w, -x, -y, -z) + }; + + let norm_sq = w * w + x * x + y * y + z * z; + let norm = norm_sq.square_root(); + let w = w / norm; + let x = x / norm; + let y = y / norm; + let z = z / norm; + vec.set_row(0, w); + vec.set_row(1, x); + vec.set_row(2, y); + vec.set_row(3, z); + }); + } + + #[allow(unused)] + fn panic_if_nan(&self) + where + T: Copy + IsNaN, + { + #[cfg(debug_assertions)] + self.filter.state_vector().inspect(|vec| { + if vec.get_row(0).is_nan() || vec.get_row(1).is_nan() || vec.get_row(2).is_nan() { + panic!("NaN angle detected in state estimate") + } + }); + } + + /// Gets the estimated quaternion in (w, x, y, z) order. + fn estimated_quaternion(&self) -> (T, T, T, T) + where + T: Copy, + { + self.filter.state_vector().inspect(|vec| { + ( + vec.get_row(0), + vec.get_row(1), + vec.get_row(2), + vec.get_row(3), + ) + }) + } + + /// Obtains the current estimates of the roll, pitch and yaw angles. + pub fn estimated_angles(&self) -> EulerAngles + where + T: MatrixDataType + + Abs + + ArcSin + + ArcTan + + NormalizeAngle, + { + EulerAngles::new(self.roll(), self.pitch(), self.yaw()) + } + + /// Obtains the current estimate of the roll angle φ (phi), in radians. + /// + /// The roll angle is defined as the amount rotation around the y-axis (forward). + pub fn roll(&self) -> T + where + T: MatrixDataType + ArcTan + NormalizeAngle, + { + let (w, x, y, z) = self.estimated_quaternion(); + let one = T::one(); + let two = one + one; + let sinr_cosp = two * (w * x + y * z); + let cosr_cosp = one - two * (x * x + y * y); + let roll = sinr_cosp.atan2(cosr_cosp); + roll.normalize_angle() + } + + /// Obtains the current estimation variance (uncertainty) of the roll angle φ (phi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn roll_variance(&self) -> T + where + T: Zero, + { + T::zero() + } + + /// Obtains the current estimate of the pitch angle θ (theta), in radians. + /// + /// The pitch angle is defined as the amount rotation around the x-axis (right). + pub fn pitch(&self) -> T + where + T: MatrixDataType + Abs + ArcSin + NormalizeAngle, + { + let (w, x, y, z) = self.estimated_quaternion(); + let one = T::one(); + let two = one + one; + let sinp = two * (w * y - z * x); + // TODO: If sin >= 1.0 || sin <= -1.0, clamp to +/- pi/2 instead + let pitch = sinp.arcsin(); + pitch.normalize_angle() + } + + /// Obtains the current estimation variance (uncertainty) of the pitch angle θ (theta), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn pitch_variance(&self) -> T + where + T: Zero, + { + T::zero() + } + + /// Obtains the current estimate of the yaw angle ψ (psi), in radians. + /// + /// The yaw angle is defined as the amount rotation around the z-axis (up). + pub fn yaw(&self) -> T + where + T: MatrixDataType + ArcTan + NormalizeAngle, + { + let (w, x, y, z) = self.estimated_quaternion(); + let one = T::one(); + let two = one + one; + let siny_cosp = two * (w * z + x * y); + let cosy_cosp = one - two * (y * y + z * z); + let yaw = siny_cosp.atan2(cosy_cosp); + yaw.normalize_angle() + } + + /// Obtains the current estimation variance (uncertainty) of the yaw angle ψ (psi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn yaw_variance(&self) -> T + where + T: Zero, + { + T::zero() + } +} + +impl OwnedOrientationEstimator { + /// Builds the Kalman filter used for prediction. + fn build_filter(process_noise_value: T) -> OwnedKalmanFilter + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // State vector. + let mut state_vec = + StateVectorBuffer::::new(MatrixData::new_array::( + [zero; STATES], + )); + + // TODO: Seed with roll/pitch from accelerometer, yaw (heading) from magnetometer. + state_vec.set_row(0, T::one()); + state_vec.set_row(1, T::zero()); + state_vec.set_row(2, T::zero()); + state_vec.set_row(3, T::zero()); + + // State transition matrix. + let mut state_transition = + StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + state_transition.make_identity(); + + // Estimate covariance matrix. + let mut estimate_covariance = + EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + estimate_covariance.make_scalar(process_noise_value); + + // Process noise matrix. + let mut process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * STATES }], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Predicted state vector. + let mut predicted_state = + PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< + STATES, + 1, + STATES, + T, + >([zero; STATES])); + predicted_state.set_all(T::zero()); + + // Temporary estimate covariance matrix. + let temp_state_matrix = + TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + ExtendedKalmanBuilder::new::( + state_transition, + state_vec, + estimate_covariance, + process_noise, + predicted_state, + temp_state_matrix, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_mag_measurement( + magnetometer_noise: &MagnetometerNoise, + ) -> OwnedVector3Observation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + observation_matrix.set_all(T::zero()); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + noise_covariance.apply(|mat| { + mat.set_at(0, 0, magnetometer_noise.x); + mat.set_at(1, 1, magnetometer_noise.y); + mat.set_at(2, 2, magnetometer_noise.z); + }); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Kalman Gain matrix + let kalman_gain = KalmanGainMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary residual covariance inverted matrix + let temp_sinv = + TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Temporary H×P matrix + let temp_hp = TemporaryHPMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + + // Temporary P×Hᵀ matrix + let temp_pht = TemporaryPHTMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + ExtendedObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_accel_measurement( + accelerometer_noise: &AccelerometerNoise, + ) -> OwnedVector3Observation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + observation_matrix.set_all(T::zero()); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + noise_covariance.apply(|mat| { + mat.set_at(0, 0, accelerometer_noise.x); + mat.set_at(1, 1, accelerometer_noise.y); + mat.set_at(2, 2, accelerometer_noise.z); + }); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Kalman Gain matrix + let kalman_gain = KalmanGainMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary residual covariance inverted matrix + let temp_sinv = + TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Temporary H×P matrix + let temp_hp = TemporaryHPMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + + // Temporary P×Hᵀ matrix + let temp_pht = TemporaryPHTMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + ExtendedObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } +} diff --git a/src/gyro_free/types.rs b/src/gyro_free/types.rs new file mode 100644 index 0000000..a162f79 --- /dev/null +++ b/src/gyro_free/types.rs @@ -0,0 +1,97 @@ +use minikalman::buffers::types::*; +use minikalman::extended::{ExtendedKalman, ExtendedObservation}; +use minikalman::prelude::*; + +pub const STATES: usize = 4; // quaternion components +pub const VEC3_OBSERVATIONS: usize = 3; // x, y, z + +// A Kalman filter of four states, using owned buffers. +pub type OwnedKalmanFilter = ExtendedKalman< + STATES, + T, + StateTransitionMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + StateVectorBuffer>, + EstimateCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + >, + DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + PredictedStateEstimateVectorBuffer>, + TemporaryStateMatrixBuffer>, +>; + +/// On observation of three states, using owned buffers. +pub type OwnedVector3Observation = ExtendedObservation< + STATES, + VEC3_OBSERVATIONS, + T, + ObservationMatrixMutBuffer< + VEC3_OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + MeasurementVectorBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + MeasurementNoiseCovarianceMatrixBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >, + >, + InnovationVectorBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + InnovationCovarianceMatrixBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + KalmanGainMatrixBuffer< + STATES, + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryResidualCovarianceInvertedMatrixBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >, + >, + TemporaryHPMatrixBuffer< + VEC3_OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + TemporaryPHTMatrixBuffer< + STATES, + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryKHPMatrixBuffer>, +>; diff --git a/src/gyroscope_bias.rs b/src/gyroscope_bias.rs new file mode 100644 index 0000000..d863657 --- /dev/null +++ b/src/gyroscope_bias.rs @@ -0,0 +1,120 @@ +use crate::impl_standard_traits; +use core::fmt::{Debug, Formatter}; +use core::ops::{Mul, Sub}; +use uniform_array_derive::UniformArray; + +#[derive(UniformArray)] +#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] +#[repr(C)] +pub struct GyroscopeBias { + /// The angular rate around the x-axis, in radians per second. + pub omega_x: T, + /// The angular rate around the y-axis, in radians per second. + pub omega_y: T, + /// The angular rate around the z-axis, in radians per second. + pub omega_z: T, +} + +impl GyroscopeBias { + /// Initializes a new [`GyroscopeBias`] instance. + #[inline(always)] + pub const fn new(omega_x: T, omega_y: T, omega_z: T) -> Self { + Self { + omega_x, + omega_y, + omega_z, + } + } +} + +impl Default for GyroscopeBias +where + T: Default, +{ + #[inline] + fn default() -> Self { + Self::new(Default::default(), Default::default(), Default::default()) + } +} + +impl Clone for GyroscopeBias +where + T: Clone, +{ + fn clone(&self) -> Self { + Self { + omega_x: self.omega_x.clone(), + omega_y: self.omega_y.clone(), + omega_z: self.omega_z.clone(), + } + } +} + +impl Debug for GyroscopeBias +where + T: Debug, +{ + fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { + f.debug_tuple("GyroscopeBias") + .field(&self.omega_x) + .field(&self.omega_y) + .field(&self.omega_z) + .finish() + } +} + +impl Mul for GyroscopeBias +where + T: Mul + Clone, +{ + type Output = GyroscopeBias; + + fn mul(self, rhs: T) -> Self::Output { + Self { + omega_x: self.omega_x * rhs.clone(), + omega_y: self.omega_y * rhs.clone(), + omega_z: self.omega_z * rhs.clone(), + } + } +} + +impl Sub for GyroscopeBias +where + T: Sub + Clone, +{ + type Output = GyroscopeBias; + + fn sub(self, rhs: T) -> Self::Output { + Self { + omega_x: self.omega_x - rhs.clone(), + omega_y: self.omega_y - rhs.clone(), + omega_z: self.omega_z - rhs.clone(), + } + } +} + +impl_standard_traits!(GyroscopeBias, T); + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_len() { + let reading = GyroscopeBias::::default(); + assert_eq!(reading.len(), 3); + } + + #[test] + fn test_index() { + let reading = GyroscopeBias:: { + omega_x: 1.0, + omega_y: 2.0, + omega_z: 3.0, + }; + + assert_eq!(reading[0], 1.0); + assert_eq!(reading[1], 2.0); + assert_eq!(reading[2], 3.0); + } +} diff --git a/src/gyroscope_noise.rs b/src/gyroscope_noise.rs new file mode 100644 index 0000000..f1b3160 --- /dev/null +++ b/src/gyroscope_noise.rs @@ -0,0 +1,101 @@ +use crate::impl_standard_traits; +use core::fmt::{Debug, Formatter}; +use core::ops::Mul; +use uniform_array_derive::UniformArray; + +#[derive(UniformArray)] +#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] +#[repr(C)] +pub struct GyroscopeNoise { + /// The rotation rate noise along the x-axis. + pub x: T, + /// The rotation rate noise along the y-axis. + pub y: T, + /// The rotation rate noise along the z-axis. + pub z: T, +} + +impl GyroscopeNoise { + /// Initializes a new [`GyroscopeNoise`] instance. + #[inline(always)] + pub const fn new(x: T, y: T, z: T) -> Self { + Self { x, y, z } + } +} + +impl Default for GyroscopeNoise +where + T: Default, +{ + #[inline] + fn default() -> Self { + Self::new(Default::default(), Default::default(), Default::default()) + } +} + +impl Clone for GyroscopeNoise +where + T: Clone, +{ + fn clone(&self) -> Self { + Self { + x: self.x.clone(), + y: self.y.clone(), + z: self.z.clone(), + } + } +} + +impl Debug for GyroscopeNoise +where + T: Debug, +{ + fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { + f.debug_tuple("GyroscopeNoise") + .field(&self.x) + .field(&self.y) + .field(&self.z) + .finish() + } +} + +impl Mul for GyroscopeNoise +where + T: Mul + Clone, +{ + type Output = GyroscopeNoise; + + fn mul(self, rhs: T) -> Self::Output { + Self { + x: self.x * rhs.clone(), + y: self.y * rhs.clone(), + z: self.z * rhs.clone(), + } + } +} + +impl_standard_traits!(GyroscopeNoise, T); + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_len() { + let reading = GyroscopeNoise::::default(); + assert_eq!(reading.len(), 3); + } + + #[test] + fn test_index() { + let reading = GyroscopeNoise:: { + x: 1.0, + y: 2.0, + z: 3.0, + }; + + assert_eq!(reading[0], 1.0); + assert_eq!(reading[1], 2.0); + assert_eq!(reading[2], 3.0); + } +} diff --git a/src/gyroscope_reading.rs b/src/gyroscope_reading.rs index 9eb6470..597c1cd 100644 --- a/src/gyroscope_reading.rs +++ b/src/gyroscope_reading.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; -use core::ops::Mul; +use core::ops::{Mul, Sub}; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct GyroscopeReading { @@ -24,11 +26,20 @@ impl GyroscopeReading { } } - /// Returns the length of the [`GyroscopeReading`] vector. - #[inline(always)] - #[allow(unused)] - pub const fn len(&self) -> usize { - 3 + /// Constructs a new [`GyroscopeReading`] instance from a reading in a given coordinate frame. + #[cfg(feature = "coordinate-frame")] + #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] + pub fn north_east_down(coordinate: C) -> Self + where + C: Into>, + T: Clone, + { + let coordinate = coordinate.into(); + Self { + omega_x: coordinate.x(), + omega_y: coordinate.y(), + omega_z: coordinate.z(), + } } } @@ -83,31 +94,30 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for GyroscopeReading { - type Output = T; +impl Sub for GyroscopeReading +where + T: Sub + Clone, +{ + type Output = GyroscopeReading; - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.omega_x, - 1 => &self.omega_y, - 2 => &self.omega_z, - _ => panic!("Index out of bounds"), + fn sub(self, rhs: T) -> Self::Output { + Self { + omega_x: self.omega_x - rhs.clone(), + omega_y: self.omega_y - rhs.clone(), + omega_z: self.omega_z - rhs.clone(), } } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for GyroscopeReading { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.omega_x, - 1 => &mut self.omega_y, - 2 => &mut self.omega_z, - _ => panic!("Index out of bounds"), - } +#[cfg(feature = "coordinate-frame")] +#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] +impl From for GyroscopeReading +where + C: coordinate_frame::CoordinateFrame, + T: Copy + coordinate_frame::SaturatingNeg, +{ + fn from(value: C) -> Self { + Self::north_east_down(value.to_ned()) } } diff --git a/src/lib.rs b/src/lib.rs index c2b523f..338277d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -9,773 +9,27 @@ mod accelerometer_noise; mod accelerometer_reading; mod euler_angles; +pub mod gyro_drift; +pub mod gyro_free; +mod gyroscope_bias; +mod gyroscope_noise; mod gyroscope_reading; mod macros; mod magnetometer_noise; mod magnetometer_reading; mod num_traits; +#[deprecated(note = "This is a test implementation not suitable for real use")] +pub mod test_estimator; mod vector3; -use crate::accelerometer_noise::AccelerometerNoise; -use crate::accelerometer_reading::AccelerometerReading; -use crate::euler_angles::EulerAngles; -use crate::magnetometer_noise::MagnetometerNoise; -use crate::magnetometer_reading::MagnetometerReading; -use crate::vector3::Vector3; -use core::ops::Neg; -use minikalman::buffers::types::*; -use minikalman::prelude::*; -use minikalman::regular::{ - Control, ControlBuilder, RegularKalman, RegularKalmanBuilder, RegularObservation, - RegularObservationBuilder, -}; +pub use crate::accelerometer_noise::AccelerometerNoise; +pub use crate::accelerometer_reading::AccelerometerReading; +pub use crate::euler_angles::EulerAngles; +pub use crate::gyroscope_bias::GyroscopeBias; +pub use crate::gyroscope_noise::GyroscopeNoise; +pub use crate::gyroscope_reading::GyroscopeReading; +pub use crate::magnetometer_noise::MagnetometerNoise; +pub use crate::magnetometer_reading::MagnetometerReading; +pub use crate::vector3::Vector3; pub use crate::num_traits::*; - -const STATES: usize = 3; // roll rate, pitch rate, yaw rate -const CONTROLS: usize = 3; // roll rate, pitch rate, yaw rate -const OBSERVATIONS: usize = 3; // roll, pitch, yaw - -/// A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator. -pub struct OrientationEstimator {} - -/// An owned orientation estimator with generic type `T`. -pub struct OwnedOrientationEstimator { - filter: OwnedKalmanFilter, - control: OwnedControlInput, - measurement: OwnedObservation, - /// The angular tolerance, in radians, to detect a Gimbal Lock. - gimbal_lock_tolerance: T, - /// The autocovariances of the noise terms. - accelerometer_noise: AccelerometerNoise, - /// The autocovariances of the noise terms. - magnetometer_noise: MagnetometerNoise, - /// A bias term to avoid divisions by zero. - epsilon: T, -} - -impl OwnedOrientationEstimator { - /// Initializes a new instance of the [`OwnedOrientationEstimator`] struct. - /// - /// ## Arguments - /// * `gimbal_lock_tolerance` - The angular tolerance, in radians, to detect a Gimbal Lock. - /// * `accelerometer_noise` - The accelerometer noise values (sigma-squared) for each axis. - /// * `magnetometer_noise` - The magnetometer noise values (sigma-squared) for each axis. - /// * `epsilon` - A small bias term to avoid divisions by zero. Set to e.g. `1e-6`. - pub fn new( - gimbal_lock_tolerance: T, - accelerometer_noise: AccelerometerNoise, - magnetometer_noise: MagnetometerNoise, - epsilon: T, - ) -> Self - where - T: MatrixDataType + Default, - { - let filter = Self::build_filter(); - let control = Self::build_control(); - let measurement = Self::build_measurement(); - - Self { - filter, - control, - measurement, - gimbal_lock_tolerance, - accelerometer_noise, - magnetometer_noise, - epsilon, - } - } -} - -impl OwnedOrientationEstimator { - /// Performs a prediction step to obtain new orientation estimates. - /// - /// ## Arguments - /// * `delta_t` - The time step for the prediction. - /// * `angular_rates` - The angular rates measured by the magnetometer. - pub fn predict(&mut self, delta_t: T, angular_rates: &MagnetometerReading) - where - T: MatrixDataType + Default, - { - // Update the Kalman filter components. - self.update_state_transition_matrix(delta_t, angular_rates); - self.update_control_input(delta_t, angular_rates); - - // Perform a regular Kalman Filter prediction step. - self.filter.predict(); - self.filter.control(&mut self.control); - - // TODO: Clamp any angles? - } - - /// Performs a correction step using accelerometer and magnetometer readings. - /// - /// ## Arguments - /// * `accelerometer` - The accelerometer reading. - /// * `magnetometer` - The magnetometer reading. - pub fn correct( - &mut self, - accelerometer: &AccelerometerReading, - magnetometer: &MagnetometerReading, - ) where - T: MatrixDataType - + ArcSin - + ArcTan - + DetectGimbalLock - + core::fmt::Debug, - { - // Normalize the vectors. - let a = Vector3::from(accelerometer).normalized(); - let m = Vector3::from(magnetometer).normalized(); - - // Obtain the Euler Angles and apply them to the measurement. - let angles = self.build_triad(a, m); - self.measurement.measurement_vector_mut().apply(|vec| { - vec.set_row(0, angles.roll_phi); - vec.set_row(1, angles.pitch_theta); - vec.set_row(2, angles.yaw_psi); - }); - - // Update the measurement noise. - self.update_measurement_noise(a, m); - - // Perform the update step. - self.filter.correct(&mut self.measurement); - - // TODO: Clamp any angles? - } - - /// Computes the Jacobian matrices and measurement noise matrix for orientation estimation - /// - /// This is done by propagating the sensor noise through the TRIAD method - /// The final measurement noise covariance matrix R is computed using J_a, R_a, J_m, and R_m - /// R represents the combined effect of accelerometer and magnetometer noise on the orientation estimates, - /// where J_a and J_m are the Jacobians of the respective measurement noise. - /// - /// ## Arguments - /// * `accelerometer` - The normalized accelerometer vector. - /// * `magnetometer` - The normalized magnetometer vector. - fn update_measurement_noise(&mut self, accelerometer: Vector3, magnetometer: Vector3) - where - T: MatrixDataType + core::fmt::Debug, - { - // Easy access to accelerometer noise. - let sa11 = self.accelerometer_noise.x; // sigma_a_xx - let sa22 = self.accelerometer_noise.y; // sigma_a_yy - let sa33 = self.accelerometer_noise.z; // sigma_a_zz - - // Easy access to magnetometer noise. - let sm11 = self.magnetometer_noise.x; // sigma_m_xx - let sm22 = self.magnetometer_noise.y; // sigma_m_yy - let sm33 = self.magnetometer_noise.z; // sigma_m_zz - - // Helper "constants". - let two = T::one() + T::one(); - let epsilon = self.epsilon; - - // Easy access to accelerometer components. - let ax = accelerometer.x; - let ay = accelerometer.y; - let az = accelerometer.z; - - // Easy access to magnetometer components. - let mx = magnetometer.x; - let my = magnetometer.y; - let mz = magnetometer.z; - - // Common squares of the accelerometer components. - let ax2 = accelerometer.x * accelerometer.x; - let ay2 = accelerometer.y * accelerometer.y; - let az2 = accelerometer.z * accelerometer.z; - - // Common squares of the magnetometer components. - let mx2 = magnetometer.x * magnetometer.x; - let my2 = magnetometer.y * magnetometer.y; - let mz2 = magnetometer.z * magnetometer.z; - - // Noise matrix. - let r = self.measurement.measurement_noise_covariance_mut(); - - // Common denominator of the matrix components. - #[rustfmt::skip] - let denom = - ax2 * az2 * mx2 - + ax2 * my2 - + two * ax2 * ay * az2 * mx * my - - two * ax * ay * mx * my - + two * ax * az2 * az * mx * mz - - two * ax * az * mx * mz - + ay2 * az2 * my2 - + ay2 * mx2 - + two * ay * az2 * az * my * mz - - two * ay * az * my * mz - + az2 * az2 * mz2 - - two * az2 * mz2 - + mz2; - - let one_minus_az2 = T::one() - az2; - let one_minus_az2_sqrt = one_minus_az2.square_root(); - - // Row 1, column 1. - let r11_nom = sa11 * sq(ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz) - + sa22 * sq(ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) - + sa33 - * sq( - ax2 * mx * my - ax * my * mx2 + ax * ay * my2 + two * ax * az * my * mz - - ay2 * mx * my - - two * ay * az * mx * mz, - ) - + sm11 * sq(ax2 * az * my + ay2 * az * my + ay * az2 * mz - ay * mz) - + sm22 * sq(ax2 * az * mx + ax * az2 * mz - ax * mz + ay2 * az * mx) - + sm33 * sq(ax * az2 * my - ax * my - ay * az2 * mx + ay * mx); - let r11 = r11_nom / (sq(denom) + epsilon); - r.set_at(0, 0, r11); - - // Row 1, column 2 and row 2, column 1. - let r12_nom = sa33 - * (-ax2 * mx * my + ax * ay * mx2 - ax * ay * my2 - two * ax * az * my * mz - + ay2 * mx * my - + two * ay * az * mx * mz); - let r12 = r12_nom / (one_minus_az2_sqrt * denom + epsilon); - r.set_symmetric(0, 1, r12); - - // Row 1, column 3 and row 3, column 1. - let r13_nom = ax * sa22 * (ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) - + ay * sa11 * (ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz); - let r13 = r13_nom / ((ax2 + ay2) * denom + epsilon); - r.set_symmetric(0, 2, r13); - - // Row 2, column 2. - let r22 = -sa33 / (ax2 - T::one() + epsilon); - r.set_at(1, 1, r22); - - // Row 2, column 3 and row 3, column 2. - r.set_symmetric(1, 2, T::zero()); - - // Row 3, column 3. - let r33 = (ax2 * sa22 + ay2 * sa11) / sq(ax2 + ay2 + epsilon); - r.set_at(2, 2, r33); - } - - /// Constructs the state transition matrix based on the angular rates. - fn update_state_transition_matrix(&mut self, delta_t: T, angular_rates: &MagnetometerReading) - where - T: MatrixDataType + Default, - { - let rates = *angular_rates * delta_t; - self.filter.state_transition_mut().apply(|mat| { - mat.set_at(0, 0, T::one()); - mat.set_at(0, 1, -rates.z); - mat.set_at(0, 2, rates.y); - - mat.set_at(1, 0, rates.z); - mat.set_at(1, 1, T::one()); - mat.set_at(1, 2, -rates.x); - - mat.set_at(2, 0, -rates.y); - mat.set_at(2, 1, rates.x); - mat.set_at(2, 2, T::one()); - }); - } - - /// Constructs the state transition matrix based on the angular rates. - fn update_control_input(&mut self, delta_t: T, angular_rates: &MagnetometerReading) - where - T: MatrixDataType + Default, - { - let rates = *angular_rates * delta_t; - self.control.control_vector_mut().apply(|vec| { - vec.set_row(0, rates.x); - vec.set_row(1, rates.y); - vec.set_row(2, rates.z); - }); - } - - /// Builds the TRIAD rotation matrix and obtains the Euler angles from it. - /// - /// ## Arguments - /// * `a` - The normalized accelerometer vector. - /// * `m` - The normalized magnetometer vector. - fn build_triad(&self, a: Vector3, m: Vector3) -> EulerAngles - where - T: MatrixDataType - + ArcTan - + ArcSin - + Neg - + DetectGimbalLock, - { - // Calculate TRIAD vectors. - let b1 = a; - let b2 = (m - a * (m * a)).normalized(); - let b3 = b2.cross(b1); - // TRIAD rotation matrix: [b1, b2, b3], stacked columns - - // Derive Euler angles from TRIAD rotation matrix (ZYX sequence). - let theta = -ArcSin::arcsin(b1.z); // pitch - - // Handle Gimbal lock situations. - if theta.close_to_zenith_or_nadir(self.gimbal_lock_tolerance) { - let psi = ArcTan::atan2(-b3.y, b2.y); // yaw - let phi = T::zero(); // roll - EulerAngles::new(phi, theta, psi) - } else { - let psi = ArcTan::atan2(b1.y, b1.x); // yaw - let phi = ArcTan::atan2(b2.z, b3.z); // roll - EulerAngles::new(phi, theta, psi) - } - } -} - -/// Calculates the square of the `value`. -#[inline] -fn sq(value: T) -> T -where - T: core::ops::Mul + Copy, -{ - value * value -} - -impl OwnedOrientationEstimator { - /// Builds the Kalman filter used for prediction. - fn build_filter() -> OwnedKalmanFilter - where - T: MatrixDataType + Default, - { - let zero = T::default(); - - // State vector. - let state_vec = - StateVectorBuffer::::new(MatrixData::new_array::( - [zero; STATES], - )); - - // State transition matrix. - let state_transition = - StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - - // Estimate covariance matrix. - let mut estimate_covariance = - EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - estimate_covariance.make_identity(); - - // Process noise matrix. - let process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( - MatrixData::new_array::( - [zero; { STATES * STATES }], - ), - ); - - // Predicted state vector. - let mut predicted_state = - PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< - STATES, - 1, - STATES, - T, - >([zero; STATES])); - predicted_state.set_all(T::one()); - - // Temporary estimate covariance matrix. - let temp_state_matrix = - TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - - RegularKalmanBuilder::new::( - state_transition, - state_vec, - estimate_covariance, - process_noise, - predicted_state, - temp_state_matrix, - ) - } - - /// Builds the Kalman filter control input. - fn build_control() -> OwnedControlInput - where - T: MatrixDataType + Default, - { - let zero = T::default(); - - // Control vector. - let control_vector = ControlVectorBuffer::::new(MatrixData::new_array::< - CONTROLS, - 1, - CONTROLS, - T, - >([zero; CONTROLS])); - - // Control matrix. - let mut control_matrix = - ControlMatrixMutBuffer::::new(MatrixData::new_array::< - STATES, - CONTROLS, - { STATES * CONTROLS }, - T, - >( - [zero; STATES * CONTROLS] - )); - control_matrix.make_identity(); - - // Process noise matrix. - let process_noise = ControlProcessNoiseCovarianceMatrixBuffer::::new( - MatrixData::new_array::( - [zero; CONTROLS * CONTROLS], - ), - ); - - // Temporary matrix. - let temp = TemporaryBQMatrixBuffer::::new(MatrixData::new_array::< - STATES, - CONTROLS, - { STATES * CONTROLS }, - T, - >( - [zero; STATES * CONTROLS], - )); - - ControlBuilder::new::( - control_matrix, - control_vector, - process_noise, - temp, - ) - } - - /// Builds the Kalman filter observation used for the prediction. - fn build_measurement() -> OwnedObservation - where - T: MatrixDataType + Default, - { - let zero = T::default(); - - // Measurement vector - let measurement = - MeasurementVectorBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - 1, - OBSERVATIONS, - T, - >([zero; OBSERVATIONS])); - - // Observation matrix - let mut observation_matrix = - ObservationMatrixMutBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - STATES, - { OBSERVATIONS * STATES }, - T, - >( - [zero; { OBSERVATIONS * STATES }], - )); - observation_matrix.make_identity(); - - // Measurement noise covariance - let noise_covariance = MeasurementNoiseCovarianceMatrixBuffer::::new( - MatrixData::new_array::( - [zero; { OBSERVATIONS * OBSERVATIONS }], - ), - ); - // TODO: Apply measurement noise! - - // Innovation vector - let innovation_vector = - InnovationVectorBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - 1, - OBSERVATIONS, - T, - >([zero; OBSERVATIONS])); - - // Innovation covariance matrix - let innovation_covariance = - InnovationCovarianceMatrixBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - OBSERVATIONS, - { OBSERVATIONS * OBSERVATIONS }, - T, - >( - [zero; { OBSERVATIONS * OBSERVATIONS }], - )); - - // Kalman Gain matrix - let kalman_gain = - KalmanGainMatrixBuffer::::new(MatrixData::new_array::< - STATES, - OBSERVATIONS, - { STATES * OBSERVATIONS }, - T, - >( - [zero; { STATES * OBSERVATIONS }], - )); - - // Temporary residual covariance inverted matrix - let temp_sinv = TemporaryResidualCovarianceInvertedMatrixBuffer::::new( - MatrixData::new_array::( - [zero; { OBSERVATIONS * OBSERVATIONS }], - ), - ); - - // Temporary H×P matrix - let temp_hp = - TemporaryHPMatrixBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - STATES, - { OBSERVATIONS * STATES }, - T, - >( - [zero; { OBSERVATIONS * STATES }], - )); - - // Temporary P×Hᵀ matrix - let temp_pht = - TemporaryPHTMatrixBuffer::::new(MatrixData::new_array::< - STATES, - OBSERVATIONS, - { STATES * OBSERVATIONS }, - T, - >( - [zero; { STATES * OBSERVATIONS }], - )); - - // Temporary K×(H×P) matrix - let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - - RegularObservationBuilder::new::( - observation_matrix, - measurement, - noise_covariance, - innovation_vector, - innovation_covariance, - kalman_gain, - temp_sinv, - temp_hp, - temp_pht, - temp_khp, - ) - } -} - -type OwnedKalmanFilter = RegularKalman< - STATES, - T, - StateTransitionMatrixMutBuffer< - STATES, - T, - MatrixDataArray, - >, - StateVectorBuffer>, - EstimateCovarianceMatrixBuffer< - STATES, - T, - MatrixDataArray, - >, - DirectProcessNoiseCovarianceMatrixMutBuffer< - STATES, - T, - MatrixDataArray, - >, - PredictedStateEstimateVectorBuffer>, - TemporaryStateMatrixBuffer>, ->; - -type OwnedControlInput = Control< - STATES, - CONTROLS, - T, - ControlMatrixMutBuffer>, - ControlVectorBuffer>, - ControlProcessNoiseCovarianceMatrixBuffer< - CONTROLS, - T, - MatrixDataArray, - >, - TemporaryBQMatrixBuffer< - STATES, - CONTROLS, - T, - MatrixDataArray, - >, ->; - -type OwnedObservation = RegularObservation< - STATES, - OBSERVATIONS, - T, - ObservationMatrixMutBuffer< - OBSERVATIONS, - STATES, - T, - MatrixDataArray, - >, - MeasurementVectorBuffer>, - MeasurementNoiseCovarianceMatrixBuffer< - OBSERVATIONS, - T, - MatrixDataArray, - >, - InnovationVectorBuffer>, - InnovationCovarianceMatrixBuffer< - OBSERVATIONS, - T, - MatrixDataArray, - >, - KalmanGainMatrixBuffer< - STATES, - OBSERVATIONS, - T, - MatrixDataArray, - >, - TemporaryResidualCovarianceInvertedMatrixBuffer< - OBSERVATIONS, - T, - MatrixDataArray, - >, - TemporaryHPMatrixBuffer< - OBSERVATIONS, - STATES, - T, - MatrixDataArray, - >, - TemporaryPHTMatrixBuffer< - STATES, - OBSERVATIONS, - T, - MatrixDataArray, - >, - TemporaryKHPMatrixBuffer>, ->; - -#[cfg(test)] -#[cfg(feature = "std")] -mod tests { - use super::*; - use crate::accelerometer_reading::AccelerometerReading; - - trait Round { - /// Rounds to two decimal places. - fn round2(self) -> T; - - /// Rounds to seven decimal places. - fn round7(self) -> T; - } - - impl Round for f32 { - fn round2(self) -> f32 { - (self * 100.0).round() * 0.01 - } - - fn round7(self) -> f32 { - (self * 1000000.0).round() * 0.000001 - } - } - - #[test] - fn test_state_transition_matrix() { - let gimbal_lock_tolerance = 0.01; - let accelerometer_noise = AccelerometerNoise::new(0.01, 0.02, 0.03); - let magnetometer_noise = MagnetometerNoise::new(0.04, 0.05, 0.06); - let epsilon = 1e-6; - - let mut estimator: OwnedOrientationEstimator = OwnedOrientationEstimator::new( - gimbal_lock_tolerance, - accelerometer_noise, - magnetometer_noise, - epsilon, - ); - - // Set up example error covariances (sigma phi/theta/psi). - estimator.filter.estimate_covariance_mut().make_scalar(0.1); - - // Set up example process noise. - // TODO: Pass as argument - estimator - .filter - .direct_process_noise_mut() - .make_scalar(0.01); - - // Set up example measurement noise. - // TODO: Pass as argument - estimator - .measurement - .measurement_noise_covariance_mut() - .make_scalar(0.01); - - let delta_t: f32 = 0.1; - let angular_rates = MagnetometerReading::new(0.01, 0.02, -0.01); - estimator.predict(delta_t, &angular_rates); - - estimator.filter.state_transition().inspect(|mat| { - assert_eq!(mat.get_at(0, 0), 1.0); - assert_eq!(mat.get_at(0, 1), 0.001); - assert_eq!(mat.get_at(0, 2), 0.002); - - assert_eq!(mat.get_at(1, 0), -0.001); - assert_eq!(mat.get_at(1, 1), 1.0); - assert_eq!(mat.get_at(1, 2), -0.001); - - assert_eq!(mat.get_at(2, 0), -0.002); - assert_eq!(mat.get_at(2, 1), 0.001); - assert_eq!(mat.get_at(2, 2), 1.0); - }); - - estimator.filter.state_vector().inspect(|vec| { - assert_eq!(vec.get_row(0), 0.001); - assert_eq!(vec.get_row(1), 0.002); - assert_eq!(vec.get_row(2), -0.001); - }); - - estimator.filter.estimate_covariance().inspect(|mat| { - assert_eq!(mat.get_at(0, 0).round2(), 0.11); - assert_eq!(mat.get_at(0, 1).round2(), -0.0); - assert_eq!(mat.get_at(0, 2).round2(), 0.0); - - assert_eq!(mat.get_at(1, 0).round2(), -0.0); - assert_eq!(mat.get_at(1, 1).round2(), 0.11); - assert_eq!(mat.get_at(1, 2).round2(), 0.0); - - assert_eq!(mat.get_at(2, 0).round2(), 0.0); - assert_eq!(mat.get_at(2, 1).round2(), 0.0); - assert_eq!(mat.get_at(2, 2).round2(), 0.11); - }); - - // Incorporate accelerometer vector. - let accelerometer = AccelerometerReading::new(0.0, 0.0, -9.81); - let magnetometer = MagnetometerReading::new(0.2, 0.0, 0.4); - - estimator.correct(&accelerometer, &magnetometer); - - estimator.filter.state_vector().inspect(|vec| { - assert_eq!(vec.get_row(0).round7(), 0.0); // replace with approximate tests - assert_eq!(vec.get_row(1).round2(), 1.23); // replace with approximate tests - assert_eq!(vec.get_row(2).round2(), -1.5699999); // replace with approximate tests - }) - } -} diff --git a/src/macros.rs b/src/macros.rs index 0728a5e..d88722d 100644 --- a/src/macros.rs +++ b/src/macros.rs @@ -2,134 +2,5 @@ macro_rules! impl_standard_traits { ($type_name:ident, $type_param:ident) => { impl<$type_param> Copy for $type_name<$type_param> where $type_param: Copy {} - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> $type_name<$type_param> - where - $type_param: Default, - { - /// Constructs a new instance from a slice. - #[allow(unused)] - #[inline] - pub fn from_slice(slice: &[$type_param]) -> &Self { - core::assert_eq!( - slice.len(), - core::mem::size_of::() / core::mem::size_of::<$type_param>() - ); - - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - unsafe { &*(slice.as_ptr() as *const Self) } - } - - /// Constructs a new instance from a mutable slice. - #[allow(unused)] - #[inline] - pub fn from_mut_slice(slice: &mut [$type_param]) -> &mut Self { - core::assert_eq!( - slice.len(), - core::mem::size_of::() / core::mem::size_of::<$type_param>() - ); - - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - unsafe { &mut *(slice.as_mut_ptr() as *mut Self) } - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::convert::AsRef<[$type_param]> for $type_name<$type_param> - where - $type_param: Default, - { - fn as_ref(&self) -> &[$type_param] { - unsafe { - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - core::slice::from_raw_parts( - self as *const _ as *const $type_param, - core::mem::size_of::<$type_name<$type_param>>() - / core::mem::size_of::<$type_param>(), - ) - } - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::convert::AsMut<[$type_param]> for $type_name<$type_param> - where - $type_param: Default, - { - fn as_mut(&mut self) -> &mut [$type_param] { - unsafe { - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - core::slice::from_raw_parts_mut( - self as *mut _ as *mut $type_param, - core::mem::size_of::<$type_name<$type_param>>() - / core::mem::size_of::<$type_param>(), - ) - } - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::ops::Deref for $type_name<$type_param> - where - $type_param: Default, - { - type Target = [$type_param]; - - fn deref(&self) -> &Self::Target { - self.as_ref() - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::ops::DerefMut for $type_name<$type_param> - where - $type_param: Default, - { - fn deref_mut(&mut self) -> &mut Self::Target { - self.as_mut() - } - } - - #[cfg(test)] - paste::paste! { - #[cfg(test)] - mod [] { - #[cfg(feature = "unsafe")] - use super::*; - - #[test] - #[cfg(feature = "unsafe")] - fn test_from_slice() { - const TYPE_SIZE: usize = core::mem::size_of::<$type_name>(); - const NUM_ELEMS: usize = TYPE_SIZE / core::mem::size_of::(); - const ARRAY_SIZE: usize = NUM_ELEMS + 1; - let data = [0; ARRAY_SIZE]; - let state = $type_name::from_slice(&data[..NUM_ELEMS]); - core::assert_eq!(state.len(), NUM_ELEMS); - core::assert!(core::ptr::eq(state.as_ptr(), data.as_ptr())); - } - - #[test] - #[cfg(feature = "unsafe")] - fn test_from_slice_mut() { - const TYPE_SIZE: usize = core::mem::size_of::<$type_name>(); - const NUM_ELEMS: usize = TYPE_SIZE / core::mem::size_of::(); - const ARRAY_SIZE: usize = NUM_ELEMS + 1; - let mut data = [0; ARRAY_SIZE]; - { - let state = $type_name::from_mut_slice(&mut data[..NUM_ELEMS]); - state[0] = 10; - core::assert!(core::ptr::eq(state.as_ptr(), data.as_ptr())); - } - core::assert_eq!(data[0], 10, "expect data to be changed"); - } - } - } }; } diff --git a/src/magnetometer_noise.rs b/src/magnetometer_noise.rs index 529f3c2..ebcfdfb 100644 --- a/src/magnetometer_noise.rs +++ b/src/magnetometer_noise.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct MagnetometerNoise { @@ -19,12 +21,6 @@ impl MagnetometerNoise { pub const fn new(x: T, y: T, z: T) -> Self { Self { x, y, z } } - - /// Returns the length of the [`MagnetometerNoise`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 - } } impl Default for MagnetometerNoise @@ -78,34 +74,6 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for MagnetometerNoise { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for MagnetometerNoise { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } - } -} - impl_standard_traits!(MagnetometerNoise, T); #[cfg(test)] diff --git a/src/magnetometer_reading.rs b/src/magnetometer_reading.rs index cddde56..77ca840 100644 --- a/src/magnetometer_reading.rs +++ b/src/magnetometer_reading.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct MagnetometerReading { @@ -20,10 +22,28 @@ impl MagnetometerReading { Self { x, y, z } } - /// Returns the length of the [`MagnetometerReading`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 + /// Constructs a new [`MagnetometerReading`] instance from a reading in a given coordinate frame. + #[cfg(feature = "coordinate-frame")] + #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] + pub fn north_east_down(coordinate: C) -> Self + where + C: Into>, + T: Clone, + { + let coordinate = coordinate.into(); + Self { + x: coordinate.x(), + y: coordinate.y(), + z: coordinate.z(), + } + } + + /// Represents this reading as a tuple. + pub fn as_tuple(&self) -> (T, T, T) + where + T: Copy, + { + (self.x, self.y, self.z) } } @@ -78,31 +98,15 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for MagnetometerReading { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for MagnetometerReading { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } +#[cfg(feature = "coordinate-frame")] +#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] +impl From for MagnetometerReading +where + C: coordinate_frame::CoordinateFrame, + T: Copy + coordinate_frame::SaturatingNeg, +{ + fn from(value: C) -> Self { + Self::north_east_down(value.to_ned()) } } diff --git a/src/num_traits.rs b/src/num_traits.rs index 8e0a68d..d0018b4 100644 --- a/src/num_traits.rs +++ b/src/num_traits.rs @@ -15,18 +15,95 @@ pub trait DetectGimbalLock: GimbalLockZenithNadir { fn close_to_zenith_or_nadir(&self, tolerance: T) -> bool; } -pub trait ArcTan { +pub trait IsNaN { + /// Determines whether this value represents "not a number" (NaN). + fn is_nan(&self) -> bool; +} + +pub trait NormalizeAngle { + type Output; + + /// Normalizes the angle into the range -π to π. + fn normalize_angle(self) -> Self::Output; +} + +pub trait ArcTan { type Output; fn atan2(self, rhs: T) -> Self::Output; } -pub trait ArcSin { +pub trait ArcSin { type Output; fn arcsin(self) -> Self::Output; } +pub trait Abs { + type Output; + + fn abs(self) -> Self::Output; +} + +impl IsNaN for f32 { + #[inline(always)] + fn is_nan(&self) -> bool { + (*self).is_nan() + } +} + +impl IsNaN for f64 { + #[inline(always)] + fn is_nan(&self) -> bool { + (*self).is_nan() + } +} + +#[cfg_attr(docsrs, doc(cfg(feature = "std")))] +#[cfg(feature = "std")] +impl NormalizeAngle for f32 { + type Output = f32; + + fn normalize_angle(self) -> Self::Output { + let mut normalized = self; + while normalized > std::f32::consts::PI { + normalized -= std::f32::consts::TAU; + } + while normalized < -std::f32::consts::PI { + normalized += std::f32::consts::TAU; + } + normalized + } +} + +#[cfg_attr(docsrs, doc(cfg(feature = "std")))] +#[cfg(feature = "std")] +impl NormalizeAngle for f64 { + type Output = f64; + + fn normalize_angle(self) -> Self::Output { + let mut normalized = self; + while normalized > std::f64::consts::PI { + normalized -= std::f64::consts::TAU; + } + while normalized < -std::f64::consts::PI { + normalized += std::f64::consts::TAU; + } + normalized + } +} + +#[cfg_attr(docsrs, doc(cfg(feature = "std")))] +#[cfg(feature = "std")] +impl Abs for f32 { + type Output = f32; + + #[inline(always)] + fn abs(self) -> Self::Output { + f32::abs(self) + } +} + #[cfg_attr(docsrs, doc(cfg(feature = "std")))] #[cfg(feature = "std")] impl ArcTan for f32 { diff --git a/src/test_estimator.rs b/src/test_estimator.rs new file mode 100644 index 0000000..94154f5 --- /dev/null +++ b/src/test_estimator.rs @@ -0,0 +1,966 @@ +use crate::vector3::Vector3; +use crate::{ + AccelerometerNoise, AccelerometerReading, ArcSin, ArcTan, DetectGimbalLock, EulerAngles, + GyroscopeBias, GyroscopeNoise, GyroscopeReading, IsNaN, MagnetometerNoise, MagnetometerReading, + NormalizeAngle, +}; +use core::ops::Neg; +use minikalman::buffers::types::*; +use minikalman::matrix::MatrixDataType; +use minikalman::prelude::*; +use minikalman::regular::{ + Control, ControlBuilder, RegularKalman, RegularKalmanBuilder, RegularObservation, + RegularObservationBuilder, +}; + +const STATES: usize = 6; // roll rate, pitch rate, yaw rate, as well as gyro bias (drift) terms +const CONTROLS: usize = 3; // roll rate, pitch rate, yaw rate +const OBSERVATIONS: usize = 3; // roll, pitch, yaw + +/// A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator with generic type `T`. +pub struct OwnedOrientationEstimator { + filter: OwnedKalmanFilter, + control: OwnedControlInput, + measurement: OwnedObservation, + /// The angular tolerance, in radians, to detect a Gimbal Lock. + gimbal_lock_tolerance: T, +} + +impl OwnedOrientationEstimator { + /// Initializes a new instance of the [`OwnedOrientationEstimator`] struct. + /// + /// ## Arguments + /// * `gimbal_lock_tolerance` - The angular tolerance, in radians, to detect a Gimbal Lock. + /// * `accelerometer_noise` - The accelerometer noise values (sigma-squared) for each axis. + /// * `gyroscope_noise` - The gyroscope noise values (sigma-squared) for each axis. + /// * `magnetometer_noise` - The magnetometer noise values (sigma-squared) for each axis. + /// * `epsilon` - A small bias term to avoid divisions by zero. Set to e.g. `1e-6`. + pub fn new( + gimbal_lock_tolerance: T, + accelerometer_noise: AccelerometerNoise, + gyroscope_noise: GyroscopeNoise, + gyroscope_bias: GyroscopeBias, + magnetometer_noise: MagnetometerNoise, + epsilon: T, + ) -> Self + where + T: MatrixDataType + Default, + { + let filter = Self::build_filter(&gyroscope_noise, &gyroscope_bias, epsilon); + let control = Self::build_control(&gyroscope_noise, epsilon); + let measurement = Self::build_measurement(&accelerometer_noise, &magnetometer_noise); + + Self { + filter, + control, + measurement, + gimbal_lock_tolerance, + } + } +} + +impl OwnedOrientationEstimator { + /// Obtains the current estimates of the roll, pitch and yaw angles. + pub fn estimated_angles(&self) -> EulerAngles + where + T: Copy, + { + EulerAngles::new(self.roll(), self.pitch(), self.yaw()) + } + + /// Obtains the current estimate of the roll angle φ (phi), in radians. + /// + /// The roll angle is defined as the amount rotation around the y-axis (forward). + pub fn roll(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(0) + } + + /// Obtains the current estimation variance (uncertainty) of the roll angle φ (phi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn roll_variance(&self) -> T + where + T: Copy, + { + self.filter.estimate_covariance().get_at(0, 0) + } + + /// Obtains the current estimate of the bias term of the roll rate, in radians. + pub fn roll_rate_bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(3) + } + + /// Obtains the current estimate of the pitch angle θ (theta), in radians. + /// + /// The pitch angle is defined as the amount rotation around the x-axis (right). + pub fn pitch(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(1) + } + + /// Obtains the current estimation variance (uncertainty) of the pitch angle θ (theta), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn pitch_variance(&self) -> T + where + T: Copy, + { + self.filter.estimate_covariance().get_at(1, 1) + } + + /// Obtains the current estimate of the bias term of the pitch rate, in radians. + pub fn pitch_rate_bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(4) + } + + /// Obtains the current estimate of the yaw angle ψ (psi), in radians. + /// + /// The yaw angle is defined as the amount rotation around the z-axis (up). + pub fn yaw(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(2) + } + + /// Obtains the current estimation variance (uncertainty) of the yaw angle ψ (psi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn yaw_variance(&self) -> T + where + T: Copy, + { + self.filter.estimate_covariance().get_at(2, 2) + } + + /// Obtains the current estimate of the bias term of the yaw rate, in radians. + pub fn yaw_rate_bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(5) + } +} + +impl OwnedOrientationEstimator { + /// Performs a prediction step to obtain new orientation estimates. + /// + /// ## Arguments + /// * `delta_t` - The time step for the prediction. + /// * `angular_rates` - The angular rates measured by the gyroscope. + pub fn predict(&mut self, delta_t: T, angular_rates: &GyroscopeReading) + where + T: MatrixDataType + Default + NormalizeAngle + IsNaN, + { + // Update the Kalman filter components. + self.update_state_transition_matrix(delta_t, angular_rates); + self.update_control_input(delta_t, angular_rates); + + // Perform a regular Kalman Filter prediction step. + self.filter.predict(); + self.panic_if_nan(); + + // Apply the Gyro control input. + self.filter.control(&mut self.control); + self.panic_if_nan(); + + // Normalize the state estimates. + self.normalize_angles(); + } + + /// Performs a correction step using accelerometer and magnetometer readings. + /// + /// ## Arguments + /// * `accelerometer` - The accelerometer reading. + /// * `magnetometer` - The magnetometer reading. + pub fn correct( + &mut self, + accelerometer: &AccelerometerReading, + magnetometer: &MagnetometerReading, + ) where + T: MatrixDataType + + ArcSin + + ArcTan + + NormalizeAngle + + DetectGimbalLock + + core::fmt::Debug + + IsNaN, + { + // Normalize the vectors. + let a = Vector3::from(accelerometer).normalized(); + let m = Vector3::from(magnetometer).normalized(); + + // Obtain the Euler Angles and apply them to the measurement. + let angles = self.build_triad(a, m); + self.measurement.measurement_vector_mut().apply(|vec| { + vec.set_row(0, angles.roll_phi); + vec.set_row(1, angles.pitch_theta); + vec.set_row(2, angles.yaw_psi); + }); + + // Update the measurement noise. + self.update_measurement_noise(a, m); + + // Perform the update step. + self.filter.correct(&mut self.measurement); + self.panic_if_nan(); + + // Normalize the state estimates. + self.normalize_angles(); + } + + // Normalizes the state estimates. + fn normalize_angles(&mut self) + where + T: Copy + NormalizeAngle, + { + self.filter.state_vector_mut().apply(|vec| { + vec.set_row(0, vec.get_row(0).normalize_angle()); + vec.set_row(1, vec.get_row(1).normalize_angle()); + vec.set_row(2, vec.get_row(2).normalize_angle()); + }); + } + + #[allow(unused)] + fn panic_if_nan(&self) + where + T: Copy + IsNaN, + { + #[cfg(debug_assertions)] + self.filter.state_vector().inspect(|vec| { + if vec.get_row(0).is_nan() || vec.get_row(1).is_nan() || vec.get_row(2).is_nan() { + panic!("NaN angle detected in state estimate") + } + }); + } + + /// Computes the Jacobian matrices and measurement noise matrix for orientation estimation + /// + /// This is done by propagating the sensor noise through the TRIAD method + /// The final measurement noise covariance matrix R is computed using J_a, R_a, J_m, and R_m + /// R represents the combined effect of accelerometer and magnetometer noise on the orientation estimates, + /// where J_a and J_m are the Jacobians of the respective measurement noise. + /// + /// ## Arguments + /// * `accelerometer` - The normalized accelerometer vector. + /// * `magnetometer` - The normalized magnetometer vector. + #[allow(unused)] + fn update_measurement_noise(&mut self, accelerometer: Vector3, magnetometer: Vector3) + where + T: MatrixDataType + core::fmt::Debug, + { + /* + // Easy access to accelerometer noise. + let sa11 = self.accelerometer_noise.x; // sigma_a_xx + let sa22 = self.accelerometer_noise.y; // sigma_a_yy + let sa33 = self.accelerometer_noise.z; // sigma_a_zz + + // Easy access to magnetometer noise. + let sm11 = self.magnetometer_noise.x; // sigma_m_xx + let sm22 = self.magnetometer_noise.y; // sigma_m_yy + let sm33 = self.magnetometer_noise.z; // sigma_m_zz + + // Helper "constants". + let two = T::one() + T::one(); + let epsilon = self.epsilon; + + // Easy access to accelerometer components. + let ax = accelerometer.x; + let ay = accelerometer.y; + let az = accelerometer.z; + + // Easy access to magnetometer components. + let mx = magnetometer.x; + let my = magnetometer.y; + let mz = magnetometer.z; + + // Common squares of the accelerometer components. + let ax2 = accelerometer.x * accelerometer.x; + let ay2 = accelerometer.y * accelerometer.y; + let az2 = accelerometer.z * accelerometer.z; + + // Common squares of the magnetometer components. + let mx2 = magnetometer.x * magnetometer.x; + let my2 = magnetometer.y * magnetometer.y; + let mz2 = magnetometer.z * magnetometer.z; + + // Noise matrix. + let r = self.measurement.measurement_noise_covariance_mut(); + + // Common denominator of the matrix components. + #[rustfmt::skip] + let denom = + ax2 * az2 * mx2 + + ax2 * my2 + + two * ax2 * ay * az2 * mx * my + - two * ax * ay * mx * my + + two * ax * az2 * az * mx * mz + - two * ax * az * mx * mz + + ay2 * az2 * my2 + + ay2 * mx2 + + two * ay * az2 * az * my * mz + - two * ay * az * my * mz + + az2 * az2 * mz2 + - two * az2 * mz2 + + mz2; + + let one_minus_az2 = T::one() - az2; + let one_minus_az2_sqrt = one_minus_az2.square_root(); + + // Row 1, column 1. + let r11_nom = sa11 * sq(ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz) + + sa22 * sq(ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) + + sa33 + * sq( + ax2 * mx * my - ax * my * mx2 + ax * ay * my2 + two * ax * az * my * mz + - ay2 * mx * my + - two * ay * az * mx * mz, + ) + + sm11 * sq(ax2 * az * my + ay2 * az * my + ay * az2 * mz - ay * mz) + + sm22 * sq(ax2 * az * mx + ax * az2 * mz - ax * mz + ay2 * az * mx) + + sm33 * sq(ax * az2 * my - ax * my - ay * az2 * mx + ay * mx); + let r11 = r11_nom / (sq(denom) + epsilon); + r.set_at(0, 0, r11); + + // Row 1, column 2 and row 2, column 1. + let r12_nom = sa33 + * (-ax2 * mx * my + ax * ay * mx2 - ax * ay * my2 - two * ax * az * my * mz + + ay2 * mx * my + + two * ay * az * mx * mz); + let r12 = r12_nom / (one_minus_az2_sqrt * denom + epsilon); + r.set_symmetric(0, 1, r12); + + // Row 1, column 3 and row 3, column 1. + let r13_nom = ax * sa22 * (ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) + + ay * sa11 * (ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz); + let r13 = r13_nom / ((ax2 + ay2) * denom + epsilon); + r.set_symmetric(0, 2, r13); + + // Row 2, column 2. + let r22 = -sa33 / (ax2 - T::one() + epsilon); + r.set_at(1, 1, r22); + + // Row 2, column 3 and row 3, column 2. + r.set_symmetric(1, 2, T::zero()); + + // Row 3, column 3. + let r33 = (ax2 * sa22 + ay2 * sa11) / (sq(ax2 + ay2) + epsilon); + r.set_at(2, 2, r33); + */ + } + + /// Constructs the state transition matrix based on the angular rates. + fn update_state_transition_matrix(&mut self, delta_t: T, angular_rates: &GyroscopeReading) + where + T: MatrixDataType + Default, + { + let rates = *angular_rates * delta_t; + let x_bias = self.roll_rate_bias() * delta_t; + let y_bias = self.pitch_rate_bias() * delta_t; + let z_bias = self.yaw_rate_bias() * delta_t; + + self.filter.state_transition_mut().apply(|mat| { + mat.set_at(0, 0, T::one()); + mat.set_at(0, 1, -(rates.omega_z - z_bias)); + mat.set_at(0, 2, rates.omega_y - y_bias); + mat.set_at(0, 3, -delta_t); + mat.set_at(0, 4, T::zero()); + mat.set_at(0, 5, T::zero()); + + mat.set_at(1, 0, rates.omega_z - z_bias); + mat.set_at(1, 1, T::one()); + mat.set_at(1, 2, -(rates.omega_x - x_bias)); + mat.set_at(1, 3, T::zero()); + mat.set_at(1, 4, -delta_t); + mat.set_at(1, 5, T::zero()); + + mat.set_at(2, 0, -(rates.omega_y - y_bias)); + mat.set_at(2, 1, rates.omega_x - x_bias); + mat.set_at(2, 2, T::one()); + mat.set_at(2, 3, T::zero()); + mat.set_at(2, 4, T::zero()); + mat.set_at(2, 5, -delta_t); + + mat.set_at(3, 3, T::one()); + mat.set_at(4, 4, T::one()); + mat.set_at(5, 5, T::one()); + }); + } + + /// Constructs the state transition matrix based on the angular rates. + fn update_control_input(&mut self, delta_t: T, angular_rates: &GyroscopeReading) + where + T: MatrixDataType + Default, + { + self.control.control_matrix_mut().apply(|mat| { + mat.set_at(0, 0, delta_t); + mat.set_at(1, 1, delta_t); + mat.set_at(2, 2, delta_t); + }); + + let rates = *angular_rates * delta_t; + self.control.control_vector_mut().apply(|vec| { + vec.set_row(0, rates.omega_x); + vec.set_row(1, rates.omega_y); + vec.set_row(2, rates.omega_z); + }); + } + + /// Builds the TRIAD rotation matrix and obtains the Euler angles from it. + /// + /// ## Arguments + /// * `a` - The normalized accelerometer vector. + /// * `m` - The normalized magnetometer vector. + fn build_triad(&self, a: Vector3, m: Vector3) -> EulerAngles + where + T: MatrixDataType + + ArcTan + + ArcSin + + Neg + + DetectGimbalLock + + NormalizeAngle, + { + // Define base vectors (i.e., TRIAD). + let z = -a; // up + let _x = m; // forward + let y = m.cross(z).normalized(); // left + let x = y.cross(z).normalized(); // forward + + // TRIAD rotation matrix: [b1, b2, b3], stacked columns + + // Derive Euler angles from TRIAD rotation matrix (Trait-Bryan XYZ order). + let theta = (-ArcSin::arcsin(z.x)).normalize_angle(); // pitch + + // Handle Gimbal lock situations. + #[allow(unreachable_code)] + if theta.close_to_zenith_or_nadir(self.gimbal_lock_tolerance) { + todo!("test this"); + let psi = ArcTan::atan2(-x.y, y.y); // yaw + let phi = T::zero(); // roll + EulerAngles::new(phi.normalize_angle(), theta, psi.normalize_angle()) + } else { + let phi = ArcTan::atan2(z.y, z.z); // roll + let psi = ArcTan::atan2(y.x, x.x); // yaw + EulerAngles::new(phi.normalize_angle(), theta, psi.normalize_angle()) + } + } +} + +impl OwnedOrientationEstimator { + /// Builds the Kalman filter used for prediction. + fn build_filter( + gyroscope_noise: &GyroscopeNoise, + gyroscope_bias: &GyroscopeBias, + process_noise_value: T, + ) -> OwnedKalmanFilter + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // State vector. + let mut state_vec = + StateVectorBuffer::::new(MatrixData::new_array::( + [zero; STATES], + )); + state_vec.set_row(3, gyroscope_bias.omega_x); + state_vec.set_row(4, gyroscope_bias.omega_y); + state_vec.set_row(5, gyroscope_bias.omega_z); + + // State transition matrix. + let state_transition = + StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + // Estimate covariance matrix. + let mut estimate_covariance = + EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + estimate_covariance.apply(|mat| { + mat.set_at(0, 0, gyroscope_noise.x); + // mat.set_at(0, 1, gyroscope_noise.z * gyroscope_noise.x); + // mat.set_at(0, 2, gyroscope_noise.y * gyroscope_noise.x); + + // mat.set_at(1, 0, gyroscope_noise.z * gyroscope_noise.y); + mat.set_at(1, 1, gyroscope_noise.y); + // mat.set_at(1, 2, gyroscope_noise.x * gyroscope_noise.y); + + // mat.set_at(2, 0, gyroscope_noise.y * gyroscope_noise.z); + // mat.set_at(2, 1, gyroscope_noise.x * gyroscope_noise.z); + mat.set_at(2, 2, gyroscope_noise.z); + + mat.set_at(3, 3, gyroscope_noise.x * gyroscope_noise.x); + mat.set_at(4, 4, gyroscope_noise.y * gyroscope_noise.y); + mat.set_at(5, 5, gyroscope_noise.z * gyroscope_noise.z); + }); + + // Process noise matrix. + let mut process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * STATES }], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Predicted state vector. + let mut predicted_state = + PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< + STATES, + 1, + STATES, + T, + >([zero; STATES])); + predicted_state.set_all(T::zero()); + + // Temporary estimate covariance matrix. + let temp_state_matrix = + TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularKalmanBuilder::new::( + state_transition, + state_vec, + estimate_covariance, + process_noise, + predicted_state, + temp_state_matrix, + ) + } + + /// Builds the Kalman filter control input. + fn build_control( + _gyroscope_noise: &GyroscopeNoise, + process_noise_value: T, + ) -> OwnedControlInput + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Control vector. + let control_vector = ControlVectorBuffer::::new(MatrixData::new_array::< + CONTROLS, + 1, + CONTROLS, + T, + >([zero; CONTROLS])); + + // Control matrix. + let mut control_matrix = + ControlMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + CONTROLS, + { STATES * CONTROLS }, + T, + >( + [zero; STATES * CONTROLS] + )); + control_matrix.apply(|mat| { + mat.set_at(0, 0, T::one()); + mat.set_at(1, 1, T::one()); + mat.set_at(2, 2, T::one()); + }); + + // Process noise matrix. + let mut process_noise = ControlProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; CONTROLS * CONTROLS], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Temporary matrix. + let temp = TemporaryBQMatrixBuffer::::new(MatrixData::new_array::< + STATES, + CONTROLS, + { STATES * CONTROLS }, + T, + >( + [zero; STATES * CONTROLS], + )); + + ControlBuilder::new::( + control_matrix, + control_vector, + process_noise, + temp, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_measurement( + accelerometer_noise: &AccelerometerNoise, + magnetometer_noise: &MagnetometerNoise, + ) -> OwnedObservation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + observation_matrix.apply(|mat| { + mat.set_at(0, 0, T::one()); + mat.set_at(1, 1, T::one()); + mat.set_at(2, 2, T::one()); + }); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >([zero; { OBSERVATIONS * OBSERVATIONS }]), + ); + + let noise_covariance_value = accelerometer_noise.x * magnetometer_noise.x + + accelerometer_noise.y * magnetometer_noise.y + + accelerometer_noise.z * magnetometer_noise.z; + noise_covariance.make_scalar(noise_covariance_value); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >( + [zero; { OBSERVATIONS * OBSERVATIONS }], + )); + + // Kalman Gain matrix + let kalman_gain = + KalmanGainMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary residual covariance inverted matrix + let temp_sinv = TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { OBSERVATIONS * OBSERVATIONS }], + ), + ); + + // Temporary H×P matrix + let temp_hp = + TemporaryHPMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + + // Temporary P×Hᵀ matrix + let temp_pht = + TemporaryPHTMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } +} + +type OwnedKalmanFilter = RegularKalman< + STATES, + T, + StateTransitionMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + StateVectorBuffer>, + EstimateCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + >, + DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + PredictedStateEstimateVectorBuffer>, + TemporaryStateMatrixBuffer>, +>; + +type OwnedControlInput = Control< + STATES, + CONTROLS, + T, + ControlMatrixMutBuffer>, + ControlVectorBuffer>, + ControlProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataArray, + >, + TemporaryBQMatrixBuffer< + STATES, + CONTROLS, + T, + MatrixDataArray, + >, +>; + +type OwnedObservation = RegularObservation< + STATES, + OBSERVATIONS, + T, + ObservationMatrixMutBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + MeasurementVectorBuffer>, + MeasurementNoiseCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + InnovationVectorBuffer>, + InnovationCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + KalmanGainMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryResidualCovarianceInvertedMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryHPMatrixBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + TemporaryPHTMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryKHPMatrixBuffer>, +>; + +#[cfg(test)] +#[cfg(feature = "std")] +mod tests { + use super::*; + use crate::accelerometer_reading::AccelerometerReading; + + trait Round { + /// Rounds to two decimal places. + fn round2(self) -> T; + + /// Rounds to seven decimal places. + fn round7(self) -> T; + } + + impl Round for f32 { + fn round2(self) -> f32 { + (self * 100.0).round() * 0.01 + } + + fn round7(self) -> f32 { + (self * 1000000.0).round() * 0.000001 + } + } + + #[test] + fn test_state_transition_matrix() { + let gimbal_lock_tolerance = 0.01; + let accelerometer_noise = AccelerometerNoise::new(0.01, 0.02, 0.03); + let gyroscope_noise = GyroscopeNoise::new(0.04, 0.05, 0.06); + let gyroscope_bias = GyroscopeBias::default(); + let magnetometer_noise = MagnetometerNoise::new(0.07, 0.08, 0.09); + let epsilon = 1e-6; + + let mut estimator: OwnedOrientationEstimator = OwnedOrientationEstimator::new( + gimbal_lock_tolerance, + accelerometer_noise, + gyroscope_noise, + gyroscope_bias, + magnetometer_noise, + epsilon, + ); + + // Set up example error covariances (sigma phi/theta/psi). + estimator.filter.estimate_covariance_mut().make_scalar(0.1); + + // Set up example process noise. + // TODO: Pass as argument + estimator + .filter + .direct_process_noise_mut() + .make_scalar(0.01); + + // Set up example measurement noise. + // TODO: Pass as argument + estimator + .measurement + .measurement_noise_covariance_mut() + .make_scalar(0.01); + + let delta_t: f32 = 0.1; + let angular_rates = GyroscopeReading::new(0.01, 0.02, -0.01); + estimator.predict(delta_t, &angular_rates); + + estimator.filter.state_transition().inspect(|mat| { + assert_eq!(mat.get_at(0, 0), 1.0); + assert_eq!(mat.get_at(0, 1), 0.001); + assert_eq!(mat.get_at(0, 2), 0.002); + + assert_eq!(mat.get_at(1, 0), -0.001); + assert_eq!(mat.get_at(1, 1), 1.0); + assert_eq!(mat.get_at(1, 2), -0.001); + + assert_eq!(mat.get_at(2, 0), -0.002); + assert_eq!(mat.get_at(2, 1), 0.001); + assert_eq!(mat.get_at(2, 2), 1.0); + }); + + estimator.filter.state_vector().inspect(|vec| { + assert_eq!(vec.get_row(0), 0.001); + assert_eq!(vec.get_row(1), 0.002); + assert_eq!(vec.get_row(2), -0.001); + }); + + estimator.filter.estimate_covariance().inspect(|mat| { + assert_eq!(mat.get_at(0, 0).round2(), 0.11); + assert_eq!(mat.get_at(0, 1).round2(), -0.0); + assert_eq!(mat.get_at(0, 2).round2(), 0.0); + + assert_eq!(mat.get_at(1, 0).round2(), -0.0); + assert_eq!(mat.get_at(1, 1).round2(), 0.11); + assert_eq!(mat.get_at(1, 2).round2(), 0.0); + + assert_eq!(mat.get_at(2, 0).round2(), 0.0); + assert_eq!(mat.get_at(2, 1).round2(), 0.0); + assert_eq!(mat.get_at(2, 2).round2(), 0.11); + }); + + // Incorporate accelerometer vector. + let accelerometer = AccelerometerReading::new(0.0, 0.0, -9.81); + let magnetometer = MagnetometerReading::new(0.2, 0.0, 0.4); + + estimator.correct(&accelerometer, &magnetometer); + + estimator.filter.state_vector().inspect(|vec| { + assert_eq!(vec.get_row(0).round7(), 0.0); // replace with approximate tests + assert_eq!(vec.get_row(1).round2(), 1.23); // replace with approximate tests + assert_eq!(vec.get_row(2).round2(), -1.5699999); // replace with approximate tests + }) + } +} diff --git a/src/vector3.rs b/src/vector3.rs index dbeedd7..1194b67 100644 --- a/src/vector3.rs +++ b/src/vector3.rs @@ -1,11 +1,13 @@ use crate::accelerometer_reading::AccelerometerReading; -use crate::impl_standard_traits; use crate::magnetometer_reading::MagnetometerReading; use core::borrow::Borrow; use core::fmt::{Debug, Formatter}; -use core::ops::{Add, Mul, Sub}; +use core::ops::{Add, Mul, Neg, Sub}; use minikalman::matrix::MatrixDataType; +use uniform_array_derive::UniformArray; +/// A three-dimensional vector. +#[derive(UniformArray, Copy)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct Vector3 { @@ -158,6 +160,23 @@ impl From> for Vector3 { } } +/// Implements the unary negation. +impl Neg for Vector3 +where + T: Neg, +{ + type Output = Vector3; + + #[inline] + fn neg(self) -> Self::Output { + Self { + x: -self.x, + y: -self.y, + z: -self.z, + } + } +} + /// Implements the vector dot product. impl Mul> for Vector3 where @@ -220,36 +239,12 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for Vector3 { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for Vector3 { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } +impl From> for (T, T, T) { + fn from(value: Vector3) -> Self { + (value.x, value.y, value.z) } } -impl_standard_traits!(Vector3, T); - #[cfg(test)] mod test { use super::*; diff --git a/tests/data b/tests/data new file mode 160000 index 0000000..de4a6e4 --- /dev/null +++ b/tests/data @@ -0,0 +1 @@ +Subproject commit de4a6e4d462603d2f10d4dfe4fdc070b69f95967