#![deny(missing_docs)]
#![deny(unsafe_code)]
#![cfg_attr(not(feature = "std"), no_std)]
use core::ops::{AddAssign, SubAssign};
use nalgebra::{
base::allocator::Allocator, DefaultAllocator, Dim, Matrix2, Matrix3, MatrixMN, MatrixN, Point3,
UnitQuaternion, Vector2, Vector3, VectorN, U1, U18, U2, U3, U5,
};
#[derive(Copy, Clone, Debug)]
pub enum Error {
InversionError,
}
pub type Result<T> = core::result::Result<T, Error>;
#[cfg(feature = "std")]
pub type Delta = std::time::Duration;
#[cfg(not(feature = "std"))]
pub type Delta = f32;
#[derive(Copy, Clone, Default, 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,
}
impl Builder {
pub fn new() -> Self {
Builder::default()
}
pub fn acceleration_variance(mut self, var: f32) -> Self {
self.var_acc = Vector3::from_element(var.powi(2));
self
}
pub fn acceleration_variance_from_vec(mut self, var: Vector3<f32>) -> Self {
self.var_acc = var.map(|e| e.powi(2));
self
}
pub fn rotation_variance(mut self, var: f32) -> Self {
self.var_rot = Vector3::from_element(var.powi(2));
self
}
pub fn rotation_variance_from_vec(mut self, var: Vector3<f32>) -> Self {
self.var_rot = var.map(|e| e.powi(2));
self
}
pub fn acceleration_bias(mut self, bias: f32) -> Self {
self.var_acc_bias = Vector3::from_element(bias.powi(2));
self
}
pub fn acceleration_bias_from_vec(mut self, bias: Vector3<f32>) -> Self {
self.var_acc_bias = bias.map(|e| e.powi(2));
self
}
pub fn rotation_bias(mut self, bias: f32) -> Self {
self.var_rot_bias = Vector3::from_element(bias.powi(2));
self
}
pub fn rotation_bias_from_vec(mut self, bias: Vector3<f32>) -> Self {
self.var_rot_bias = bias.map(|e| e.powi(2));
self
}
pub fn initial_covariance(mut self, covar: f32) -> Self {
self.process_covariance = covar;
self
}
pub fn build(self) -> ESKF {
ESKF {
position: Point3::origin(),
velocity: Vector3::zeros(),
orientation: UnitQuaternion::identity(),
accel_bias: Vector3::zeros(),
rot_bias: Vector3::zeros(),
gravity: Vector3::new(0f32, 0f32, 9.81),
covariance: MatrixN::<f32, U18>::identity() * self.process_covariance,
var_acc: self.var_acc,
var_rot: self.var_rot,
var_acc_bias: self.var_acc_bias,
var_rot_bias: self.var_rot_bias,
}
}
}
#[derive(Copy, Clone, Debug)]
pub struct ESKF {
pub position: Point3<f32>,
pub velocity: Vector3<f32>,
pub orientation: UnitQuaternion<f32>,
pub accel_bias: Vector3<f32>,
pub rot_bias: Vector3<f32>,
pub gravity: Vector3<f32>,
covariance: MatrixN<f32, U18>,
var_acc: Vector3<f32>,
var_rot: Vector3<f32>,
var_acc_bias: Vector3<f32>,
var_rot_bias: Vector3<f32>,
}
impl ESKF {
pub fn variance_from_element(var: f32) -> Matrix3<f32> {
Matrix3::from_diagonal_element(var)
}
pub fn variance_from_diagonal(var: Vector3<f32>) -> Matrix3<f32> {
Matrix3::from_diagonal(&var)
}
fn uncertainty3(&self, start: usize) -> Vector3<f32> {
self.covariance
.diagonal()
.fixed_slice::<U3, U1>(start, 0)
.map(|var| var.sqrt())
}
pub fn position_uncertainty(&self) -> Vector3<f32> {
self.uncertainty3(0)
}
pub fn velocity_uncertainty(&self) -> Vector3<f32> {
self.uncertainty3(3)
}
pub fn orientation_uncertainty(&self) -> Vector3<f32> {
self.uncertainty3(6)
}
pub fn predict(&mut self, acceleration: Vector3<f32>, rotation: Vector3<f32>, delta: Delta) {
#[cfg(feature = "std")]
let delta_t = delta.as_secs_f32();
#[cfg(not(feature = "std"))]
let delta_t = delta;
let rot_acc_grav = self
.orientation
.transform_vector(&(acceleration - self.accel_bias))
+ self.gravity;
let norm_rot = UnitQuaternion::from_scaled_axis((rotation - self.rot_bias) * delta_t);
let orient_mat = self.orientation.to_rotation_matrix().into_inner();
self.position += self.velocity * delta_t + 0.5 * rot_acc_grav * delta_t.powi(2);
self.velocity += rot_acc_grav * delta_t;
self.orientation *= norm_rot;
let ident_delta = Matrix3::<f32>::identity() * delta_t;
let mut error_jacobian = MatrixN::<f32, U18>::identity();
error_jacobian
.fixed_slice_mut::<U3, U3>(0, 3)
.copy_from(&ident_delta);
error_jacobian
.fixed_slice_mut::<U3, U3>(3, 6)
.copy_from(&(-orient_mat * skew(&(acceleration - self.accel_bias)) * delta_t));
error_jacobian
.fixed_slice_mut::<U3, U3>(3, 9)
.copy_from(&(-orient_mat * delta_t));
error_jacobian
.fixed_slice_mut::<U3, U3>(3, 15)
.copy_from(&ident_delta);
error_jacobian
.fixed_slice_mut::<U3, U3>(6, 6)
.copy_from(&norm_rot.to_rotation_matrix().into_inner().transpose());
error_jacobian
.fixed_slice_mut::<U3, U3>(6, 12)
.copy_from(&-ident_delta);
self.covariance = error_jacobian * self.covariance * error_jacobian.transpose();
let mut diagonal = self.covariance.diagonal();
diagonal
.fixed_slice_mut::<U3, U1>(3, 0)
.add_assign(self.var_acc * delta_t.powi(2));
diagonal
.fixed_slice_mut::<U3, U1>(6, 0)
.add_assign(self.var_rot * delta_t.powi(2));
diagonal
.fixed_slice_mut::<U3, U1>(9, 0)
.add_assign(self.var_acc_bias * delta_t);
diagonal
.fixed_slice_mut::<U3, U1>(12, 0)
.add_assign(self.var_rot_bias * delta_t);
self.covariance.set_diagonal(&diagonal);
}
pub fn update<R: Dim>(
&mut self,
jacobian: MatrixMN<f32, R, U18>,
difference: VectorN<f32, R>,
variance: MatrixN<f32, R>,
) -> Result<()>
where
DefaultAllocator: Allocator<f32, R>
+ Allocator<f32, R, R>
+ Allocator<f32, R, U18>
+ Allocator<f32, U18, R>,
{
let kalman_gain = self.covariance
* &jacobian.transpose()
* (&jacobian * self.covariance * &jacobian.transpose() + &variance)
.try_inverse()
.ok_or(Error::InversionError)?;
let error_state = &kalman_gain * difference;
if cfg!(feature = "cov-symmetric") {
self.covariance -= &kalman_gain
* (&jacobian * self.covariance * &jacobian.transpose() + &variance)
* &kalman_gain.transpose();
} else if cfg!(feature = "cov-joseph") {
let step1 = MatrixN::<f32, U18>::identity() - &kalman_gain * &jacobian;
let step2 = &kalman_gain * &variance * &kalman_gain.transpose();
self.covariance = step1 * self.covariance * step1.transpose() + step2;
} else {
self.covariance =
(MatrixN::<f32, U18>::identity() - &kalman_gain * &jacobian) * self.covariance;
}
self.position += error_state.fixed_slice::<U3, U1>(0, 0);
self.velocity += error_state.fixed_slice::<U3, U1>(3, 0);
self.orientation *=
UnitQuaternion::from_scaled_axis(error_state.fixed_slice::<U3, U1>(6, 0));
self.accel_bias += error_state.fixed_slice::<U3, U1>(9, 0);
self.rot_bias += error_state.fixed_slice::<U3, U1>(12, 0);
self.gravity += error_state.fixed_slice::<U3, U1>(15, 0);
if cfg!(feature = "full-reset") {
let mut g = MatrixN::<f32, U18>::identity();
g.fixed_slice_mut::<U3, U3>(6, 6)
.sub_assign(0.5 * skew(&error_state.fixed_slice::<U3, U1>(6, 0).clone_owned()));
self.covariance = g * self.covariance * g.transpose();
}
Ok(())
}
pub fn observe_position_velocity2d(
&mut self,
position: Point3<f32>,
position_var: Matrix3<f32>,
velocity: Vector2<f32>,
velocity_var: Matrix2<f32>,
) -> Result<()> {
let mut jacobian = MatrixMN::<f32, U5, U18>::zeros();
jacobian
.fixed_slice_mut::<U5, U5>(0, 0)
.fill_with_identity();
let mut diff = VectorN::<f32, U5>::zeros();
diff.fixed_slice_mut::<U3, U1>(0, 0)
.copy_from(&(position - self.position));
diff.fixed_slice_mut::<U2, U1>(3, 0)
.copy_from(&(velocity - self.velocity.xy()));
let mut var = MatrixN::<f32, U5>::zeros();
var.fixed_slice_mut::<U3, U3>(0, 0).copy_from(&position_var);
var.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&velocity_var);
self.update(jacobian, diff, var)
}
pub fn observe_position(
&mut self,
measurement: Point3<f32>,
variance: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = MatrixMN::<f32, U3, U18>::zeros();
jacobian
.fixed_slice_mut::<U3, U3>(0, 0)
.fill_with_identity();
let diff = measurement - self.position;
self.update(jacobian, diff, variance)
}
pub fn observe_height(&mut self, measured: f32, variance: f32) -> Result<()> {
let mut jacobian = MatrixMN::<f32, U1, U18>::zeros();
jacobian
.fixed_slice_mut::<U1, U1>(0, 2)
.fill_with_identity();
let diff = VectorN::<f32, U1>::new(measured - self.position.z);
let var = MatrixMN::<f32, U1, U1>::new(variance);
self.update(jacobian, diff, var)
}
pub fn observe_velocity(
&mut self,
measurement: Vector3<f32>,
variance: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = MatrixMN::<f32, U3, U18>::zeros();
jacobian
.fixed_slice_mut::<U3, U3>(0, 3)
.fill_with_identity();
let diff = measurement - self.velocity;
self.update(jacobian, diff, variance)
}
pub fn observe_velocity2d(
&mut self,
measurement: Vector2<f32>,
variance: Matrix2<f32>,
) -> Result<()> {
let mut jacobian = MatrixMN::<f32, U2, U18>::zeros();
jacobian
.fixed_slice_mut::<U2, U2>(0, 3)
.fill_with_identity();
let diff = Vector2::new(
measurement.x - self.velocity.x,
measurement.y - self.velocity.y,
);
self.update(jacobian, diff, variance)
}
pub fn observe_orientation(
&mut self,
measurement: UnitQuaternion<f32>,
variance: Matrix3<f32>,
) -> Result<()> {
let mut jacobian = MatrixMN::<f32, U3, U18>::zeros();
jacobian
.fixed_slice_mut::<U3, U3>(0, 6)
.fill_with_identity();
let diff = measurement * self.orientation;
self.update(jacobian, diff.scaled_axis(), variance)
}
}
#[rustfmt::skip]
fn skew(v: &Vector3<f32>) -> Matrix3<f32> {
Matrix3::new(0., -v.z, v.y,
v.z, 0., -v.x,
-v.y, v.x, 0.)
}
#[cfg(test)]
mod test {
use super::Builder;
use approx::assert_relative_eq;
use nalgebra::{Point3, UnitQuaternion, Vector3};
use std::f32::consts::FRAC_PI_2;
use std::time::Duration;
#[test]
fn creation() {
let filter = Builder::new().build();
assert_relative_eq!(filter.position, Point3::origin());
assert_relative_eq!(filter.velocity, Vector3::zeros());
}
#[test]
fn linear_motion() {
let mut filter = Builder::new().build();
filter.predict(
Vector3::new(1.0, 0.0, -9.81),
Vector3::zeros(),
Duration::from_millis(1000),
);
assert_relative_eq!(filter.position, Point3::new(0.5, 0.0, 0.0));
assert_relative_eq!(filter.velocity, Vector3::new(1.0, 0.0, 0.0));
assert_relative_eq!(filter.orientation, UnitQuaternion::identity());
filter.predict(
Vector3::new(0.0, 0.0, -9.81),
Vector3::zeros(),
Duration::from_millis(500),
);
assert_relative_eq!(filter.position, Point3::new(1.0, 0.0, 0.0));
assert_relative_eq!(filter.velocity, Vector3::new(1.0, 0.0, 0.0));
assert_relative_eq!(filter.orientation, UnitQuaternion::identity());
filter.predict(
Vector3::new(-1.0, -1.0, -9.81),
Vector3::zeros(),
Duration::from_millis(1000),
);
assert_relative_eq!(filter.position, Point3::new(1.5, -0.5, 0.0));
assert_relative_eq!(filter.velocity, Vector3::new(0.0, -1.0, 0.0));
assert_relative_eq!(filter.orientation, UnitQuaternion::identity());
}
#[test]
fn rotational_motion() {
let mut filter = Builder::new().build();
filter.predict(
Vector3::zeros(),
Vector3::new(FRAC_PI_2, 0.0, 0.0),
Duration::from_millis(1000),
);
assert_relative_eq!(
filter.orientation,
UnitQuaternion::from_euler_angles(FRAC_PI_2, 0.0, 0.0)
);
filter.predict(
Vector3::zeros(),
Vector3::new(-FRAC_PI_2, 0.0, 0.0),
Duration::from_millis(1000),
);
assert_relative_eq!(
filter.orientation,
UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0)
);
let mut filter = Builder::new().build();
filter.predict(
Vector3::zeros(),
Vector3::new(0.0, -FRAC_PI_2, 0.0),
Duration::from_millis(1000),
);
assert_relative_eq!(
filter.orientation,
UnitQuaternion::from_euler_angles(0.0, -FRAC_PI_2, 0.0)
);
}
}