WIP: Initial implementation for the rust kalman filter

Cargo.toml

@@ -5,3 +5,9 @@ edition = "2021"
 
 [dependencies]
 rumqttc = "0.24"
+eskf = "0.2.0"
+nalgebra = "=0.25.4"
+quaternion-core = "0.5.0"
+tokio = { version = "1.17.0", features = ["full"] }
+serde = { version = "1.0", features = ["derive"] }
+serde_json = "1.0.116"

src/bin/eskf.rs

@@ -0,0 +1,92 @@
+use nalgebra::{Matrix2, Matrix3, Point3, Vector2, Vector3};
+use paho_mqtt::Client;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    // Measure realization
+    let acc = Vector3::new(1.1, 2.1, 0.1);
+    let orient = Vector3::new(0.4, 0.7, 0.3);
+    let pos = Point3::new(10.0, 15.0, 0.0);
+    let vel = Vector2::new(5.0, 3.0);
+    let high: f32 = 12.2;
+    let high_var: f32 = 0.01;
+
+    // Measure variances
+    let acc_var: f32 = 0.01;
+    let orient_var: f32 = 0.01;
+    let pos_var: f32 = 0.02;
+    let vel_var: f32 = 0.001;
+    let ts = std::time::Duration::from_millis(10);
+
+    // Measure struct
+    struct Imu {
+        acceleration: Vector3<f32>,
+        rotation: Vector3<f32>,
+    }
+
+    struct Gnss {
+        position: Point3<f32>,
+        pos_variance: Matrix3<f32>,
+        velocity: Vector2<f32>,
+        vel_variance: Matrix2<f32>,
+        high: f32,
+        high_var: f32,
+    }
+
+    let imu_read = Imu {
+        acceleration: acc,
+        rotation: orient,
+    };
+
+    let gps = Gnss {
+        position: pos,
+        pos_variance: Matrix3::identity() * pos_var,
+        velocity: vel,
+        vel_variance: Matrix2::identity() * vel_var,
+        high,
+        high_var,
+    };
+
+    println!("GNSS and IMU INIT");
+
+    // Just making sure that we didn't accidentally create a null pointer here
+    println!(
+        "IMU acceleration x: {}, y: {}, z: {}",
+        imu_read.acceleration.x, imu_read.acceleration.y, imu_read.acceleration.z
+    );
+    println!(
+        "IMU rotation x: {}, y: {}, z: {}",
+        imu_read.rotation.x, imu_read.rotation.y, imu_read.rotation.z
+    );
+    println!(
+        "GNSS position x: {}, y: {}, z: {}",
+        gps.position.x, gps.position.y, gps.position.z
+    );
+    println!("GNSS velocity x: {}, y: {}", gps.velocity.x, gps.velocity.y);
+    print!("GNSS high: {}", gps.high);
+    // ESKF Object construction
+    let mut filter = eskf::Builder::new()
+        .acceleration_variance(acc_var)
+        .rotation_variance(orient_var)
+        .build();
+
+    println!("\n Filter built");
+
+    filter.predict(imu_read.acceleration, imu_read.rotation, ts);
+
+    println!("Filter prediction");
+
+    let _result = filter.observe_position_velocity2d(
+        gps.position,
+        gps.pos_variance,
+        gps.velocity,
+        gps.vel_variance,
+    );
+
+    println!("Filter filtered prediction with XY velocity");
+
+    let _result_high = filter.observe_height(gps.high, gps.high_var);
+
+    println!("Filter filtered prediction with height");
+
+    Ok(())
+}

src/bin/rumqtt.rs

@@ -0,0 +1,86 @@
+
+// paho-mqtt/examples/topic_publish.rs
+//
+// Example application for Paho MQTT Rust library.
+//
+//! This is a simple asynchronous publisher that uses a topic object to
+//! repeatedly publish messages on the same topic.
+//!
+//! This sample demonstrates:
+//!   - Connecting to an MQTT broker
+//!   - Publishing a message asynchronously
+//!   - Using a 'Topic' object to publish multiple messages to the same topic.
+//!
+
+/*******************************************************************************
+ * Copyright (c) 2017-2023 Frank Pagliughi <fpagliughi@mindspring.com>
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v2.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v20.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Frank Pagliughi - initial implementation and documentation
+ *******************************************************************************/
+
+ use paho_mqtt as mqtt;
+ use std::{env, process};
+ use serde::{Serialize, Deserialize};
+ use serde_json::json;
+
+ const QOS: i32 = 1;
+
+ struct message {
+    sog: f32,
+    cog: f32,
+    heading: i32,
+ }
+
+ /////////////////////////////////////////////////////////////////////////////
+
+ fn main() { 
+
+    let mess = message {sog: 5.0, cog: 10.0, heading: 100};
+    let json_message = serde_json::to_string(&mess)?;
+
+     // Parse command line argumentS
+     let host = env::args()
+         .nth(1)
+         .unwrap_or_else(|| "mqtt://localhost:1883".to_string());
+
+     // Create a client & define connect options
+     let cli = mqtt::AsyncClient::new(host).unwrap_or_else(|err| {
+         println!("Error creating the client: {}", err);
+         process::exit(1);
+     });
+
+     let conn_opts = mqtt::ConnectOptions::new();
+
+     // Connect and wait for it to complete or fail
+     if let Err(e) = cli.connect(conn_opts).wait() {
+         println!("Unable to connect: {:?}", e);
+         process::exit(1);
+     }
+
+     // Create a topic and publish to it
+     println!("Publishing messages on the 'test' topic");
+     let topic = mqtt::Topic::new(&cli, "test", QOS);
+     for _ in 0..5 {
+         let tok = topic.publish("Hello there");
+
+         if let Err(e) = tok.wait() {
+             println!("Error sending message: {:?}", e);
+             break;
+         }
+     }
+
+     // Disconnect from the broker
+     let tok = cli.disconnect(None);
+     tok.wait().unwrap();
+ }
+