eskf 0.2.0

Navigation filter based on an Error State Kalman Filter (ESKF)
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109

//! Integration tests which uses data from the [CARLA simulator](https://carla.org/)
use approx::assert_relative_eq;
use eskf;
use nalgebra::{Point3, UnitQuaternion, Vector3};
use serde::Deserialize;
use serde_json;
use std::fs::File;
use std::io::BufReader;

#[derive(Deserialize, Debug)]
struct Variance {
    imu_acceleration: f32,
    imu_rotation: f32,
    gnss_position: f32,
}

#[derive(Deserialize, Debug)]
struct GroundTruth {
    acceleration: Vector3<f32>,
    velocity: Vector3<f32>,
    position: Point3<f32>,
    orientation_euler: Vector3<f32>,
}

#[derive(Deserialize, Debug)]
struct IMU {
    acceleration: Vector3<f32>,
    rotation: Vector3<f32>,
}

#[derive(Deserialize, Debug)]
struct GNSS {
    position: Option<Point3<f32>>,
}

#[derive(Deserialize, Debug)]
struct Measurement {
    time: f32,
    ground_truth: GroundTruth,
    imu: IMU,
    gnss: GNSS,
}

#[derive(Deserialize, Debug)]
struct Dataset {
    variance: Variance,
    data: Vec<Measurement>,
}

#[test]
fn dataset1() {
    // Read dataset from JSON
    let file =
        File::open("tests/carla_dataset1.json").expect("Could not open 'tests/carla_dataset1.json");
    let reader = BufReader::new(file);
    let dataset: Dataset = serde_json::from_reader(reader).expect("Could not parse JSON");
    let position_variance =
        eskf::ESKF::variance_from_element(dataset.variance.gnss_position.sqrt());
    // Create filter based on dataset
    let mut filter = eskf::Builder::new()
        .acceleration_variance(dataset.variance.imu_acceleration)
        .rotation_variance(dataset.variance.imu_rotation)
        .initial_covariance(1e-1)
        .build();
    // Insert a first measurement into the filter
    let m = &dataset.data[0];
    // Time delta is based on viewing the dataset and choosing a small value that could fit
    filter.predict(
        m.imu.acceleration,
        m.imu.rotation,
        std::time::Duration::from_millis(5),
    );
    // Iterate measurements and update filter
    let mut last_time = m.time;
    // There are > 8000 data points in dataset 1, unfortunately the data starts to drift after a
    // while, but this limited number should still be able to test the filter
    for m in dataset.data.iter().skip(1).take(4000) {
        let delta = std::time::Duration::from_secs_f32(m.time - last_time);
        last_time = m.time;

        filter.predict(m.imu.acceleration, m.imu.rotation, delta);
        if let Some(position) = m.gnss.position {
            filter
                .observe_position(position, position_variance)
                .expect("Position observation failed");
        }

        let gt_orient = UnitQuaternion::from_euler_angles(
            m.ground_truth.orientation_euler.x,
            m.ground_truth.orientation_euler.y,
            m.ground_truth.orientation_euler.z,
        );
        assert_relative_eq!(
            filter.position,
            m.ground_truth.position,
            epsilon = filter.position_uncertainty().norm()
        );
        assert_relative_eq!(
            filter.velocity,
            m.ground_truth.velocity,
            epsilon = filter.velocity_uncertainty().norm()
        );
        assert_relative_eq!(
            filter.orientation,
            gt_orient,
            epsilon = filter.orientation_uncertainty().norm()
        );
    }
}