Use submodule for test data

.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

Cargo.toml

@@ -17,7 +17,7 @@ unsafe = []
 coordinate-frame = ["dep:coordinate-frame"]
 
 [dependencies]
-coordinate-frame = { version = "0.3.2", optional = true, features = ["num-traits", "nalgebra"] }
+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"

examples/simulation.rs

@@ -1,37 +1,33 @@
+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, Rotation3, Scalar, Translation3, UnitQuaternion, Vector3,
-};
+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,
 };
-use serde::de::DeserializeOwned;
-use serde::Deserialize;
-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};
-
-const DISPLAY_REFERENCE: bool = true;
-const DISPLAY_ESTIMATIONS: bool = false;
-const DISPLAY_RAW_ACCEL: bool = true;
-const DISPLAY_RAW_MAG: bool = true;
 
-// const DATASET: &str = "serial-sensors/2024-07-10/stm32f3discovery/stationary";
-// const DATASET: &str = "serial-sensors/2024-07-10/stm32f3discovery/x-forward-rotate-around-up-ccw";
-// const DATASET: &str = "serial-sensors/2024-07-10/stm32f3discovery/x-forward-tilt-top-east";
-const DATASET: &str = "serial-sensors/2024-07-10/stm32f3discovery/x-forward-tilt-nose-up";
+// 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<T> = WestUpNorth<T>;
@@ -304,6 +300,9 @@ fn main() -> Result<(), Box<dyn Error>> {
     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.
@@ -327,6 +326,15 @@ fn main() -> Result<(), Box<dyn Error>> {
                 } 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;
                 }
             }
         }
@@ -425,9 +433,9 @@ fn main() -> Result<(), Box<dyn Error>> {
         }
 
         // Obtain a rotation matrix from the estimated angles.
-        let north = estimator.rotate_vector(marg_orientation::Vector3::new(1.0, 0.0, 0.0));
-        let east = estimator.rotate_vector(marg_orientation::Vector3::new(0.0, 1.0, 0.0));
-        let down = estimator.rotate_vector(marg_orientation::Vector3::new(0.0, 0.0, 1.0));
+        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));
@@ -436,8 +444,7 @@ fn main() -> Result<(), Box<dyn Error>> {
         update_arrow_orientation(&mut arrows, filter_x, filter_y, filter_z);
 
         // Display default coordinate system.
-        #[allow(dead_code)]
-        if DISPLAY_REFERENCE {
+        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);
@@ -448,26 +455,23 @@ fn main() -> Result<(), Box<dyn Error>> {
         }
 
         // Convert estimations.
-        #[allow(dead_code)]
-        if DISPLAY_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.
-        let am = NorthEastDown::new(accel_meas.x, accel_meas.y, accel_meas.z);
-        #[allow(dead_code)]
-        if DISPLAY_RAW_ACCEL {
+        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.
-        let mm = NorthEastDown::new(compass_meas.x, compass_meas.y, compass_meas.z);
-        #[allow(dead_code)]
-        if DISPLAY_RAW_MAG {
+        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));
         }

math/quaternion_jacobian.m

@@ -8,7 +8,7 @@
 % Define the 3D vector
 v = [x; y; z];
 
-% Down vetor.
+% Down vector.
 % v(1) = 0;
 % v(2) = 0;
 % v(3) = 1;

src/gyro_drift/types.rs

@@ -40,21 +40,13 @@ pub type OwnedObservation<T> = RegularObservation<
         T,
         MatrixDataArray<OBSERVATIONS, STATES, { OBSERVATIONS * STATES }, T>,
     >,
-    MeasurementVectorBuffer<
-        OBSERVATIONS,
-        T,
-        MatrixDataArray<OBSERVATIONS, 1, OBSERVATIONS, T>,
-    >,
+    MeasurementVectorBuffer<OBSERVATIONS, T, MatrixDataArray<OBSERVATIONS, 1, OBSERVATIONS, T>>,
     MeasurementNoiseCovarianceMatrixBuffer<
         OBSERVATIONS,
         T,
         MatrixDataArray<OBSERVATIONS, OBSERVATIONS, { OBSERVATIONS * OBSERVATIONS }, T>,
     >,
-    InnovationVectorBuffer<
-        OBSERVATIONS,
-        T,
-        MatrixDataArray<OBSERVATIONS, 1, OBSERVATIONS, T>,
-    >,
+    InnovationVectorBuffer<OBSERVATIONS, T, MatrixDataArray<OBSERVATIONS, 1, OBSERVATIONS, T>>,
     InnovationCovarianceMatrixBuffer<
         OBSERVATIONS,
         T,

src/gyro_free/filter.rs

@@ -182,9 +182,9 @@ impl<T> OwnedOrientationEstimator<T> {
                 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));
+                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.
@@ -201,13 +201,6 @@ impl<T> OwnedOrientationEstimator<T> {
         self.panic_if_nan();
     }
 
-    pub fn rotate_vector(&self, vector: Vector3<T>) -> Vector3<T>
-    where
-        T: MatrixDataType,
-    {
-        Self::rotate_vector_internal(self.filter.state_vector(), vector)
-    }
-
     fn rotate_vector_internal(
         state: &StateVectorBufferOwnedType<STATES, T>,
         vec: Vector3<T>,
@@ -237,6 +230,44 @@ impl<T> OwnedOrientationEstimator<T> {
         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<T>) -> Vector3<T>
+    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<T>) -> Vector3<T>
+    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,

src/gyro_free/types.rs

@@ -48,7 +48,12 @@ pub type OwnedVector3Observation<T> = ExtendedObservation<
     MeasurementNoiseCovarianceMatrixBuffer<
         VEC3_OBSERVATIONS,
         T,
-        MatrixDataArray<VEC3_OBSERVATIONS, VEC3_OBSERVATIONS, { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, T>,
+        MatrixDataArray<
+            VEC3_OBSERVATIONS,
+            VEC3_OBSERVATIONS,
+            { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS },
+            T,
+        >,
     >,
     InnovationVectorBuffer<
         VEC3_OBSERVATIONS,
@@ -69,7 +74,12 @@ pub type OwnedVector3Observation<T> = ExtendedObservation<
     TemporaryResidualCovarianceInvertedMatrixBuffer<
         VEC3_OBSERVATIONS,
         T,
-        MatrixDataArray<VEC3_OBSERVATIONS, VEC3_OBSERVATIONS, { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, T>,
+        MatrixDataArray<
+            VEC3_OBSERVATIONS,
+            VEC3_OBSERVATIONS,
+            { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS },
+            T,
+        >,
     >,
     TemporaryHPMatrixBuffer<
         VEC3_OBSERVATIONS,

src/test_estimator.rs

@@ -4,14 +4,14 @@ use crate::{
     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,
 };
-use core::ops::Neg;
 
 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