formating pass

Author: jbrodovsky

core/src/earth.rs

@@ -42,9 +42,9 @@
 // Rotations can be handled using nalgebra's Rotation3 type, which can be converted to a DCM using the into() method. The Rotation3 type can be created from
 // Euler angles for the body to local-level frame rotation. The inverse of the Rotation3 type can be used to convert from the local-level frame to the body frame.
 // ----------
+use crate::{wrap_latitude, wrap_to_180};
 use ::nalgebra::{Matrix3, Vector3};
 use ::nav_types::{ECEF, WGS84};
-use crate::{wrap_to_180, wrap_latitude};
 
 // Earth constants (WGS84)
 /// Earth's rotation rate rad/s ($\omega_{ie}$)
@@ -317,8 +317,8 @@ pub fn gravity(latitude: &f64, altitude: &f64) -> f64 {
 /// with the rotational effects of the Earth to calculate the effective gravity vector. This
 /// differs from the gravity scalar in that it includes the centrifugal effects of the Earth's
 /// rotation.
-/// 
-/// *Note:* Local level frame coordintaes are odd and mixed and can be defined as North, East, 
+///
+/// *Note:* Local level frame coordintaes are odd and mixed and can be defined as North, East,
 /// Down (NED) or East, North, Up (ENU). This function uses the ENU convention, thus gravity acts
 /// along the negative Z-axis (downward) in the local-level frame.
 ///
@@ -650,7 +650,6 @@ mod tests {
         assert_approx_eq!(grav[0], 0.0);
         assert_approx_eq!(grav[1], 0.0);
         assert_approx_eq!(grav[2], GP, 1e-2);
-
     }
     #[test]
     fn magnetic_radial_field() {

core/src/filter.rs

@@ -467,7 +467,7 @@ impl UKF {
         let mut cov_bar = DMatrix::<f64>::zeros(self.state_size, self.state_size);
         // Update the covariance through a naive loop
         for (i, sigma_point) in sigma_points.iter().enumerate() {
-          //let sigma_point = &sigma_points[i];
+            //let sigma_point = &sigma_points[i];
             let weight_cov = self.weights_cov[i];
             let diff = sigma_point.to_vector(false) - &mu_bar;
             cov_bar += weight_cov * (&diff * &diff.transpose());
@@ -577,17 +577,16 @@ impl UKF {
         sigma_points
     }
     /// Convert a Vec<SigmaPoint> to a DMatrix<f64>
-    pub fn sigma_points_as_matrix(
-        &self,
-    ) -> DMatrix<f64> {
-        let p = (self.state_size as f64 + self.lambda)  * self.covariance.clone();
+    pub fn sigma_points_as_matrix(&self) -> DMatrix<f64> {
+        let p = (self.state_size as f64 + self.lambda) * self.covariance.clone();
         let sqrt_p = matrix_square_root(&p);
         let mu = self.mean_state.clone();
         let mut pts = DMatrix::<f64>::zeros(self.state_size, 2 * self.state_size + 1);
         pts.column_mut(0).copy_from(&mu);
         for i in 0..sqrt_p.ncols() {
-            pts.column_mut(i+1).copy_from(&(&mu + sqrt_p.column(i)));
-            pts.column_mut(i+1+self.state_size).copy_from(&(&mu - sqrt_p.column(i)));
+            pts.column_mut(i + 1).copy_from(&(&mu + sqrt_p.column(i)));
+            pts.column_mut(i + 1 + self.state_size)
+                .copy_from(&(&mu - sqrt_p.column(i)));
         }
         pts
     }
@@ -921,7 +920,7 @@ mod tests {
         // test forward propagation
         // Test at rest
         let imu_data = IMUData {
-            accel: Vector3::new(0.0, 0.0, earth::gravity(&0.0, &0.0)), 
+            accel: Vector3::new(0.0, 0.0, earth::gravity(&0.0, &0.0)),
             gyro: Vector3::new(0.0, 0.0, 0.0),
         };
         let dt = 1.0;
@@ -1022,7 +1021,7 @@ mod tests {
         assert_approx_eq!(wcs[0], wc_0, 1e-6);
         for i in 1..wms.len() {
             assert_approx_eq!(wms[i], w_i, 1e-6);
-            assert_approx_eq!(wcs[i], w_i, 1e-6);   
+            assert_approx_eq!(wcs[i], w_i, 1e-6);
         }
     }
     #[test]
@@ -1062,7 +1061,7 @@ mod tests {
         );
         let sigma_points = ukf.get_sigma_points();
         assert_eq!(sigma_points.len(), (2 * ukf.state_size) + 1);
-        
+
         let mu = ukf.sigma_points_as_matrix() * ukf.weights_mean;
         println!("mu: {}", mu);
         assert_eq!(mu.nrows(), ukf.state_size);
@@ -1114,9 +1113,10 @@ mod tests {
         );
         let dt = 1.0;
         let imu_data = IMUData::new_from_vec(
-            vec![0.0, 0.0, earth::gravity(&0.0, &0.0)], 
-            vec![0.0, 0.0, 0.0]);
-        
+            vec![0.0, 0.0, earth::gravity(&0.0, &0.0)],
+            vec![0.0, 0.0, 0.0],
+        );
+
         ukf.predict(&imu_data, dt);
         assert!(
             ukf.mean_state.len()
@@ -1171,8 +1171,9 @@ mod tests {
     #[test]
     fn ukf_hover() {
         let imu_data = IMUData::new_from_vec(
-            vec![0.0, 0.0, earth::gravity(&0.0, &0.0)], 
-            vec![0.0, 0.0, 0.0]);
+            vec![0.0, 0.0, earth::gravity(&0.0, &0.0)],
+            vec![0.0, 0.0, 0.0],
+        );
         let position = vec![0.0, 0.0, 0.0];
         let velocity = [0.0, 0.0, 0.0];
         let attitude = [0.0, 0.0, 0.0];
@@ -1209,8 +1210,8 @@ mod tests {
         let measurement_noise = ukf.position_measurement_noise(true);
         let measurement = DVector::from_vec(position.clone());
         //for _i in 0..60 {
-            ukf.predict(&imu_data, dt);
-            //ukf.update(&measurement, &measurement_sigma_points, &measurement_noise);
+        ukf.predict(&imu_data, dt);
+        //ukf.update(&measurement, &measurement_sigma_points, &measurement_noise);
         //}
         assert_approx_eq!(ukf.mean_state[0], position[0], 1e-6);
         assert_approx_eq!(ukf.mean_state[1], position[1], 1e-6);

core/src/sim.rs

@@ -574,7 +574,9 @@ pub fn closed_loop(records: &[TestDataRecord]) -> Vec<NavigationResult> {
     let total: usize = records.len();
     let mut i: usize = 1;
     for record in records.iter().skip(1) {
-        println!("======================================================================================================");
+        println!(
+            "======================================================================================================"
+        );
         i = print_progress(i, total, 10);
         print_ukf(&ukf, record);
         // Calculate time difference from the previous record

core/src/strapdown.rs

@@ -567,12 +567,12 @@ impl StrapdownState {
         let gravity = Vector3::new(
             0.0,
             0.0,
-            earth::gravity(&self.latitude.to_degrees(), &self.altitude)
+            earth::gravity(&self.latitude.to_degrees(), &self.altitude),
         );
         //let accel = &gravity;
         //println!("[StrapdownState::velocity_update] position: [{:?}, {:?}, {:?}]", self.latitude.to_degrees(), self.longitude.to_degrees(), self.altitude);
         //println!("[StrapdownState::velocity_update] gravity vector: [{:?}, {:?}, {:?}] | Gravity scalar {:?}", accel[0], accel[1], accel[2], (accel[0].powi(2) + accel[1].powi(2) + accel[2].powi(2)).sqrt());
-    
+
         velocity + (f - gravity - r * (transport_rate + 2.0 * rotation_rate) * velocity) * dt
     }
 }
@@ -698,12 +698,12 @@ pub fn add(a: f64, b: f64) -> f64 {
     a + b
 }
 /// Wrap latitude to the range -90 to 90 degrees
-/// 
+///
 /// This function is generic and can be used with any type that implements the necessary traits.
-/// This function is useful for ensuring that latitude values remain within the valid range for 
+/// This function is useful for ensuring that latitude values remain within the valid range for
 /// WGS84 coordinates. Keep in mind that the local level frame (NED/ENU) is typically used for
-/// navigation and positioning in middling latitudes. 
-/// 
+/// navigation and positioning in middling latitudes.
+///
 /// # Arguments
 /// * `latitude` - The latitude to be wrapped, which can be of any type that implements the necessary traits.
 /// # Returns
@@ -847,15 +847,15 @@ mod tests {
         assert_eq!(state.velocity_down, 0.0);
         let imu_data = IMUData::new_from_vector(
             Vector3::new(0.0, 0.0, earth::gravity(&0.0, &0.0)), // free fall acceleration in m/s^2
-            Vector3::new(0.0, 0.0, 0.0), // No rotation
+            Vector3::new(0.0, 0.0, 0.0),                        // No rotation
         );
         let dt = 1.0; // Example time step in seconds
         state.forward(&imu_data, dt);
         // After a forward step, the state should still be at rest
         assert_approx_eq!(state.latitude, 0.0, 1e-6);
         assert_approx_eq!(state.longitude, 0.0, 1e-6);
         assert_approx_eq!(state.altitude, 0.0, 0.1);
-        assert_approx_eq!(state.velocity_north, 0.0,1e-3);
+        assert_approx_eq!(state.velocity_north, 0.0, 1e-3);
         assert_approx_eq!(state.velocity_east, 0.0, 1e-3);
         assert_approx_eq!(state.velocity_down, 0.0, 0.1);
         //assert_approx_eq!(state.attitude, Rotation3::identity(), 1e-3);
@@ -869,7 +869,6 @@ mod tests {
         assert_approx_eq!(attitude[(2, 0)], 0.0, 1e-3);
         assert_approx_eq!(attitude[(2, 1)], 0.0, 1e-3);
         assert_approx_eq!(attitude[(2, 2)], 0.0, 1e-3);
-
     }
     #[test]
     fn test_wrap_to_180() {