Skip to content

Commit

Permalink
Various improvement to execution efficiency and low accuracy on measu…
Browse files Browse the repository at this point in the history 
…re handling
  • Loading branch information
EdoZua95 committed May 17, 2024
1 parent eed80e4 commit d152d5c
Showing 1 changed file with 73 additions and 117 deletions.
190 changes: 73 additions & 117 deletions src/bin/sailtrack-kalman.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,10 +76,10 @@ struct Boat {

#[derive(Debug, Clone, Copy)]
struct Measure {
position: Point3<f32>,
velocity_xy: Vector2<f32>,
position: Point3<f32>,
velocity_xy: Vector2<f32>,
velocity_z: f32,
pos_variance: Matrix3<f32>,
pos_variance: Matrix3<f32>,
vel_variance: Matrix2<f32>,
vel_z_variance: f32,
new_measure: bool,
Expand All @@ -92,70 +92,74 @@ struct Input {
new_input: bool,
}

#[derive(Debug, Clone, Copy)]
struct Debug {
mqtt_recieve_process_counter:u64,
mqtt_send_process_counter:u64,
filter_process_counter:u64,
}


// Function to continuously try locking the mutex until successful
fn acquire_lock<T>(mutex: &Arc<Mutex<T>>) -> std::sync::MutexGuard<T> {
let mut count = 0;
loop {
if let Ok(guard) = mutex.try_lock() {
return guard;
}
let mut rng = rand::thread_rng();
let sleep_time: u64 = rng.gen_range(5..10);
thread::sleep(Duration::from_millis(sleep_time));
count += 1;

if count > 500{
println!("Acquire lock failed on mutex containig value {:?}", std::any::type_name::<T>());
}

}
}

// Function to compute the measure for the Kalman filter from the raw GPS data
fn get_measure_forom_gps(
measure: &Gps, reference: &Gps,
) -> Measure {
fn get_measure_forom_gps(measure: &Gps, reference: &Gps) -> Measure {
let position = Point3::new(
(measure.lat*f32::powf(10.0, -7.0) - reference.lat*f32::powf(10.0, -7.0)) * EARTH_CIRCUMFERENCE_METERS / 360.0,
(measure.lon*f32::powf(10.0, -7.0) - reference.lon*f32::powf(10.0, -7.0)) * EARTH_CIRCUMFERENCE_METERS * LAT_FACTOR / 360.0,
measure.h_msl*f32::powf(10.0, -3.0) - reference.h_msl*f32::powf(10.0, -3.0),
(measure.lat * f32::powf(10.0, -7.0) - reference.lat * f32::powf(10.0, -7.0))
* EARTH_CIRCUMFERENCE_METERS
/ 360.0,
(measure.lon * f32::powf(10.0, -7.0) - reference.lon * f32::powf(10.0, -7.0))
* EARTH_CIRCUMFERENCE_METERS
* LAT_FACTOR
/ 360.0,
measure.h_msl * f32::powf(10.0, -3.0) - reference.h_msl * f32::powf(10.0, -3.0),
);
let velocity_xy = Vector2::new(measure.vel_n*f32::powf(10.0, -3.0), measure.vel_e*f32::powf(10.0, -3.0));
let velocity_z = -measure.vel_d*f32::powf(10.0, -3.0);
let velocity_xy = Vector2::new(
measure.vel_n * f32::powf(10.0, -3.0),
measure.vel_e * f32::powf(10.0, -3.0),
);
let velocity_z = -measure.vel_d * f32::powf(10.0, -3.0);
let accuracy_penality_factor = 100.0;
let mut pos_variance = Matrix3::zeros();
pos_variance[(0, 0)] = 0.25 * (measure.h_acc*f32::powf(10.0, -3.0)).powf(2.0);
pos_variance[(1, 1)] = 0.25 * (measure.h_acc*f32::powf(10.0, -3.0)).powf(2.0);
pos_variance[(2, 2)] = 0.25 * (measure.v_acc*f32::powf(10.0, -3.0)).powf(2.0);
let vel_variance = Matrix2::identity() * 0.25 * (measure.s_acc*f32::powf(10.0, -3.0)).powf(2.0);
let vel_z_variance = 0.25 * (measure.s_acc*f32::powf(10.0, -3.0)).powf(2.0);
let measure = Measure {
pos_variance[(0, 0)] = 0.25 * (measure.h_acc * f32::powf(10.0, -3.0)).powf(2.0);
pos_variance[(1, 1)] = 0.25 * (measure.h_acc * f32::powf(10.0, -3.0)).powf(2.0);
pos_variance[(2, 2)] = 0.25 * (measure.v_acc * f32::powf(10.0, -3.0)).powf(2.0);
let mut vel_variance =
Matrix2::identity() * 0.25 * (measure.s_acc * f32::powf(10.0, -3.0)).powf(2.0);
let mut vel_z_variance = 0.25 * (measure.s_acc * f32::powf(10.0, -3.0)).powf(2.0);
if measure.fix_type != 3 {
pos_variance[(0, 0)] *= accuracy_penality_factor;
pos_variance[(1, 1)] *= accuracy_penality_factor;
pos_variance[(2, 2)] *= accuracy_penality_factor;
vel_variance *= accuracy_penality_factor;
vel_z_variance *= accuracy_penality_factor;
}
Measure {
position,
velocity_xy,
velocity_z,
pos_variance,
vel_variance,
vel_z_variance,
new_measure: true,
};
measure
}
}


// Function that keeps on controll if the GPS fix is obtained
fn wait_for_fix_tipe(gps_ref_arc: &Arc<Mutex<Gps>>) {
fn wait_for_fix_tipe(gps_ref_arc: &Arc<Mutex<Gps>>) -> bool {
let gps_ref_lock = acquire_lock(gps_ref_arc);
if gps_ref_lock.fix_type == 3 {
drop(gps_ref_lock);
return true;
}
drop(gps_ref_lock);
loop {
let gps_ref_lock = acquire_lock(gps_ref_arc);
if gps_ref_lock.fix_type == 3 {
drop(gps_ref_lock);
break;
return true;
}
drop(gps_ref_lock);
thread::sleep(Duration::from_millis(1000));
Expand All @@ -177,21 +181,16 @@ fn on_message_imu(message: Imu, input: &Arc<Mutex<Input>>) {
}

fn on_message_gps(message: Gps, gps_ref_arc: &Arc<Mutex<Gps>>, measure_arc: &Arc<Mutex<Measure>>) {

let mut gps_ref_lock = acquire_lock(gps_ref_arc);
let mut measure_lock = acquire_lock(measure_arc);
println!("Message recieved: {:?}", message);

if gps_ref_lock.fix_type != 3 {
*gps_ref_lock = message;
println!("GPS fix obtained: {:?}", gps_ref_lock);
}
let measure = get_measure_forom_gps(&message, &gps_ref_lock);
*measure_lock = measure;
println!("Measure obtained: {:?}", measure);
drop(measure_lock);
drop(gps_ref_lock);
println!("Mutex Dropped");
}

// Kalman predict function on new input
Expand All @@ -203,15 +202,19 @@ fn filter_predict(kalman: &mut ESKF, input: &mut Input, filter_ts: Duration) {
// Kalman update function on new measure
fn filter_update(kalman: &mut ESKF, measure: &mut Measure) {
kalman
.observe_position_velocity2d(measure.position, measure.pos_variance, measure.velocity_xy, measure.vel_variance)
.unwrap();
.observe_position_velocity2d(
measure.position,
measure.pos_variance,
measure.velocity_xy,
measure.vel_variance,
)
.unwrap();
kalman
.observe_height(measure.velocity_z, measure.vel_z_variance)
.unwrap();
.observe_height(measure.velocity_z, measure.vel_z_variance)
.unwrap();
measure.new_measure = false;
}


fn angle_wrap_180(angle: f32) -> f32 {
(angle + 180.0) % 360.0 - 180.0
}
Expand Down Expand Up @@ -274,16 +277,6 @@ fn main() {
let input_mutex = Arc::new(Mutex::new(input));
let filter_mutex = Arc::new(Mutex::new(filter));

// DEBUG
let debug = Debug {
mqtt_recieve_process_counter:0,
mqtt_send_process_counter:0,
filter_process_counter:0,
};

let debug_mutex = Arc::new(Mutex::new(debug));


// TODO: Add username and password authentication
let mqqt_opts = MqttOptions::new("sailtrack-aggregator", "localhost", 1883);

Expand All @@ -295,7 +288,6 @@ fn main() {
let gps_ref_clone = Arc::clone(&gps_ref_mutex);
let measure_clone = Arc::clone(&measure_mutex);
let input_clone = Arc::clone(&input_mutex);
let debug_clone = Arc::clone(&debug_mutex);
thread::spawn(move || {
for notification in connection.iter().flatten() {
if let Event::Incoming(Incoming::Publish(packet)) = notification {
Expand All @@ -320,10 +312,6 @@ fn main() {
_ => (),
}
}
// Degug
let mut debug_lock = acquire_lock(&debug_clone);
debug_lock.mqtt_recieve_process_counter += 1;
drop(debug_lock);
}
});

Expand All @@ -332,7 +320,6 @@ fn main() {
let measure_clone = Arc::clone(&measure_mutex);
let input_clone = Arc::clone(&input_mutex);
let filter_clone = Arc::clone(&filter_mutex);
let debug_clone = Arc::clone(&debug_mutex);
thread::spawn(move || loop {
// Check if the GPS fix has been obtained
wait_for_fix_tipe(&gps_ref_clone);
Expand All @@ -341,44 +328,34 @@ fn main() {
let mut input_lock = acquire_lock(&input_clone);
let mut filter_lock = acquire_lock(&filter_clone);

// Reliable GPS and IMU data: predict and update
if measure_lock.new_measure && input_lock.new_input {
println!("Predict and update");
filter_predict(&mut filter_lock, &mut input_lock, filter_ts);
filter_update(&mut filter_lock, &mut measure_lock);
drop(input_lock);
drop(measure_lock);
drop(filter_lock);
}
// Only reliable GPS data: only update
else if !input_lock.new_input && measure_lock.new_measure {
println!("Update");
filter_update(&mut filter_lock, &mut measure_lock);
drop(measure_lock);
drop(filter_lock);
}
// Only reliable or old IMU data: only predict
else {
println!("Predict");
filter_predict(&mut filter_lock, &mut input_lock, filter_ts);
drop(input_lock);
drop(filter_lock);
match (measure_lock.new_measure, input_lock.new_input) {
(true, true) => {
filter_predict(&mut filter_lock, &mut input_lock, filter_ts);
filter_update(&mut filter_lock, &mut measure_lock);
drop(input_lock);
drop(measure_lock);
drop(filter_lock);
}
(true, false) => {
filter_update(&mut filter_lock, &mut measure_lock);
drop(measure_lock);
drop(filter_lock);
}
_ => {
filter_predict(&mut filter_lock, &mut input_lock, filter_ts);
drop(input_lock);
drop(filter_lock);
}
}
let elapsed = thread_start.elapsed();
if elapsed.as_millis() < filter_ts.as_millis() {
thread::sleep(filter_ts - elapsed);
}
// Degug
let mut debug_lock = acquire_lock(&debug_clone);
debug_lock.filter_process_counter += 1;
drop(debug_lock);
});


//MQTT publish thread
let gps_ref_clone = Arc::clone(&gps_ref_mutex);
let filter_clone = Arc::clone(&filter_mutex);
let debug_clone = Arc::clone(&debug_mutex);
thread::spawn(move || loop {
// Check if the GPS fix has been obtained
wait_for_fix_tipe(&gps_ref_clone);
Expand Down Expand Up @@ -410,10 +387,11 @@ fn main() {
}
}
let gps_ref_lock = acquire_lock(&gps_ref_clone);
let lat =
position.x * 360.0 / EARTH_CIRCUMFERENCE_METERS / LAT_FACTOR + gps_ref_lock.lat*f32::powf(10.0, -7.0);
let lon: f32 = position.y * 360.0 / EARTH_CIRCUMFERENCE_METERS + gps_ref_lock.lon*f32::powf(10.0, -7.0);
let altitude = position.z + gps_ref_lock.h_msl*f32::powf(10.0, -3.0);
let lat = position.x * 360.0 / EARTH_CIRCUMFERENCE_METERS / LAT_FACTOR
+ gps_ref_lock.lat * f32::powf(10.0, -7.0);
let lon: f32 = position.y * 360.0 / EARTH_CIRCUMFERENCE_METERS
+ gps_ref_lock.lon * f32::powf(10.0, -7.0);
let altitude = position.z + gps_ref_lock.h_msl * f32::powf(10.0, -3.0);
drop(gps_ref_lock);

let message = Boat {
Expand All @@ -437,27 +415,5 @@ fn main() {
)
.unwrap();
thread::sleep(Duration::from_millis(1000 / MQTT_PUBLISH_FREQ_HZ));

// Degug
let mut debug_lock = acquire_lock(&debug_clone);
debug_lock.mqtt_send_process_counter += 1;
drop(debug_lock);
});

let debug_clone = Arc::clone(&debug_mutex);
let gps_ref_clone = Arc::clone(&gps_ref_mutex);
loop {
let debug_lock = acquire_lock(&debug_clone);
println!(
"MQTT send process counter: {}, MQTT receive process counter: {}, Filter process counter: {}",
debug_lock.mqtt_send_process_counter, debug_lock.mqtt_recieve_process_counter, debug_lock.filter_process_counter
);
drop(debug_lock);
thread::sleep(Duration::from_secs(1));
let gps_ref_lock = acquire_lock(&gps_ref_clone);
drop(gps_ref_lock);
println!("GPS ref access granted:");
};

}

0 comments on commit d152d5c

Please sign in to comment.