Skip to content

Commit

Permalink
Cargo Format
Browse files Browse the repository at this point in the history
  • Loading branch information
Edoalto-metis authored and Edoalto-metis committed Oct 3, 2024
1 parent ca77cef commit b8457e8
Showing 1 changed file with 37 additions and 35 deletions.
72 changes: 37 additions & 35 deletions src/bin/sailtrack-kalman.rs
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -49,15 +49,18 @@ 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,
z: 0.0,
},
}
}

}

#[derive(Serialize, Deserialize, Clone, Copy, Debug)]
Expand Down Expand Up @@ -119,7 +122,7 @@ struct Boat {
struct BoatInfo {
filter: ESKF,
ref_pos: Gps,
orientation: Orientation
orientation: Orientation,
}

fn angle_wrap_180(angle: f32) -> f32 {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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...");
Expand All @@ -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);
Expand All @@ -289,42 +292,41 @@ 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();
boat_info.filter = filter;
}
}
}
}
}

0 comments on commit b8457e8

Please sign in to comment.