Skip to content

Commit

Permalink
Revert "Fix, document state estimator, rename methods"
Browse files Browse the repository at this point in the history 
This reverts commit ca7796f.
  • Loading branch information
@KoffeinFlummi
KoffeinFlummi committed Apr 19, 2024
1 parent 16297e8 commit f2affae
Show file tree
Hide file tree
Showing 3 changed files with 35 additions and 36 deletions.
65 changes: 31 additions & 34 deletions src/state_estimation.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,15 +49,9 @@ impl StateEstimator {
let ahrs = ahrs::Mahony::new(dt, settings.mahony_kp, settings.mahony_ki);

let mut kalman = KalmanFilter::default();
// Höhe z, Vel z, Beschleunigung z

// State Vector
kalman.x = vector![
0.0, 0.0, 0.0, // XYZ position (m)
0.0, 0.0, 0.0, // XYZ velocity (m/s)
0.0, 0.0, 0.0 // XYZ acceleration (m/s^2)
];

// State Transition Matrix
kalman.x = vector![0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; // state vec
kalman.F = matrix![
1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.5 * dt * dt, 0.0, 0.0;
0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 0.5 * dt * dt, 0.0;
Expand All @@ -70,18 +64,23 @@ impl StateEstimator {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
];

// Measurement Matrix
kalman.H = matrix![
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; // barometer measures Z pos
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0; // acceleration X
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0; // acceleration Y
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; // acceleration Z
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0;
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0;
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
];

// State Covariance Matrix (initialized to a high value)
/*
kalman.P = Matrix3::new(
settings.std_dev_barometer.powi(2), 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, settings.std_dev_accelerometer.powi(2),
);
*/

kalman.P = Matrix::<f32, U9, U9, _>::identity() * 999.0 ;

// Process Covariance Matrix
kalman.Q = matrix![
0.25f32 * dt.powi(4), 0.0, 0.0, 0.5f32 * dt.powi(3), 0.0, 0.0, 0.5f32 * dt.powi(2), 0.0, 0.0;
0.0, 0.25f32 * dt.powi(4), 0.0, 0.0, 0.5f32 * dt.powi(3), 0.0, 0.0, 0.5f32 * dt.powi(2), 0.0;
Expand All @@ -94,7 +93,6 @@ impl StateEstimator {
0.0, 0.0, 0.5f32 * dt.powi(2), 0.0, 0.0, dt, 0.0, 0.0, 1.0;
] * settings.std_dev_process.powi(2);

// Measurement Covariance Matrix
kalman.R *= Matrix4::new(
settings.std_dev_barometer.powi(2), 0.0, 0.0, 0.0,
0.0, settings.std_dev_accelerometer.powi(2), 0.0, 0.0,
Expand Down Expand Up @@ -165,12 +163,11 @@ impl StateEstimator {
self.acceleration_world = None;
}

// Update the Kalman filter with barometric altitude and world-space acceleration
// Update the Kalman filter with barometric altitude and world-space vertical acceleration
let altitude_baro = barometer
.and_then(|a| (!a.is_nan()).then(|| a)) // NaN is not a valid altitude
.and_then(|a| (a > -100.0 && a < 12_000.0).then(|| a)); // neither is -13000
let accel = self.acceleration_world
.and_then(|a| (!(a.x.is_nan() || a.y.is_nan() || a.z.is_nan())).then_some(a));
let accel = self.acceleration_world.map.and_then(|a| (!(a.x.is_nan() || a.y.is_nan() || a.z.is_nan())).then_some(a));

match (accel, altitude_baro) {
(Some(accel), Some(altitude_baro)) => {
Expand Down Expand Up @@ -209,22 +206,10 @@ impl StateEstimator {
}
}

pub fn acceleration_world_raw(&self) -> Option<&Vector3<f32>> {
pub fn acceleration_world(&self) -> Option<&Vector3<f32>> {
self.acceleration_world.as_ref()
}

pub fn position_local(&self) -> Vector3<f32> {
Vector3::new(self.kalman.x[0], self.kalman.x[1], self.kalman.x[2])
}

pub fn velocity(&self) -> Vector3<f32> {
Vector3::new(self.kalman.x[3], self.kalman.x[4], self.kalman.x[5])
}

pub fn acceleration_world(&self) -> Vector3<f32> {
Vector3::new(self.kalman.x[6], self.kalman.x[7], self.kalman.x[8])
}

pub fn altitude_asl(&self) -> f32 {
self.position_local().z
}
Expand All @@ -237,14 +222,26 @@ impl StateEstimator {
self.velocity().z
}

pub fn vertical_acceleration(&self) -> f32 {
self.acceleration_world().z
pub fn vertical_accel(&self) -> f32 {
self.acceleration().z
}

pub fn time_in_mode(&self) -> u32 {
(self.time - self.mode_time).0
}

pub fn position_local(&self) -> Vector3<f32> {
Vector3::new(self.kalman.x[0], self.kalman.x[1], self.kalman.x[2])
}

pub fn velocity(&self) -> Vector3<f32> {
Vector3::new(self.kalman.x[3], self.kalman.x[4], self.kalman.x[5])
}

pub fn acceleration(&self) -> Vector3<f32> {
Vector3::new(self.kalman.x[6], self.kalman.x[7], self.kalman.x[8])
}

/// Main flight logic. This function is responsible for deciding whether to switch to a new
/// flight mode based on sensor data and therefore controls when the important events of the
/// flight will take place.. Generally, all conditions have to be true for a given
Expand Down
2 changes: 2 additions & 0 deletions src/telemetry.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@ use std::string::{String, ToString};
#[cfg(not(target_os = "none"))]
use std::vec::Vec;

#[cfg(target_os="none")]
use core::f32::consts::PI;
#[cfg(not(target_os = "none"))]
use std::f32::consts::PI;

Expand Down
4 changes: 2 additions & 2 deletions src/vehicle.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,8 @@ impl Into<VehicleState> for &mut Vehicle {
mode: Some(self.mode),
orientation: self.state_estimator.orientation,
vertical_speed: Some(self.state_estimator.vertical_speed()),
vertical_accel: self.state_estimator.acceleration_world_raw().map(|v| v.z),
vertical_accel_filtered: Some(self.state_estimator.vertical_acceleration()),
vertical_accel: self.state_estimator.acceleration_world().map(|v| v.z),
vertical_accel_filtered: Some(self.state_estimator.vertical_accel()),
altitude_asl: Some(self.state_estimator.altitude_asl()),
altitude_ground_asl: Some(self.state_estimator.altitude_ground),
apogee_asl: Some(self.state_estimator.altitude_max),
Expand Down

0 comments on commit f2affae

Please sign in to comment.