Skip to content

Commit f3a5664

Browse files
jbrodovskyjbrodovsky
committed
formatting pass
1 parent 689048c commit f3a5664

2 files changed

Lines changed: 76 additions & 120 deletions

File tree

‎core/src/filter.rs‎

Lines changed: 60 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -435,11 +435,11 @@ impl UKF {
435435
/// sigma points. The measurement model is specific to a given implementation of the UKF
436436
/// and must be provided by the user as the model determines the shape and quantities of
437437
/// the measurement vector and the measurement sigma points. Measurement models should be
438-
/// implemented as traits and applied to the UKF as needed.
439-
///
440-
/// This module contains some standard GNSS-aided measurements models (`position_measurement_model`,
441-
/// `velocity_measurement_model`, and `position_and_velocity_measurement_model`) that can be
442-
/// used. See the `sim` module for a canonical example of a GPS-aided INS implementation
438+
/// implemented as traits and applied to the UKF as needed.
439+
///
440+
/// This module contains some standard GNSS-aided measurements models (`position_measurement_model`,
441+
/// `velocity_measurement_model`, and `position_and_velocity_measurement_model`) that can be
442+
/// used. See the `sim` module for a canonical example of a GPS-aided INS implementation
443443
/// that uses these models.
444444
///
445445
/// **Note**: Canonical INS implementations use a position measurement model. Typically,
@@ -688,10 +688,7 @@ pub trait GPS {
688688
///
689689
/// # Returns
690690
/// * A vector of measurement sigma points either (N x 3) or (N x 2) depending on the `with_altitude` flag
691-
fn position_measurement_model(
692-
&self,
693-
with_altitude: bool,
694-
) -> Vec<DVector<f64>>;
691+
fn position_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>>;
695692
/// UKF velocity-based measurement model
696693
///
697694
/// Velocity measurement model. This model is used to update the state based on
@@ -704,10 +701,7 @@ pub trait GPS {
704701
///
705702
/// # Returns
706703
/// * A vector of measurement sigma points either (N x 3) or (N x 2) depending on the `with_altitude` flag
707-
fn velocity_measurement_model(
708-
&self,
709-
with_altitude: bool,
710-
) -> Vec<DVector<f64>>;
704+
fn velocity_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>>;
711705
/// GPS or Position-based measurement model
712706
///
713707
/// GPS-aided INS measurement model for a combined position-velocity measuremet.
@@ -722,53 +716,39 @@ pub trait GPS {
722716
///
723717
/// # Returns
724718
/// * A vector of measurement sigma points either (N x 6) or (N x 4) depending on the `with_altitude` flag
725-
fn position_and_velocity_measurement_model(
726-
&self,
727-
with_altitude: bool,
728-
) -> Vec<DVector<f64>>;
719+
fn position_and_velocity_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>>;
729720
/// Position measurement noise model
730-
fn position_measurement_noise(
731-
&self,
732-
with_altitude: bool,
733-
) -> DMatrix<f64>;
721+
fn position_measurement_noise(&self, with_altitude: bool) -> DMatrix<f64>;
734722
/// Velocity measurement noise model
735-
fn velocity_measurement_noise(
736-
&self,
737-
with_altitude: bool,
738-
) -> DMatrix<f64>;
723+
fn velocity_measurement_noise(&self, with_altitude: bool) -> DMatrix<f64>;
739724
/// Position and velocity measurement noise model
740-
fn position_and_velocity_measurement_noise(
741-
&self,
742-
with_altitude: bool,
743-
) -> DMatrix<f64>;
725+
fn position_and_velocity_measurement_noise(&self, with_altitude: bool) -> DMatrix<f64>;
744726
}
745-
impl GPS for UKF {
746-
fn position_measurement_model(&self,
747-
with_altitude: bool,
748-
) -> Vec<DVector<f64>> {
727+
impl GPS for UKF {
728+
fn position_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>> {
749729
let sigma_points = self.get_sigma_points();
750730
let mut measurement_sigma_points = Vec::<DVector<f64>>::with_capacity(sigma_points.len());
751731
for sigma_point in sigma_points {
752-
let mut measurement_sigma_point = DVector::<f64>::zeros(if with_altitude { 3 } else { 2 });
732+
let mut measurement_sigma_point =
733+
DVector::<f64>::zeros(if with_altitude { 3 } else { 2 });
753734
if with_altitude {
754-
measurement_sigma_point[0] = sigma_point.nav_state.latitude;
735+
measurement_sigma_point[0] = sigma_point.nav_state.latitude;
755736
measurement_sigma_point[1] = sigma_point.nav_state.longitude;
756-
measurement_sigma_point[2] = sigma_point.nav_state.altitude;
737+
measurement_sigma_point[2] = sigma_point.nav_state.altitude;
757738
} else {
758-
measurement_sigma_point[0] = sigma_point.nav_state.latitude;
759-
measurement_sigma_point[1] = sigma_point.nav_state.longitude;
739+
measurement_sigma_point[0] = sigma_point.nav_state.latitude;
740+
measurement_sigma_point[1] = sigma_point.nav_state.longitude;
760741
}
761742
measurement_sigma_points.push(measurement_sigma_point);
762743
}
763744
measurement_sigma_points
764745
}
765-
fn velocity_measurement_model(&self,
766-
with_altitude: bool,
767-
) -> Vec<DVector<f64>> {
746+
fn velocity_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>> {
768747
let sigma_points = self.get_sigma_points();
769748
let mut measurement_sigma_points = Vec::<DVector<f64>>::with_capacity(sigma_points.len());
770749
for sigma_point in sigma_points {
771-
let mut measurement_sigma_point = DVector::<f64>::zeros(if with_altitude { 3 } else { 2 });
750+
let mut measurement_sigma_point =
751+
DVector::<f64>::zeros(if with_altitude { 3 } else { 2 });
772752
if with_altitude {
773753
measurement_sigma_point[0] = sigma_point.nav_state.velocity_north; // Northward
774754
measurement_sigma_point[1] = sigma_point.nav_state.velocity_east; // Eastward
@@ -781,18 +761,16 @@ impl GPS for UKF {
781761
}
782762
measurement_sigma_points
783763
}
784-
fn position_and_velocity_measurement_model(&self,
785-
with_altitude: bool,
786-
) -> Vec<DVector<f64>> {
764+
fn position_and_velocity_measurement_model(&self, with_altitude: bool) -> Vec<DVector<f64>> {
787765
let sigma_points = self.get_sigma_points();
788766
let mut measurement_sigma_points = Vec::<DVector<f64>>::with_capacity(sigma_points.len());
789767
for sigma_point in sigma_points {
790768
if with_altitude {
791769
// Position and velocity with altitude
792770
let measurement_sigma_point = DVector::<f64>::from_vec(vec![
793-
sigma_point.nav_state.latitude, // Latitude
794-
sigma_point.nav_state.longitude, // Longitude
795-
sigma_point.nav_state.altitude, // Altitude
771+
sigma_point.nav_state.latitude, // Latitude
772+
sigma_point.nav_state.longitude, // Longitude
773+
sigma_point.nav_state.altitude, // Altitude
796774
sigma_point.nav_state.velocity_north, // Northward velocity
797775
sigma_point.nav_state.velocity_east, // Eastward velocity
798776
sigma_point.nav_state.velocity_down, // Downward velocity
@@ -801,8 +779,8 @@ impl GPS for UKF {
801779
} else {
802780
// Position and velocity without altitude
803781
let measurement_sigma_point = DVector::<f64>::from_vec(vec![
804-
sigma_point.nav_state.latitude, // Latitude
805-
sigma_point.nav_state.longitude, // Longitude
782+
sigma_point.nav_state.latitude, // Latitude
783+
sigma_point.nav_state.longitude, // Longitude
806784
sigma_point.nav_state.velocity_north, // Northward velocity
807785
sigma_point.nav_state.velocity_east, // Eastward velocity
808786
]);
@@ -815,61 +793,50 @@ impl GPS for UKF {
815793
fn position_measurement_noise(&self, with_altitude: bool) -> DMatrix<f64> {
816794
// Default implementation returns an identity matrix, can be overridden
817795
match with_altitude {
818-
true => DMatrix::from_diagonal(&DVector::from_vec(
819-
vec![
820-
5.0 * METERS_TO_DEGREES,
821-
5.0 * METERS_TO_DEGREES,
822-
5.0 ]
823-
)),
824-
false => DMatrix::from_diagonal(&DVector::from_vec(
825-
vec![
826-
5.0 * METERS_TO_DEGREES,
827-
5.0 * METERS_TO_DEGREES
828-
]
829-
))
796+
true => DMatrix::from_diagonal(&DVector::from_vec(vec![
797+
5.0 * METERS_TO_DEGREES,
798+
5.0 * METERS_TO_DEGREES,
799+
5.0,
800+
])),
801+
false => DMatrix::from_diagonal(&DVector::from_vec(vec![
802+
5.0 * METERS_TO_DEGREES,
803+
5.0 * METERS_TO_DEGREES,
804+
])),
830805
}
831806
}
832807
/// Velocity measurement noise covariance matrix
833808
fn velocity_measurement_noise(&self, with_altitude: bool) -> DMatrix<f64> {
834809
// Default implementation returns an identity matrix, can be overridden
835810
match with_altitude {
836-
true => DMatrix::from_diagonal(&DVector::from_vec(
837-
vec![
838-
0.1, // Northward velocity noise (m/s)
839-
0.1, // Eastward velocity noise (m/s)
840-
0.1, // Downward velocity noise (m/s)
841-
]
842-
)),
843-
false => DMatrix::from_diagonal(&DVector::from_vec(
844-
vec![
845-
0.1, // Northward velocity noise (m/s)
846-
0.1, // Eastward velocity noise (m/s)
847-
]
848-
))
811+
true => DMatrix::from_diagonal(&DVector::from_vec(vec![
812+
0.1, // Northward velocity noise (m/s)
813+
0.1, // Eastward velocity noise (m/s)
814+
0.1, // Downward velocity noise (m/s)
815+
])),
816+
false => DMatrix::from_diagonal(&DVector::from_vec(vec![
817+
0.1, // Northward velocity noise (m/s)
818+
0.1, // Eastward velocity noise (m/s)
819+
])),
849820
}
850821
}
851822
/// Position and velocity measurement noise covariance matrix
852823
fn position_and_velocity_measurement_noise(&self, with_altitude: bool) -> DMatrix<f64> {
853824
// Default implementation returns an identity matrix, can be overridden
854825
match with_altitude {
855-
true => DMatrix::from_diagonal(&DVector::from_vec(
856-
vec![
857-
5.0 * METERS_TO_DEGREES, // Latitude noise (degrees)
858-
5.0 * METERS_TO_DEGREES, // Longitude noise (degrees)
859-
5.0, // Altitude noise (meters)
860-
0.1, // Northward velocity noise (m/s)
861-
0.1, // Eastward velocity noise (m/s)
862-
0.1, // Downward velocity noise (m/s)
863-
]
864-
)),
865-
false => DMatrix::from_diagonal(&DVector::from_vec(
866-
vec![
867-
5.0 * METERS_TO_DEGREES, // Latitude noise (degrees)
868-
5.0 * METERS_TO_DEGREES, // Longitude noise (degrees)
869-
0.1, // Northward velocity noise (m/s)
870-
0.1, // Eastward velocity noise (m/s)
871-
]
872-
))
826+
true => DMatrix::from_diagonal(&DVector::from_vec(vec![
827+
5.0 * METERS_TO_DEGREES, // Latitude noise (degrees)
828+
5.0 * METERS_TO_DEGREES, // Longitude noise (degrees)
829+
5.0, // Altitude noise (meters)
830+
0.1, // Northward velocity noise (m/s)
831+
0.1, // Eastward velocity noise (m/s)
832+
0.1, // Downward velocity noise (m/s)
833+
])),
834+
false => DMatrix::from_diagonal(&DVector::from_vec(vec![
835+
5.0 * METERS_TO_DEGREES, // Latitude noise (degrees)
836+
5.0 * METERS_TO_DEGREES, // Longitude noise (degrees)
837+
0.1, // Northward velocity noise (m/s)
838+
0.1, // Eastward velocity noise (m/s)
839+
])),
873840
}
874841
}
875842
}

‎core/src/sim.rs‎

Lines changed: 16 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ use nalgebra::{DMatrix, DVector};
1616
use serde::{Deserialize, Deserializer, Serialize, Serializer};
1717

1818
use crate::earth::METERS_TO_DEGREES;
19-
use crate::filter::{UKF, GPS, StrapdownParams};
19+
use crate::filter::{GPS, StrapdownParams, UKF};
2020
use crate::{IMUData, StrapdownState};
2121
/// Struct representing a single row of test data from the CSV file.
2222
///
@@ -460,15 +460,12 @@ pub fn dead_reckoning(records: &[TestDataRecord]) -> Vec<NavigationResult> {
460460
true,
461461
);
462462
// Store the initial state and metadata
463-
results.push(NavigationResult::from((
464-
&state,
465-
&records[0].time,
466-
)));
463+
results.push(NavigationResult::from((&state, &records[0].time)));
467464
let mut previous_time = records[0].time.clone();
468465
// Process each subsequent record
469466
for record in records.iter().skip(1) {
470467
// Try to calculate time difference from timestamps, default to 1 second if parsing fails
471-
let current_time = record.time.clone();
468+
let current_time = record.time.clone();
472469
let dt = (current_time - previous_time).as_seconds_f64();
473470
// Create IMU data from the record
474471
let imu_data = IMUData::new_from_vec(
@@ -477,10 +474,7 @@ pub fn dead_reckoning(records: &[TestDataRecord]) -> Vec<NavigationResult> {
477474
);
478475
// Propagate the state forward (replace with stub for now)
479476
state.forward(&imu_data, dt);
480-
results.push(NavigationResult::from((
481-
&state,
482-
&current_time,
483-
)));
477+
results.push(NavigationResult::from((&state, &current_time)));
484478
previous_time = record.time;
485479
}
486480
results
@@ -500,10 +494,7 @@ pub fn closed_loop(records: &[TestDataRecord]) -> Vec<NavigationResult> {
500494
// Initialize the UKF with the first record
501495
let mut ukf = initialize_ukf(records[0].clone(), None, None);
502496
// Set the initial result to the UKF initial state
503-
results.push(NavigationResult::from((
504-
&ukf,
505-
&records[0].time,
506-
)));
497+
results.push(NavigationResult::from((&ukf, &records[0].time)));
507498
let mut previous_timestamp = records[0].time.clone();
508499
// Iterate through the records, updating the UKF with each IMU measurement
509500
let total: usize = records.len();
@@ -602,17 +593,17 @@ pub fn initialize_ukf(
602593
Some(imu_biases) => {
603594
covariance_diagonal.extend(&imu_biases);
604595
imu_biases
605-
},
596+
}
606597
None => {
607598
covariance_diagonal.extend(vec![1e-3; 6]);
608599
vec![0.0; 6] // Default values if not provided
609-
},
600+
}
610601
};
611602
let mut process_noise_diagonal = vec![1e-9; 9];
612603
process_noise_diagonal.extend(vec![1e-3; 6]); // Process noise for imu biases
613604
let process_noise_diagonal = DVector::from_vec(process_noise_diagonal);
614605
//DVector::from_vec(vec![0.0; 15]);
615-
let measurement_noise_diagonal = DVector::from_vec(vec![1e-12; 3]);
606+
let measurement_noise_diagonal = DVector::from_vec(vec![1e-12; 3]);
616607
UKF::new(
617608
ukf_params,
618609
imu_bias,
@@ -621,7 +612,7 @@ pub fn initialize_ukf(
621612
DMatrix::from_diagonal(&process_noise_diagonal),
622613
1e-3, // Use a scalar for measurement noise as expected by UKF::new
623614
2.0,
624-
0.0
615+
0.0,
625616
)
626617
}
627618

@@ -648,12 +639,9 @@ mod tests {
648639
let time_str: String = format!("2023-01-01 00:{:02}:{:02}+00:00", t / 60, t % 60);
649640

650641
records.push(TestDataRecord {
651-
time: DateTime::parse_from_str(
652-
&time_str,
653-
"%Y-%m-%d %H:%M:%S%z",
654-
)
655-
.map(|dt| dt.with_timezone(&Utc))
656-
.unwrap(),
642+
time: DateTime::parse_from_str(&time_str, "%Y-%m-%d %H:%M:%S%z")
643+
.map(|dt| dt.with_timezone(&Utc))
644+
.unwrap(),
657645
bearing_accuracy: 0.0,
658646
speed_accuracy: 0.0,
659647
vertical_accuracy: 0.0,
@@ -780,9 +768,10 @@ mod tests {
780768
&9.0,
781769
Some(vec![1.0, 2.0, 3.0]),
782770
);
783-
let expected_timestamp = chrono::DateTime::parse_from_str("2023-01-01 00:00:00+00:00", "%Y-%m-%d %H:%M:%S%z")
784-
.unwrap()
785-
.with_timezone(&chrono::Utc);
771+
let expected_timestamp =
772+
chrono::DateTime::parse_from_str("2023-01-01 00:00:00+00:00", "%Y-%m-%d %H:%M:%S%z")
773+
.unwrap()
774+
.with_timezone(&chrono::Utc);
786775
assert_eq!(nav.timestamp, expected_timestamp);
787776
assert_eq!(nav.latitude, 1.0);
788777
assert_eq!(nav.covariance.as_ref().unwrap().len(), 3);

0 commit comments

Comments
 (0)