Skip to content

Commit

Permalink
Revert "updated state estimator to 3d"
Browse files Browse the repository at this point in the history 
This reverts commit 36670c7.
  • Loading branch information
@KoffeinFlummi
KoffeinFlummi committed Apr 19, 2024
1 parent f2affae commit bf65c59
Show file tree
Hide file tree
Showing 3 changed files with 32 additions and 80 deletions.
12 changes: 0 additions & 12 deletions Cargo.lock
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

2 changes: 1 addition & 1 deletion Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ serde = { version = "1", default-features = false, features = ["derive", "alloc"
postcard = "1.0"
crc = "3"
ahrs = { git = "https://github.com/KoffeinFlummi/ahrs-rs", branch="main", default-features = false }
nalgebra = { version = "0.31", default-features = false, features = ["alloc", "serde-serialize-no-std", "macros"] }
nalgebra = { version = "0.31", default-features = false, features = ["alloc", "serde-serialize-no-std"] }
filter = { git = "https://github.com/KoffeinFlummi/filter-rs", rev="5945c79426c9a16767929eb15a33efa4e51a4a39" }
siphasher = { version = "0.3", default-features = false }
rand = { version = "0.8", default-features = false }
Expand Down
98 changes: 31 additions & 67 deletions src/state_estimation.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ pub struct StateEstimator {
/// orientation
ahrs: ahrs::Mahony<f32>,
/// main Kalman filter
kalman: KalmanFilter<f32, U9, U4, U0>,
kalman: KalmanFilter<f32, U3, U2, U0>,
/// current orientation
pub orientation: Option<Unit<Quaternion<f32>>>,
/// current vehicle-space acceleration, switched between low- and high-G accelerometer
Expand All @@ -49,55 +49,30 @@ 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

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;
0.0, 0.0, 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.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0;
0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt;
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;
];

kalman.H = matrix![
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;
];

/*
kalman.x = Vector3::new(0.0, 0.0, 0.0);
kalman.F = Matrix3::new(
1.0, dt, dt * dt * 0.5,
0.0, 1.0, dt,
0.0, 0.0, 1.0
);
kalman.H = Matrix2x3::new(
1.0, 0.0, 0.0,
0.0, 0.0, 1.0
);
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 ;

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;
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.5f32 * dt.powi(3), 0.0, 0.0, dt.powi(2), 0.0, 0.0, dt, 0.0, 0.0;
0.0, 0.5f32 * dt.powi(3), 0.0, 0.0, dt.powi(2), 0.0, 0.0, dt, 0.0;
0.0, 0.0, 0.5f32 * dt.powi(3), 0.0, 0.0, dt.powi(2), 0.0, 0.0, dt;
0.5f32 * dt.powi(2), 0.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, 0.0;
0.0, 0.5f32 * dt.powi(2), 0.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0;
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);

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,
0.0, 0.0, settings.std_dev_accelerometer.powi(2), 0.0,
0.0, 0.0, 0.0, settings.std_dev_accelerometer.powi(2),
kalman.Q = Matrix3::new(
0.25f32 * dt.powi(4), 0.5f32 * dt.powi(3), 0.5f32 * dt.powi(2),
0.5f32 * dt.powi(3), dt.powi(2), dt,
0.5f32 * dt.powi(2), dt, 1.0f32,
) * settings.std_dev_process.powi(2);

kalman.R *= Matrix2::new(
settings.std_dev_barometer.powi(2), 0.0,
0.0, settings.std_dev_accelerometer.powi(2)
);

Self {
Expand Down Expand Up @@ -167,24 +142,25 @@ impl StateEstimator {
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.map.and_then(|a| (!(a.x.is_nan() || a.y.is_nan() || a.z.is_nan())).then_some(a));
let accel_z = self.acceleration_world.map(|a| a.z)
.and_then(|a| (!a.is_nan()).then(|| a));

match (accel, altitude_baro) {
(Some(accel), Some(altitude_baro)) => {
match (accel_z, altitude_baro) {
(Some(accel_z), Some(altitude_baro)) => {
self.kalman.predict(None, None, None, None);
let z = Vector4::new(altitude_baro, accel.x, accel.y , accel.z);
let z = Vector2::new(altitude_baro, accel_z);
self.kalman.update(&z, None, None);
},
(Some(accel), None) => {
(Some(accel_z), None) => {
// Use predicted altitude values, basically attempting to do inertial navigation.
self.kalman.predict(None, None, None, None);
let z = Vector4::new(self.altitude_asl(), accel.x, accel.y , accel.z);
let z = Vector2::new(self.altitude_asl(), accel_z);
self.kalman.update(&z, None, None);
}
(None, Some(altitude_baro)) => {
// Just assume acceleration is zero.
self.kalman.predict(None, None, None, None);
let z = Vector4::new(altitude_baro, 0.0, 0.0, 0.0);
let z = Vector2::new(altitude_baro, 0.0);
self.kalman.update(&z, None, None);
},
(None, None) => {
Expand All @@ -211,37 +187,25 @@ impl StateEstimator {
}

pub fn altitude_asl(&self) -> f32 {
self.position_local().z
self.kalman.x.x
}

pub fn altitude_agl(&self) -> f32 {
self.altitude_asl() - self.altitude_ground
}

pub fn vertical_speed(&self) -> f32 {
self.velocity().z
self.kalman.x.y
}

pub fn vertical_accel(&self) -> f32 {
self.acceleration().z
self.kalman.x.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

0 comments on commit bf65c59

Please sign in to comment.