Skip to content

Commit c87e0a0

Browse files
jbrodovskyjbrodovsky
authored
Merge pull request #59 from jbrodovsky:joss
Joss
2 parents 6e51d59 + ba1b4d6 commit c87e0a0

4 files changed

Lines changed: 54 additions & 93 deletions

File tree

‎core/src/earth.rs‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -648,14 +648,14 @@ pub fn magnetic_declination(latitude: &f64, longitude: &f64, altitude: &f64) ->
648648
}
649649
/// Calculate the magnetic anomaly at a given location
650650
pub fn magnetic_anomaly(
651-
latitude: &f64,
652-
longitude: &f64,
653-
altitude: &f64,
654-
mag_x: &f64,
655-
mag_y: &f64,
656-
mag_z: &f64,
651+
latitude: f64,
652+
longitude: f64,
653+
altitude: f64,
654+
mag_x: f64,
655+
mag_y: f64,
656+
mag_z: f64,
657657
) -> f64 {
658-
0.0 // Placeholder for magnetic anomaly calculation
658+
0.0 * latitude * longitude * altitude * mag_x * mag_y * mag_z // Placeholder for magnetic anomaly calculation
659659
}
660660
// === Unit tests ===
661661
#[cfg(test)]

‎core/src/filter.rs‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
//! measurements in the local level frame (i.e. a GPS fix).
1717
use crate::earth::METERS_TO_DEGREES;
1818
use crate::linalg::matrix_square_root;
19-
use crate::{IMUData, StrapdownState, wrap_to_2pi, wrap_to_360};
19+
use crate::{IMUData, StrapdownState, forward, wrap_to_2pi, wrap_to_360};
2020
use nalgebra::{DMatrix, DVector, Rotation3, SVector};
2121
use rand;
2222
use rand_distr::{Distribution, Normal};
@@ -146,11 +146,11 @@ impl SigmaPoint {
146146
/// let imu_data = IMUData::new_from_vec(vec![0.0, 0.0, 0.0], vec![0.0, 0.0, 0.0]);
147147
/// let dt = 0.1;
148148
///
149-
/// sigma_point.forward(&imu_data, dt, None);
149+
/// sigma_point.forward(imu_data, dt, None);
150150
/// ```
151-
pub fn forward(&mut self, imu_data: &IMUData, dt: f64, noise: Option<Vec<f64>>) {
151+
pub fn forward(&mut self, imu_data: IMUData, dt: f64, noise: Option<Vec<f64>>) {
152152
// Propagate the strapdown state using the strapdown equations
153-
self.nav_state.forward(imu_data, dt);
153+
self.nav_state = forward(self.nav_state, imu_data, dt);
154154
let noise_vect = match noise {
155155
None => return, // UKF mode, does not use noise in propagation
156156
Some(v) => v, // Particle filter mode, uses noise in propagation
@@ -464,7 +464,8 @@ impl UKF {
464464
// Propagate the strapdown state using the strapdown equations
465465
let mut sigma_points = self.get_sigma_points();
466466
for sigma_point in &mut sigma_points {
467-
sigma_point.forward(imu_data, dt, None);
467+
println!("{}", sigma_point.get_string());
468+
sigma_point.forward(*imu_data, dt, None);
468469
}
469470
// Update the mean state as mu_bar
470471
let mut mu_bar = DVector::<f64>::zeros(self.state_size);
@@ -618,7 +619,7 @@ impl ParticleFilter {
618619
/// * `imu_data` - The IMU measurements to propagate the particles with.
619620
pub fn propagate(&mut self, imu_data: &IMUData, dt: f64) {
620621
for particle in &mut self.particles {
621-
particle.forward(imu_data, dt, None);
622+
particle.forward(*imu_data, dt, None);
622623
}
623624
}
624625
/// Update the weights of the particles based on a measurement
@@ -901,8 +902,6 @@ impl GPS for UKF {
901902
/// Tests
902903
#[cfg(test)]
903904
mod tests {
904-
use std::os::unix::process;
905-
906905
use crate::earth;
907906

908907
use super::*;
@@ -929,7 +928,7 @@ mod tests {
929928
gyro: Vector3::new(0.0, 0.0, 0.0),
930929
};
931930
let dt = 1.0;
932-
sigma_point.forward(&imu_data, dt, None);
931+
sigma_point.forward(imu_data.clone(), dt, None);
933932

934933
let position = [0.0, 0.0, 0.0];
935934
let velocity = [0.0, 0.0, 0.0];
@@ -949,7 +948,7 @@ mod tests {
949948
0.01, 0.01, 0.01, // IMU biases noise
950949
0.01, 0.01, 0.01, // Measurement bias noise
951950
];
952-
sigma_point.forward(&imu_data, dt, Some(noise));
951+
sigma_point.forward(imu_data.clone(), dt, Some(noise));
953952
// Check that the state has changed
954953
assert!(sigma_point.nav_state.latitude != position[0]);
955954
assert!(sigma_point.nav_state.longitude != position[1]);

‎core/src/sim.rs‎

Lines changed: 14 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ use serde::{Deserialize, Serialize};
1717
use crate::earth;
1818
use crate::earth::METERS_TO_DEGREES;
1919
use crate::filter::{GPS, StrapdownParams, UKF};
20-
use crate::{IMUData, StrapdownState};
20+
use crate::{IMUData, StrapdownState, forward};
2121
/// Struct representing a single row of test data from the CSV file.
2222
///
2323
/// Fields correspond to columns in the CSV, with appropriate renaming for Rust style.
@@ -103,54 +103,6 @@ impl TestDataRecord {
103103
/// # Returns
104104
/// * `Ok(Vec<TestDataRecord>)` if successful.
105105
/// * `Err` if the file cannot be read or parsed.
106-
///
107-
/// # Example
108-
///
109-
/// ```
110-
/// use strapdown::sim::TestDataRecord;
111-
/// use std::path::Path;
112-
///
113-
/// let record = TestDataRecord {
114-
/// time: chrono::Utc::now(),
115-
/// bearing_accuracy: 0.1,
116-
/// speed_accuracy: 0.1,
117-
/// vertical_accuracy: 0.1,
118-
/// horizontal_accuracy: 0.1,
119-
/// speed: 1.0,
120-
/// bearing: 90.0,
121-
/// altitude: 100.0,
122-
/// longitude: -122.0,
123-
/// latitude: 37.0,
124-
/// qz: 0.0,
125-
/// qy: 0.0,
126-
/// qx: 0.0,
127-
/// qw: 1.0,
128-
/// roll: 0.0,
129-
/// pitch: 0.0,
130-
/// yaw: 0.0,
131-
/// acc_z: 9.81,
132-
/// acc_y: 0.0,
133-
/// acc_x: 0.0,
134-
/// gyro_z: 0.01,
135-
/// gyro_y: 0.01,
136-
/// gyro_x: 0.01,
137-
/// mag_z: 50.0,
138-
/// mag_y: -30.0,
139-
/// mag_x: -20.0,
140-
/// relative_altitude: 0.0,
141-
/// pressure: 1013.25,
142-
/// grav_z: 9.81,
143-
/// grav_y: 0.0,
144-
/// grav_x: 0.0,
145-
/// };
146-
/// let records = vec![record];
147-
/// TestDataRecord::to_csv(&records, "data.csv")
148-
/// .expect("Failed to write test data to CSV");
149-
/// let read_records = TestDataRecord::from_csv("data.csv")
150-
/// .expect("Failed to read test data from CSV");
151-
/// // doctest cleanup
152-
/// std::fs::remove_file("data.csv").unwrap();
153-
/// ```
154106
pub fn from_csv<P: AsRef<std::path::Path>>(
155107
path: P,
156108
) -> Result<Vec<Self>, Box<dyn std::error::Error>> {
@@ -657,12 +609,12 @@ impl From<(&DateTime<Utc>, &UKF, &IMUData, &f64, &f64, &f64, &f64)> for Navigati
657609
.sqrt(),
658610
),
659611
mag_anomaly: earth::magnetic_anomaly(
660-
&state[0].to_radians(),
661-
&state[1].to_radians(),
662-
&state[2],
663-
mag_x,
664-
mag_y,
665-
mag_z,
612+
state[0].to_radians(),
613+
state[1].to_radians(),
614+
state[2],
615+
*mag_x,
616+
*mag_y,
617+
*mag_z,
666618
),
667619
}
668620
}
@@ -744,12 +696,12 @@ impl
744696
.sqrt(),
745697
),
746698
mag_anomaly: earth::magnetic_anomaly(
747-
&state.latitude,
748-
&state.longitude,
749-
&state.altitude,
750-
mag_x,
751-
mag_y,
752-
mag_z,
699+
state.latitude,
700+
state.longitude,
701+
state.altitude,
702+
*mag_x,
703+
*mag_y,
704+
*mag_z,
753705
),
754706
}
755707
}
@@ -822,7 +774,7 @@ pub fn dead_reckoning(records: &[TestDataRecord]) -> Vec<NavigationResult> {
822774
vec![record.gyro_x, record.gyro_y, record.gyro_z],
823775
);
824776
// Propagate the state forward (replace with stub for now)
825-
state.forward(&imu_data, dt);
777+
state = forward(state, imu_data, dt);
826778
results.push(NavigationResult::from((
827779
&current_time,
828780
&state,

‎core/src/strapdown.rs‎

Lines changed: 24 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -556,13 +556,23 @@ impl StrapdownState {
556556
let rotation_rate: Matrix3<f64> =
557557
earth::vector_to_skew_symmetric(&earth::earth_rate_lla(&self.latitude.to_degrees()));
558558
let r = earth::ecef_to_lla(&self.latitude.to_degrees(), &self.longitude.to_degrees());
559+
// let grav: Vector3<f64> = earth::gravitation(&self.position[0], &self.position[1], &self.position[2]);
559560
let velocity: Vector3<f64> =
560561
Vector3::new(self.velocity_north, self.velocity_east, self.velocity_down);
562+
// let gravity = earth::gravitation(
563+
// &self.latitude.to_degrees(),
564+
// &self.longitude.to_degrees(),
565+
// &self.altitude,
566+
// );
561567
let gravity = Vector3::new(
562568
0.0,
563569
0.0,
564570
earth::gravity(&self.latitude.to_degrees(), &self.altitude),
565571
);
572+
//let accel = &gravity;
573+
//println!("[StrapdownState::velocity_update] position: [{:?}, {:?}, {:?}]", self.latitude.to_degrees(), self.longitude.to_degrees(), self.altitude);
574+
//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());
575+
566576
velocity + (f - gravity - r * (transport_rate + 2.0 * rotation_rate) * velocity) * dt
567577
}
568578
}
@@ -835,7 +845,7 @@ mod tests {
835845
Vector3::new(0.0, 0.0, 0.0), // No rotation
836846
);
837847
let dt = 1.0; // Example time step in seconds
838-
state.forward(&imu_data, dt);
848+
state = forward(state, imu_data, dt);
839849
// After a forward step, the state should still be at rest
840850
assert_approx_eq!(state.latitude, 0.0, 1e-6);
841851
assert_approx_eq!(state.longitude, 0.0, 1e-6);
@@ -866,7 +876,7 @@ mod tests {
866876
assert_approx_eq!(state.attitude.euler_angles().2, 0.1, 1e-6); // Check initial yaw
867877
let gyros = Vector3::new(0.0, 0.0, 0.1); // Gyro data for yawing
868878
let dt = 1.0; // Example time step in seconds
869-
let new_attitude = Rotation3::from_matrix(&state.attitude_update(&gyros, dt));
879+
let new_attitude = Rotation3::from_matrix(&attitude_update(&state, gyros, dt));
870880
// Check if the yaw has changed
871881
let new_yaw = new_attitude.euler_angles().2;
872882
assert_approx_eq!(new_yaw, 0.1 + 0.1, 1e-3); // 0.1 rad initial + 0.1 rad
@@ -882,7 +892,7 @@ mod tests {
882892
assert_approx_eq!(state.attitude.euler_angles().0, 0.1, 1e-6); // Check initial roll
883893
let gyros = Vector3::new(0.10, 0.0, 0.0); // Gyro data for yawing
884894
let dt = 1.0; // Example time step in seconds
885-
let new_attitude = Rotation3::from_matrix(&state.attitude_update(&gyros, dt));
895+
let new_attitude = Rotation3::from_matrix(&attitude_update(&state, gyros, dt));
886896
// Check if the yaw has changed
887897
let new_roll = new_attitude.euler_angles().0;
888898
assert_approx_eq!(new_roll, 0.1 + 0.1, 1e-3); // 0.1 rad initial + 0.1 rad
@@ -898,7 +908,7 @@ mod tests {
898908
assert_approx_eq!(state.attitude.euler_angles().1, 0.1, 1e-6); // Check initial yaw
899909
let gyros = Vector3::new(0.0, 0.1, 0.0); // Gyro data for yawing
900910
let dt = 1.0; // Example time step in seconds
901-
let new_attitude = Rotation3::from_matrix(&state.attitude_update(&gyros, dt));
911+
let new_attitude = Rotation3::from_matrix(&attitude_update(&state, gyros, dt));
902912
// Check if the yaw has changed
903913
let new_pitch = new_attitude.euler_angles().1;
904914
assert_approx_eq!(new_pitch, 0.1 + 0.1, 1e-3); // 0.1 rad initial + 0.1 rad
@@ -967,7 +977,7 @@ mod tests {
967977
earth::gravity(&0.0, &0.0), // Gravity vector in NED
968978
);
969979
let dt = 1.0;
970-
let v_new = state.velocity_update(&f, dt);
980+
let v_new = velocity_update(&state, f, dt);
971981
assert_eq!(v_new[0], 0.0);
972982
assert_eq!(v_new[1], 0.0);
973983
assert_eq!(v_new[2], 0.0);
@@ -978,7 +988,7 @@ mod tests {
978988
let state = StrapdownState::new();
979989
let f = nalgebra::Vector3::new(1.0, 0.0, earth::gravity(&0.0, &0.0)); // 1 m/s^2 north
980990
let dt = 2.0;
981-
let v_new = state.velocity_update(&f, dt);
991+
let v_new = velocity_update(&state, f, dt);
982992
// Should be v = a * dt
983993
assert!((v_new[0] - 2.0).abs() < 1e-6);
984994
assert!((v_new[1]).abs() < 1e-6);
@@ -993,7 +1003,7 @@ mod tests {
9931003
state.velocity_down = 2.0;
9941004
let f = Vector3::from_vec(vec![0.0, 0.0, earth::gravity(&0.0, &0.0)]);
9951005
let dt = 1.0;
996-
let v_new = state.velocity_update(&f, dt);
1006+
let v_new = velocity_update(&state, f, dt);
9971007
assert_approx_eq!(v_new[0], 5.0, 1e-3);
9981008
assert_approx_eq!(v_new[1], -3.0, 1e-3);
9991009
assert_approx_eq!(v_new[2], 2.0, 1e-3);
@@ -1007,7 +1017,7 @@ mod tests {
10071017
state.velocity_down = 0.0;
10081018
let f = Vector3::from_vec(vec![0.0, 0.0, 2.0 * earth::gravity(&0.0, &0.0)]); // Downward acceleration
10091019
let dt = 1.0;
1010-
let v_new = state.velocity_update(&f, dt);
1020+
let v_new = velocity_update(&state, f, dt);
10111021
assert_approx_eq!(v_new[2], earth::gravity(&0.0, &0.0), 1e-3);
10121022
}
10131023
#[test]
@@ -1019,7 +1029,7 @@ mod tests {
10191029
);
10201030
let imu_data = IMUData::new_from_vec(vec![0.0, 0.0, 0.0], vec![0.0, 0.0, 0.1]);
10211031
let dt = 1.0;
1022-
state.forward(&imu_data, dt);
1032+
state = forward(state, imu_data, dt);
10231033
let (_, _, yaw) = state.attitude.euler_angles();
10241034
assert!((yaw - 0.1).abs() < 1e-3);
10251035
}
@@ -1033,7 +1043,7 @@ mod tests {
10331043
);
10341044
let imu_data = IMUData::new_from_vec(vec![0.0, 0.0, 0.0], vec![0.1, 0.0, 0.0]);
10351045
let dt = 1.0;
1036-
state.forward(&imu_data, dt);
1046+
state = forward(state, imu_data, dt);
10371047
let (roll, _, _) = state.attitude.euler_angles();
10381048
assert!((roll - 0.1).abs() < 1e-3);
10391049
}
@@ -1047,7 +1057,7 @@ mod tests {
10471057
);
10481058
let imu_data = IMUData::new_from_vec(vec![0.0, 0.0, 0.0], vec![0.0, 0.1, 0.0]);
10491059
let dt = 1.0;
1050-
state.forward(&imu_data, dt);
1060+
state = forward(state, imu_data, dt);
10511061
let (_, pitch, _) = state.attitude.euler_angles();
10521062
assert!((pitch - 0.1).abs() < 1e-3);
10531063
}
@@ -1061,7 +1071,7 @@ mod tests {
10611071
);
10621072
let imu_data = IMUData::new_from_vec(vec![1.0, 0.0, 0.0], vec![0.0, 0.0, 0.0]);
10631073
let dt = 2.0;
1064-
state.forward(&imu_data, dt);
1074+
state = forward(state, imu_data, dt);
10651075
assert!((state.velocity_north - 2.0).abs() < 1e-3);
10661076
}
10671077

@@ -1074,7 +1084,7 @@ mod tests {
10741084
);
10751085
let imu_data = IMUData::new_from_vec(vec![0.0, 1.0, 0.0], vec![0.0, 0.0, 0.0]);
10761086
let dt = 2.0;
1077-
state.forward(&imu_data, dt);
1087+
state = forward(state, imu_data, dt);
10781088
assert!((state.velocity_east - 2.0).abs() < 1e-3);
10791089
}
10801090

@@ -1090,7 +1100,7 @@ mod tests {
10901100
vec![0.0, 0.0, 0.0],
10911101
);
10921102
let dt = 1.0;
1093-
state.forward(&imu_data, dt);
1103+
state = forward(state, imu_data, dt);
10941104
assert_approx_eq!(state.velocity_down, 2.0, 1e-3);
10951105
}
10961106
}

0 commit comments

Comments
 (0)