Skip to content

Commit

Permalink
Merge pull request #14 from avsaase/remove-gravity-state
Browse files Browse the repository at this point in the history 
Remove gravity state
  • Loading branch information
@nordmoen
nordmoen authored Dec 9, 2024
2 parents 74323ef + 6fada96 commit 8e16b9c
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 25 deletions.
2 changes: 1 addition & 1 deletion Cargo.toml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[package]
name = "eskf"
version = "0.3.0"
version = "0.4.0"
authors = ["Jørgen Nordmoen <[email protected]>"]
description = "Navigation filter based on an Error State Kalman Filter (ESKF)"
keywords = ["navigation", "filter", "orientation", "kalman"]
Expand Down
68 changes: 44 additions & 24 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,10 @@
#![cfg_attr(not(feature = "std"), no_std)]

use core::ops::{AddAssign, SubAssign};

use nalgebra::{
base::allocator::Allocator, DefaultAllocator, Dim, Matrix2, Matrix3, OMatrix, OVector, Point3,
UnitQuaternion, Vector2, Vector3, U1, U18, U2, U3, U5, U6,
UnitQuaternion, Vector2, Vector3, U1, U15, U2, U3, U5, U6,
};
#[cfg(feature = "no_std")]
use num_traits::float::Float;

Check warning on line 48 in src/lib.rs

View workflow job for this annotation

GitHub Actions / ci (stable, ubuntu-latest, --no-default-features --features no_std) 

unused import: `num_traits::float::Float`

Check warning on line 48 in src/lib.rs

View workflow job for this annotation

GitHub Actions / ci (stable, macOS-latest, --no-default-features --features no_std) 

unused import: `num_traits::float::Float`

Check warning on line 48 in src/lib.rs

View workflow job for this annotation

GitHub Actions / ci (nightly, ubuntu-latest, --no-default-features --features no_std) 

unused import: `num_traits::float::Float`

Check warning on line 48 in src/lib.rs

View workflow job for this annotation

GitHub Actions / ci (stable, windows-latest, --no-default-features --features no_std) 

unused import: `num_traits::float::Float`
Expand Down Expand Up @@ -75,13 +76,14 @@ pub type Delta = std::time::Duration;
pub type Delta = f32;

/// Builder for [`ESKF`]
#[derive(Copy, Clone, Default, Debug)]
#[derive(Copy, Clone, Debug)]
pub struct Builder {
var_acc: Vector3<f32>,
var_rot: Vector3<f32>,
var_acc_bias: Vector3<f32>,
var_rot_bias: Vector3<f32>,
process_covariance: f32,
gravity: f32,
}

impl Builder {
Expand Down Expand Up @@ -156,14 +158,23 @@ impl Builder {

/// Set the initial covariance for the process matrix
///
/// The covariance value should be a small process value so that the covariance of the filter
/// The covariance value should be a small process value so tha
/// t the covariance of the filter
/// quickly converges to the correct value. Too small values could lead to the filter taking a
/// long time to converge and report a lower covariance than what it should.
pub fn initial_covariance(mut self, covar: f32) -> Self {
self.process_covariance = covar;
self
}

/// Set the used gravity in m/s².
///
/// The default value is 9.81 m/s².
pub fn gravity(mut self, gravity: f32) -> Self {
self.gravity = gravity;
self
}

/// Convert the builder into a new filter
pub fn build(self) -> ESKF {
ESKF {
Expand All @@ -172,8 +183,8 @@ impl Builder {
orientation: UnitQuaternion::identity(),
accel_bias: Vector3::zeros(),
rot_bias: Vector3::zeros(),
gravity: Vector3::new(0f32, 0f32, 9.81),
covariance: OMatrix::<f32, U18, U18>::identity() * self.process_covariance,
gravity: Vector3::new(0.0, 0.0, self.gravity),
covariance: OMatrix::<f32, U15, U15>::identity() * self.process_covariance,
var_acc: self.var_acc,
var_rot: self.var_rot,
var_acc_bias: self.var_acc_bias,
Expand All @@ -182,6 +193,19 @@ impl Builder {
}
}

impl Default for Builder {
fn default() -> Self {
Self {
var_acc: Default::default(),
var_rot: Default::default(),
var_acc_bias: Default::default(),
var_rot_bias: Default::default(),
process_covariance: Default::default(),
gravity: 9.81,
}
}
}

/// Error State Kalman Filter
///
/// The filter works by calling [`predict`](ESKF::predict) and one or more of the
Expand All @@ -207,10 +231,10 @@ pub struct ESKF {
pub accel_bias: Vector3<f32>,
/// Estimated rotation bias
pub rot_bias: Vector3<f32>,
/// Estimated gravity vector
/// Gravity vector.
pub gravity: Vector3<f32>,
/// Covariance of filter state
covariance: OMatrix<f32, U18, U18>,
covariance: OMatrix<f32, U15, U15>,
/// Acceleration variance
var_acc: Vector3<f32>,
/// Rotation variance
Expand Down Expand Up @@ -283,7 +307,7 @@ impl ESKF {
// Propagate uncertainty, since we have not observed any new information about the state of
// the filter we need to update our estimate of the uncertainty of the filer
let ident_delta = Matrix3::<f32>::identity() * delta_t;
let mut error_jacobian = OMatrix::<f32, U18, U18>::identity();
let mut error_jacobian = OMatrix::<f32, U15, U15>::identity();
error_jacobian
.fixed_view_mut::<3, 3>(0, 3)
.copy_from(&ident_delta);
Expand All @@ -293,9 +317,6 @@ impl ESKF {
error_jacobian
.fixed_view_mut::<3, 3>(3, 9)
.copy_from(&(-orient_mat * delta_t));
error_jacobian
.fixed_view_mut::<3, 3>(3, 15)
.copy_from(&ident_delta);
error_jacobian
.fixed_view_mut::<3, 3>(6, 6)
.copy_from(&norm_rot.to_rotation_matrix().into_inner().transpose());
Expand Down Expand Up @@ -329,12 +350,12 @@ impl ESKF {
/// - `variance` is the uncertainty of the observation
pub fn update<R: Dim>(
&mut self,
jacobian: OMatrix<f32, R, U18>,
jacobian: OMatrix<f32, R, U15>,
difference: OVector<f32, R>,
variance: OMatrix<f32, R, R>,
) -> Result<()>
where
DefaultAllocator: Allocator<R> + Allocator<R, R> + Allocator<R, U18> + Allocator<U18, R>,
DefaultAllocator: Allocator<R> + Allocator<R, R> + Allocator<R, U15> + Allocator<U15, R>,
{
// Correct filter based on Kalman gain
let kalman_gain = self.covariance
Expand All @@ -349,26 +370,25 @@ impl ESKF {
* (&jacobian * self.covariance * &jacobian.transpose() + &variance)
* &kalman_gain.transpose();
} else if cfg!(feature = "cov-joseph") {
let step1 = OMatrix::<f32, U18, U18>::identity() - &kalman_gain * &jacobian;
let step1 = OMatrix::<f32, U15, U15>::identity() - &kalman_gain * &jacobian;
let step2 = &kalman_gain * &variance * &kalman_gain.transpose();
self.covariance = step1 * self.covariance * step1.transpose() + step2;
} else {
self.covariance =
(OMatrix::<f32, U18, U18>::identity() - &kalman_gain * &jacobian) * self.covariance;
(OMatrix::<f32, U15, U15>::identity() - &kalman_gain * &jacobian) * self.covariance;
}
// Inject error state into nominal
self.position += error_state.fixed_view::<3, 1>(0, 0);
self.velocity += error_state.fixed_view::<3, 1>(3, 0);
self.orientation *= UnitQuaternion::from_scaled_axis(error_state.fixed_view::<3, 1>(6, 0));
self.accel_bias += error_state.fixed_view::<3, 1>(9, 0);
self.rot_bias += error_state.fixed_view::<3, 1>(12, 0);
self.gravity += error_state.fixed_view::<3, 1>(15, 0);
// Perform full ESKF reset
//
// Since the orientation error is usually relatively small this step can be skipped, but
// the full formulation can lead to better stability of the filter
if cfg!(feature = "full-reset") {
let mut g = OMatrix::<f32, U18, U18>::identity();
let mut g = OMatrix::<f32, U15, U15>::identity();
g.fixed_view_mut::<3, 3>(6, 6)
.sub_assign(0.5 * skew(&error_state.fixed_view::<3, 1>(6, 0).clone_owned()));
self.covariance = g * self.covariance * g.transpose();
Expand All @@ -388,7 +408,7 @@ impl ESKF {
velocity: Vector2<f32>,
velocity_var: Matrix2<f32>,
) -> Result<()> {
let mut jacobian = OMatrix::<f32, U5, U18>::zeros();
let mut jacobian = OMatrix::<f32, U5, U15>::zeros();
jacobian.fixed_view_mut::<5, 5>(0, 0).fill_with_identity();

let mut diff = OVector::<f32, U5>::zeros();
Expand Down Expand Up @@ -416,7 +436,7 @@ impl ESKF {
velocity: Vector3<f32>,
velocity_var: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = OMatrix::<f32, U6, U18>::zeros();
let mut jacobian = OMatrix::<f32, U6, U15>::zeros();
jacobian.fixed_view_mut::<6, 6>(0, 0).fill_with_identity();

let mut diff = OVector::<f32, U6>::zeros();
Expand All @@ -438,15 +458,15 @@ impl ESKF {
measurement: Point3<f32>,
variance: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = OMatrix::<f32, U3, U18>::zeros();
let mut jacobian = OMatrix::<f32, U3, U15>::zeros();
jacobian.fixed_view_mut::<3, 3>(0, 0).fill_with_identity();
let diff = measurement - self.position;
self.update(jacobian, diff, variance)
}

/// Update the filter with an observation of the height alone
pub fn observe_height(&mut self, measured: f32, variance: f32) -> Result<()> {
let mut jacobian = OMatrix::<f32, U1, U18>::zeros();
let mut jacobian = OMatrix::<f32, U1, U15>::zeros();
jacobian.fixed_view_mut::<1, 1>(0, 2).fill_with_identity();
let diff = OVector::<f32, U1>::new(measured - self.position.z);
let var = OMatrix::<f32, U1, U1>::new(variance);
Expand All @@ -464,7 +484,7 @@ impl ESKF {
measurement: Vector3<f32>,
variance: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = OMatrix::<f32, U3, U18>::zeros();
let mut jacobian = OMatrix::<f32, U3, U15>::zeros();
jacobian.fixed_view_mut::<3, 3>(0, 3).fill_with_identity();
let diff = measurement - self.velocity;
self.update(jacobian, diff, variance)
Expand All @@ -481,7 +501,7 @@ impl ESKF {
measurement: Vector2<f32>,
variance: Matrix2<f32>,
) -> Result<()> {
let mut jacobian = OMatrix::<f32, U2, U18>::zeros();
let mut jacobian = OMatrix::<f32, U2, U15>::zeros();
jacobian.fixed_view_mut::<2, 2>(0, 3).fill_with_identity();
let diff = Vector2::new(
measurement.x - self.velocity.x,
Expand All @@ -496,7 +516,7 @@ impl ESKF {
measurement: UnitQuaternion<f32>,
variance: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = OMatrix::<f32, U3, U18>::zeros();
let mut jacobian = OMatrix::<f32, U3, U15>::zeros();
jacobian.fixed_view_mut::<3, 3>(0, 6).fill_with_identity();
let diff = measurement * self.orientation;
self.update(jacobian, diff.scaled_axis(), variance)
Expand Down

0 comments on commit 8e16b9c

Please sign in to comment.