diff --git a/src/bin/sailtrack-kalman.rs b/src/bin/sailtrack-kalman.rs index 5783efa..2a7bf2e 100644 --- a/src/bin/sailtrack-kalman.rs +++ b/src/bin/sailtrack-kalman.rs @@ -1,5 +1,6 @@ use eskf::ESKF; use log::{debug, info}; +use map_3d::{geodetic2ned, ned2geodetic, Ellipsoid}; use nalgebra::{Matrix3, Point3, Rotation3, Vector3}; use rumqttc::Event::Incoming; use rumqttc::Packet::Publish; @@ -8,7 +9,6 @@ use serde::{Deserialize, Serialize}; use std::sync::{Arc, RwLock}; use std::thread::{sleep, spawn}; use std::time::{Duration, Instant}; -use map_3d::{Ellipsoid, geodetic2ned, ned2geodetic}; // Connection parameters const MQTT_PUBLISH_FREQ_HZ: u64 = 5; @@ -49,7 +49,11 @@ struct Imu { impl Default for Imu { fn default() -> Imu { Imu { - euler: Euler { x: 0.0, y: 0.0, z: 0.0 }, + euler: Euler { + x: 0.0, + y: 0.0, + z: 0.0, + }, linear_accel: LinearAccel { x: 0.0, y: 0.0, @@ -57,7 +61,6 @@ impl Default for Imu { }, } } - } #[derive(Serialize, Deserialize, Clone, Copy, Debug)] @@ -119,7 +122,7 @@ struct Boat { struct BoatInfo { filter: ESKF, ref_pos: Gps, - orientation: Orientation + orientation: Orientation, } fn angle_wrap_180(angle: f32) -> f32 { @@ -166,16 +169,17 @@ fn main() { let gps_ref = boat_info.ref_pos; let filter = boat_info.filter; let orientation = boat_info.orientation; - drop (boat_info); + drop(boat_info); // Get Boat Metrics let position = filter.position; let velocity = filter.velocity; // From IMU to Geodetic reference frame transformation - let rotation_mtx = Rotation3::from_euler_angles(0.0, 0.0, -orientation.heading.to_radians()); + let rotation_mtx = + Rotation3::from_euler_angles(0.0, 0.0, -orientation.heading.to_radians()); let neu_pos = rotation_mtx.transform_point(&position); - + let n: f64 = neu_pos.x as f64; let e: f64 = neu_pos.y as f64; let d: f64 = -neu_pos.z as f64; @@ -243,23 +247,22 @@ fn main() { if message.topic == "sensor/imu0" { let input: Imu = serde_json::from_slice(&message.payload).unwrap(); - let acceleration = Vector3::new(input.linear_accel.x, input.linear_accel.y, input.linear_accel.z); + let acceleration = Vector3::new( + input.linear_accel.x, + input.linear_accel.y, + input.linear_accel.z, + ); orientation.roll = input.euler.x; - orientation.pitch = - input.euler.y; + orientation.pitch = -input.euler.y; orientation.heading = 360.0 - input.euler.z; let rotation = Vector3::new(input.euler.x, input.euler.y, input.euler.z); let elapsed = delta.elapsed(); info!("Received IMU measurement: {input:?}. Updating filter prediction (delta={}ms)...", elapsed.as_millis()); - filter.predict( - acceleration, - rotation, - elapsed, - ); + filter.predict(acceleration, rotation, elapsed); let mut boat_info = boat_info_mutex.write().unwrap(); boat_info.filter = filter; boat_info.orientation = orientation; delta = Instant::now(); - } else if message.topic == "sensor/gps0" { let gps_data: Gps = serde_json::from_slice(&message.payload).unwrap(); info!("Received GPS measurement: {gps_data:?}. Updating filter observation..."); @@ -271,16 +274,16 @@ fn main() { continue; } // Measure Unit Conversions - let lat:f64 = (gps_data.lat * f32::powf(10.0, -7.0)).to_radians() as f64; - let lon:f64 = (gps_data.lon * f32::powf(10.0, -7.0)).to_radians() as f64; - let alt:f64 = (gps_data.h_msl * f32::powf(10.0, -3.0)) as f64; + let lat: f64 = (gps_data.lat * f32::powf(10.0, -7.0)).to_radians() as f64; + let lon: f64 = (gps_data.lon * f32::powf(10.0, -7.0)).to_radians() as f64; + let alt: f64 = (gps_data.h_msl * f32::powf(10.0, -3.0)) as f64; let vel_n = gps_data.vel_n * f32::powf(10.0, -3.0); let vel_e = gps_data.vel_e * f32::powf(10.0, -3.0); - let vel_u = - gps_data.vel_d * f32::powf(10.0, -3.0); + let vel_u = -gps_data.vel_d * f32::powf(10.0, -3.0); - let lat0:f64 = (ref_pos.lat * f32::powf(10.0, -7.0)).to_radians() as f64; - let lon0:f64 = (ref_pos.lon * f32::powf(10.0, -7.0)).to_radians() as f64; - let alt0:f64 = (ref_pos.h_msl * f32::powf(10.0, -3.0)) as f64; + let lat0: f64 = (ref_pos.lat * f32::powf(10.0, -7.0)).to_radians() as f64; + let lon0: f64 = (ref_pos.lon * f32::powf(10.0, -7.0)).to_radians() as f64; + let alt0: f64 = (ref_pos.h_msl * f32::powf(10.0, -3.0)) as f64; let h_acc = gps_data.h_acc * f32::powf(10.0, -3.0); let v_acc = gps_data.v_acc * f32::powf(10.0, -3.0); @@ -289,37 +292,36 @@ fn main() { // GPS Data To Measure Conversions let (n, e, d) = geodetic2ned(lat, lon, alt, lat0, lon0, alt0, Ellipsoid::default()); let position = Point3::new(n as f32, e as f32, -d as f32); - let mut orizontal_std = 0.5 * h_acc/f32::sqrt(2.0); + let mut orizontal_std = 0.5 * h_acc / f32::sqrt(2.0); let mut vertical_std = 0.5 * v_acc; let mut speed_std = 0.5 * s_acc; if gps_data.fix_type != 3 { orizontal_std *= 2.0; vertical_std *= 2.0; - speed_std *= 2.0; + speed_std *= 2.0; } - let pos_var = Matrix3::from_diagonal(&Vector3::new(orizontal_std.powi(2), orizontal_std.powi(2), vertical_std.powi(2))); + let pos_var = Matrix3::from_diagonal(&Vector3::new( + orizontal_std.powi(2), + orizontal_std.powi(2), + vertical_std.powi(2), + )); let velocity = Vector3::new(vel_n, vel_e, vel_u); let vel_variance = speed_std.powi(2) * Matrix3::identity(); // Rotation to IMU Reference Frame - let rotation = Rotation3::from_euler_angles(0.0, 0.0, orientation.heading.to_radians()); + let rotation = + Rotation3::from_euler_angles(0.0, 0.0, orientation.heading.to_radians()); let rot_position = rotation.transform_point(&position); let rot_pos_var = rotation * pos_var * rotation.transpose(); let rot_velocity = rotation.transform_vector(&velocity); let rot_vel_variance = rotation * vel_variance * rotation.transpose(); - filter - .observe_position( - rot_position, - rot_pos_var) - .unwrap(); + filter.observe_position(rot_position, rot_pos_var).unwrap(); filter - .observe_velocity( - rot_velocity, - rot_vel_variance) + .observe_velocity(rot_velocity, rot_vel_variance) .unwrap(); let mut boat_info = boat_info_mutex.write().unwrap(); @@ -327,4 +329,4 @@ fn main() { } } } -} \ No newline at end of file +}