Refactor strapdown equations #57

Author: jbrodovsky

core/src/filter.rs

@@ -16,7 +16,7 @@
 //! measurements in the local level frame (i.e. a GPS fix).
 use crate::earth::METERS_TO_DEGREES;
 use crate::linalg::matrix_square_root;
-use crate::{IMUData, StrapdownState, wrap_to_2pi, wrap_to_360};
+use crate::{IMUData, StrapdownState, forward, wrap_to_2pi, wrap_to_360};
 use nalgebra::{DMatrix, DVector, Rotation3, SVector};
 use rand;
 use rand_distr::{Distribution, Normal};
@@ -146,11 +146,11 @@ impl SigmaPoint {
     /// let imu_data = IMUData::new_from_vec(vec![0.0, 0.0, 0.0], vec![0.0, 0.0, 0.0]);
     /// let dt = 0.1;
     ///
-    /// sigma_point.forward(&imu_data, dt, None);
+    /// sigma_point.forward(imu_data, dt, None);
     /// ```
-    pub fn forward(&mut self, imu_data: &IMUData, dt: f64, noise: Option<Vec<f64>>) {
+    pub fn forward(&mut self, imu_data: IMUData, dt: f64, noise: Option<Vec<f64>>) {
         // Propagate the strapdown state using the strapdown equations
-        self.nav_state.forward(imu_data, dt);
+        self.nav_state = forward(self.nav_state, imu_data, dt);
         let noise_vect = match noise {
             None => return, // UKF mode, does not use noise in propagation
             Some(v) => v,   // Particle filter mode, uses noise in propagation
@@ -465,7 +465,7 @@ impl UKF {
         let mut sigma_points = self.get_sigma_points();
         for sigma_point in &mut sigma_points {
             println!("{}", sigma_point.get_string());
-            sigma_point.forward(imu_data, dt, None);
+            sigma_point.forward(*imu_data, dt, None);
         }
         // Update the mean state as mu_bar
         let mut mu_bar = DVector::<f64>::zeros(self.state_size);
@@ -624,7 +624,7 @@ impl ParticleFilter {
     /// * `imu_data` - The IMU measurements to propagate the particles with.
     pub fn propagate(&mut self, imu_data: &IMUData, dt: f64) {
         for particle in &mut self.particles {
-            particle.forward(imu_data, dt, None);
+            particle.forward(*imu_data, dt, None);
         }
     }
     /// Update the weights of the particles based on a measurement
@@ -907,8 +907,6 @@ impl GPS for UKF {
 /// Tests
 #[cfg(test)]
 mod tests {
-    use std::os::unix::process;
-
     use crate::earth;
 
     use super::*;
@@ -935,7 +933,7 @@ mod tests {
             gyro: Vector3::new(0.0, 0.0, 0.0),
         };
         let dt = 1.0;
-        sigma_point.forward(&imu_data, dt, None);
+        sigma_point.forward(imu_data.clone(), dt, None);
 
         let position = [0.0, 0.0, 0.0];
         let velocity = [0.0, 0.0, 0.0];
@@ -955,7 +953,7 @@ mod tests {
             0.01, 0.01, 0.01, // IMU biases noise
             0.01, 0.01, 0.01, // Measurement bias noise
         ];
-        sigma_point.forward(&imu_data, dt, Some(noise));
+        sigma_point.forward(imu_data.clone(), dt, Some(noise));
         // Check that the state has changed
         assert!(sigma_point.nav_state.latitude != position[0]);
         assert!(sigma_point.nav_state.longitude != position[1]);

core/src/sim.rs

@@ -17,7 +17,7 @@ use serde::{Deserialize, Serialize};
 use crate::earth;
 use crate::earth::METERS_TO_DEGREES;
 use crate::filter::{GPS, StrapdownParams, UKF};
-use crate::{IMUData, StrapdownState};
+use crate::{IMUData, StrapdownState, forward};
 /// Struct representing a single row of test data from the CSV file.
 ///
 /// Fields correspond to columns in the CSV, with appropriate renaming for Rust style.
@@ -103,54 +103,6 @@ impl TestDataRecord {
     /// # Returns
     /// * `Ok(Vec<TestDataRecord>)` if successful.
     /// * `Err` if the file cannot be read or parsed.
-    ///
-    /// # Example
-    ///
-    /// ```
-    /// use strapdown::sim::TestDataRecord;
-    /// use std::path::Path;
-    ///
-    /// let record = TestDataRecord {
-    ///     time: chrono::Utc::now(),
-    ///     bearing_accuracy: 0.1,
-    ///     speed_accuracy: 0.1,
-    ///     vertical_accuracy: 0.1,
-    ///     horizontal_accuracy: 0.1,
-    ///     speed: 1.0,
-    ///     bearing: 90.0,
-    ///     altitude: 100.0,
-    ///     longitude: -122.0,
-    ///     latitude: 37.0,
-    ///     qz: 0.0,
-    ///     qy: 0.0,
-    ///     qx: 0.0,
-    ///     qw: 1.0,
-    ///     roll: 0.0,
-    ///     pitch: 0.0,
-    ///     yaw: 0.0,
-    ///     acc_z: 9.81,
-    ///     acc_y: 0.0,
-    ///     acc_x: 0.0,
-    ///     gyro_z: 0.01,
-    ///     gyro_y: 0.01,
-    ///     gyro_x: 0.01,
-    ///     mag_z: 50.0,
-    ///     mag_y: -30.0,
-    ///     mag_x: -20.0,
-    ///     relative_altitude: 0.0,
-    ///     pressure: 1013.25,
-    ///     grav_z: 9.81,
-    ///     grav_y: 0.0,
-    ///     grav_x: 0.0,
-    /// };
-    /// let records = vec![record];
-    /// TestDataRecord::to_csv(&records, "data.csv")
-    ///    .expect("Failed to write test data to CSV");
-    /// let read_records = TestDataRecord::from_csv("data.csv")
-    ///   .expect("Failed to read test data from CSV");
-    /// // doctest cleanup
-    /// std::fs::remove_file("data.csv").unwrap();
-    /// ```
     pub fn from_csv<P: AsRef<std::path::Path>>(
         path: P,
     ) -> Result<Vec<Self>, Box<dyn std::error::Error>> {
@@ -657,12 +609,12 @@ impl From<(&DateTime<Utc>, &UKF, &IMUData, &f64, &f64, &f64, &f64)> for Navigati
                 .sqrt(),
             ),
             mag_anomaly: earth::magnetic_anomaly(
-                &state[0].to_radians(),
-                &state[1].to_radians(),
-                &state[2],
-                mag_x,
-                mag_y,
-                mag_z,
+                state[0].to_radians(),
+                state[1].to_radians(),
+                state[2],
+                *mag_x,
+                *mag_y,
+                *mag_z,
             ),
         }
     }
@@ -744,12 +696,12 @@ impl
                 .sqrt(),
             ),
             mag_anomaly: earth::magnetic_anomaly(
-                &state.latitude,
-                &state.longitude,
-                &state.altitude,
-                mag_x,
-                mag_y,
-                mag_z,
+                state.latitude,
+                state.longitude,
+                state.altitude,
+                *mag_x,
+                *mag_y,
+                *mag_z,
             ),
         }
     }
@@ -821,7 +773,7 @@ pub fn dead_reckoning(records: &[TestDataRecord]) -> Vec<NavigationResult> {
             vec![record.gyro_x, record.gyro_y, record.gyro_z],
         );
         // Propagate the state forward (replace with stub for now)
-        state.forward(&imu_data, dt);
+        state = forward(state, imu_data, dt);
         results.push(NavigationResult::from((
             &current_time,
             &state,