Skip to content

Commit 9a6b615

Browse files
jbrodovskyjbrodovsky
committed
cargo fmt pass
1 parent 21aad98 commit 9a6b615

5 files changed

Lines changed: 93 additions & 73 deletions

File tree

‎core/src/filter.rs‎

Lines changed: 27 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
use crate::earth::METERS_TO_DEGREES;
1818
use crate::linalg::matrix_square_root;
1919
use crate::{IMUData, StrapdownState, wrap_to_2pi, wrap_to_360};
20-
use nalgebra::{DMatrix, DVector, SVector, Rotation3, Vector3};
20+
use nalgebra::{DMatrix, DVector, Rotation3, SVector, Vector3};
2121
use rand;
2222
use rand_distr::{Distribution, Normal};
2323
use std::fmt::Debug;
@@ -163,7 +163,7 @@ impl SigmaPoint {
163163
// Propagate the strapdown state using the strapdown equations
164164
self.nav_state.forward(&imu_data, dt);
165165
let noise_vect = match noise {
166-
None => return, // UKF mode, does not use noise in propagation
166+
None => return, // UKF mode, does not use noise in propagation
167167
Some(v) => v, // Particle filter mode, uses noise in propagation
168168
};
169169
let mut rng = rand::rng();
@@ -261,11 +261,23 @@ impl SigmaPoint {
261261
velocity_east: state[4],
262262
velocity_down: state[5],
263263
attitude: Rotation3::from_euler_angles(
264-
if in_degrees { wrap_to_2pi(state[6].to_radians()) } else { wrap_to_360(state[6]) },
265-
if in_degrees { wrap_to_2pi(state[7].to_radians()) } else { wrap_to_360(state[7]) },
266-
if in_degrees { wrap_to_2pi(state[8].to_radians()) } else { wrap_to_360(state[8]) },
264+
if in_degrees {
265+
wrap_to_2pi(state[6].to_radians())
266+
} else {
267+
wrap_to_360(state[6])
268+
},
269+
if in_degrees {
270+
wrap_to_2pi(state[7].to_radians())
271+
} else {
272+
wrap_to_360(state[7])
273+
},
274+
if in_degrees {
275+
wrap_to_2pi(state[8].to_radians())
276+
} else {
277+
wrap_to_360(state[8])
278+
},
267279
),
268-
coordinate_convention: true
280+
coordinate_convention: true,
269281
};
270282
SigmaPoint::new(nav_state, other_states, weight)
271283
}
@@ -506,7 +518,7 @@ impl UKF {
506518
);
507519
// Print the measurement sigma points recieved
508520
// for sigma in measurement_sigma_points.iter() {
509-
// println!("[UKF::update] measurement sigma point: [{:.4}, {:.4}, {:.2}]",
521+
// println!("[UKF::update] measurement sigma point: [{:.4}, {:.4}, {:.2}]",
510522
// sigma[0].to_degrees(), sigma[1].to_degrees(), sigma[2]);
511523
// }
512524
// Calculate expected measurement
@@ -777,7 +789,7 @@ pub trait GPS {
777789
}
778790
impl GPS for UKF {
779791
fn position_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>> {
780-
let sigma_points = self.get_sigma_points(); // Crashing issue occurs here
792+
let sigma_points = self.get_sigma_points(); // Crashing issue occurs here
781793
let mut measurement_sigma_points = Vec::<DVector<f64>>::with_capacity(sigma_points.len());
782794
for sigma_point in sigma_points {
783795
let mut measurement_sigma_point =
@@ -847,8 +859,7 @@ impl GPS for UKF {
847859
true => DMatrix::from_diagonal(&DVector::from_vec(vec![
848860
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Latitude noise (degrees)
849861
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Longitude noise (degrees)
850-
(5.0_f64).powf(2.0), // Altitude noise (meters)
851-
862+
(5.0_f64).powf(2.0), // Altitude noise (meters)
852863
])),
853864
false => DMatrix::from_diagonal(&DVector::from_vec(vec![
854865
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Latitude noise (degrees)
@@ -878,16 +889,16 @@ impl GPS for UKF {
878889
true => DMatrix::from_diagonal(&DVector::from_vec(vec![
879890
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Latitude noise (degrees)
880891
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Longitude noise (degrees)
881-
(5.0_f64).powf(2.0), // Altitude noise (meters)
882-
(0.1_f64).powf(2.0), // Northward velocity noise (m/s)
883-
(0.1_f64).powf(2.0), // Eastward velocity noise (m/s)
884-
(0.1_f64).powf(2.0), // Downward velocity noise (m/s)
892+
(5.0_f64).powf(2.0), // Altitude noise (meters)
893+
(0.1_f64).powf(2.0), // Northward velocity noise (m/s)
894+
(0.1_f64).powf(2.0), // Eastward velocity noise (m/s)
895+
(0.1_f64).powf(2.0), // Downward velocity noise (m/s)
885896
])),
886897
false => DMatrix::from_diagonal(&DVector::from_vec(vec![
887898
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Latitude noise (degrees)
888899
((5.0 * METERS_TO_DEGREES).to_radians()).powf(2.0), // Longitude noise (degrees)
889-
(0.1_f64).powf(2.0), // Northward velocity noise (m/s)
890-
(0.1_f64).powf(2.0), // Eastward velocity noise (m/s)
900+
(0.1_f64).powf(2.0), // Northward velocity noise (m/s)
901+
(0.1_f64).powf(2.0), // Eastward velocity noise (m/s)
891902
])),
892903
}
893904
}

‎core/src/linalg.rs‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,10 @@ pub fn matrix_square_root(matrix: &DMatrix<f64>) -> DMatrix<f64> {
5252
match eigenvalue_pass(matrix) {
5353
Some(eigen_sqrt) => eigen_sqrt,
5454
None => {
55-
panic!("Cholesky and Eigenvalue decomposition failed. No valid square root found for the covariance matrix: \n {:?}", matrix);
55+
panic!(
56+
"Cholesky and Eigenvalue decomposition failed. No valid square root found for the covariance matrix: \n {:?}",
57+
matrix
58+
);
5659
}
5760
}
5861
}

‎core/src/main.rs‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,11 @@ fn main() -> Result<(), Box<dyn Error>> {
4242
return Err("Invalid mode specified. Use 'open-loop' or 'closed-loop'.".into());
4343
}
4444
let records: Vec<TestDataRecord> = rdr.deserialize().collect::<Result<_, _>>()?;
45-
println!("Read {} records from {}", records.len(), &args.input.display());
45+
println!(
46+
"Read {} records from {}",
47+
records.len(),
48+
&args.input.display()
49+
);
4650
let results: Vec<NavigationResult>;
4751
if args.mode == "closed-loop" {
4852
results = closed_loop(&records);

‎core/src/sim.rs‎

Lines changed: 56 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -560,7 +560,7 @@ pub fn closed_loop(records: &[TestDataRecord]) -> Vec<NavigationResult> {
560560
let mut results: Vec<NavigationResult> = Vec::with_capacity(records.len());
561561
// Initialize the UKF with the first record
562562
let mut ukf = initialize_ukf(
563-
records[0].clone(),
563+
records[0].clone(),
564564
None, //Some(vec![1e-9; 3]), // attitude covariance
565565
None, //Some(vec![1e-6; 6])
566566
);
@@ -632,55 +632,60 @@ pub fn closed_loop(records: &[TestDataRecord]) -> Vec<NavigationResult> {
632632
results
633633
}
634634
pub fn print_ukf(ukf: &UKF, record: &TestDataRecord) {
635-
println!("\rUKF position: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4e}, {:.4e}, {:.4} | Error: {:.4e}, {:.4e}, {:.4}",
636-
ukf.get_mean()[0].to_degrees(),
637-
ukf.get_mean()[1].to_degrees(),
638-
ukf.get_mean()[2],
639-
ukf.get_covariance()[(0,0)],
640-
ukf.get_covariance()[(1,1)],
641-
ukf.get_covariance()[(2,2)],
642-
ukf.get_mean()[0].to_degrees() - record.latitude,
643-
ukf.get_mean()[1].to_degrees() - record.longitude,
644-
ukf.get_mean()[2] - record.altitude
645-
);
646-
println!("\rUKF velocity: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4}, {:.4}, {:.4} | Error: {:.4}, {:.4}, {:.4}",
647-
ukf.get_mean()[3],
648-
ukf.get_mean()[4],
649-
ukf.get_mean()[5],
650-
ukf.get_covariance()[(3,3)],
651-
ukf.get_covariance()[(4,4)],
652-
ukf.get_covariance()[(5,5)],
653-
ukf.get_mean()[3] - record.speed * record.bearing.cos(),
654-
ukf.get_mean()[4] - record.speed * record.bearing.sin(),
655-
ukf.get_mean()[5] - 0.0 // Assuming no vertical velocity
656-
);
657-
println!("\rUKF attitude: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4}, {:.4}, {:.4} | Error: {:.4}, {:.4}, {:.4}",
658-
ukf.get_mean()[6],
659-
ukf.get_mean()[7],
660-
ukf.get_mean()[8],
661-
ukf.get_covariance()[(6,6)],
662-
ukf.get_covariance()[(7,7)],
663-
ukf.get_covariance()[(8,8)],
664-
ukf.get_mean()[6] - record.roll,
665-
ukf.get_mean()[7] - record.pitch,
666-
ukf.get_mean()[8] - record.yaw
667-
);
668-
println!("\rUKF accel biases: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4e}, {:.4e}, {:.4e}",
669-
ukf.get_mean()[9],
670-
ukf.get_mean()[10],
671-
ukf.get_mean()[11],
672-
ukf.get_covariance()[(9,9)],
673-
ukf.get_covariance()[(10,10)],
674-
ukf.get_covariance()[(11,11)]
675-
);
676-
println!("\rUKF gyro biases: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4e}, {:.4e}, {:.4e}",
677-
ukf.get_mean()[12],
678-
ukf.get_mean()[13],
679-
ukf.get_mean()[14],
680-
ukf.get_covariance()[(12,12)],
681-
ukf.get_covariance()[(13,13)],
682-
ukf.get_covariance()[(14,14)]
683-
);
635+
println!(
636+
"\rUKF position: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4e}, {:.4e}, {:.4} | Error: {:.4e}, {:.4e}, {:.4}",
637+
ukf.get_mean()[0].to_degrees(),
638+
ukf.get_mean()[1].to_degrees(),
639+
ukf.get_mean()[2],
640+
ukf.get_covariance()[(0, 0)],
641+
ukf.get_covariance()[(1, 1)],
642+
ukf.get_covariance()[(2, 2)],
643+
ukf.get_mean()[0].to_degrees() - record.latitude,
644+
ukf.get_mean()[1].to_degrees() - record.longitude,
645+
ukf.get_mean()[2] - record.altitude
646+
);
647+
println!(
648+
"\rUKF velocity: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4}, {:.4}, {:.4} | Error: {:.4}, {:.4}, {:.4}",
649+
ukf.get_mean()[3],
650+
ukf.get_mean()[4],
651+
ukf.get_mean()[5],
652+
ukf.get_covariance()[(3, 3)],
653+
ukf.get_covariance()[(4, 4)],
654+
ukf.get_covariance()[(5, 5)],
655+
ukf.get_mean()[3] - record.speed * record.bearing.cos(),
656+
ukf.get_mean()[4] - record.speed * record.bearing.sin(),
657+
ukf.get_mean()[5] - 0.0 // Assuming no vertical velocity
658+
);
659+
println!(
660+
"\rUKF attitude: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4}, {:.4}, {:.4} | Error: {:.4}, {:.4}, {:.4}",
661+
ukf.get_mean()[6],
662+
ukf.get_mean()[7],
663+
ukf.get_mean()[8],
664+
ukf.get_covariance()[(6, 6)],
665+
ukf.get_covariance()[(7, 7)],
666+
ukf.get_covariance()[(8, 8)],
667+
ukf.get_mean()[6] - record.roll,
668+
ukf.get_mean()[7] - record.pitch,
669+
ukf.get_mean()[8] - record.yaw
670+
);
671+
println!(
672+
"\rUKF accel biases: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4e}, {:.4e}, {:.4e}",
673+
ukf.get_mean()[9],
674+
ukf.get_mean()[10],
675+
ukf.get_mean()[11],
676+
ukf.get_covariance()[(9, 9)],
677+
ukf.get_covariance()[(10, 10)],
678+
ukf.get_covariance()[(11, 11)]
679+
);
680+
println!(
681+
"\rUKF gyro biases: ({:.4}, {:.4}, {:.4}) | Covariance: {:.4e}, {:.4e}, {:.4e}",
682+
ukf.get_mean()[12],
683+
ukf.get_mean()[13],
684+
ukf.get_mean()[14],
685+
ukf.get_covariance()[(12, 12)],
686+
ukf.get_covariance()[(13, 13)],
687+
ukf.get_covariance()[(14, 14)]
688+
);
684689
}
685690

686691
/// Helper function to initialize a UKF for closed-loop mode.
@@ -716,7 +721,7 @@ pub fn initialize_ukf(
716721
in_degrees: true,
717722
};
718723
// Covariance parameters
719-
let position_accuracy = initial_pose.horizontal_accuracy;//.sqrt();
724+
let position_accuracy = initial_pose.horizontal_accuracy; //.sqrt();
720725
let mut covariance_diagonal = vec![
721726
(position_accuracy * METERS_TO_DEGREES).powf(2.0),
722727
(position_accuracy * METERS_TO_DEGREES).powf(2.0),

‎core/src/strapdown.rs‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -555,10 +555,7 @@ impl StrapdownState {
555555
));
556556
let rotation_rate: Matrix3<f64> =
557557
earth::vector_to_skew_symmetric(&earth::earth_rate_lla(&self.latitude.to_degrees()));
558-
let r = earth::ecef_to_lla(
559-
&self.latitude.to_degrees(),
560-
&self.longitude.to_degrees()
561-
);
558+
let r = earth::ecef_to_lla(&self.latitude.to_degrees(), &self.longitude.to_degrees());
562559
// let grav: Vector3<f64> = earth::gravitation(&self.position[0], &self.position[1], &self.position[2]);
563560
let velocity: Vector3<f64> =
564561
Vector3::new(self.velocity_north, self.velocity_east, self.velocity_down);

0 commit comments

Comments
 (0)