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
110
111
112
113
114
115
116
117
118
119
120
121
122
extern crate std;
// The prelude is the one list of public types; the root adds back the four it leaves out.
pub use *;
pub use ConfigBound;
pub use Frame;
pub use STATES;
pub use Quaternion;
/// Everything needed to write an integration loop, in one import.
///
/// ```
/// use fusion_nav::prelude::*;
///
/// let mut filter = Eskf::default();
/// let dt = Seconds::from_secs(0.0025);
/// let at = |sample: u64| Timestamp::from_micros(2_500 * sample);
/// let gravity = Acceleration::body(0.0, 0.0, -GRAVITY);
/// let still = |sample| ImuSample::from_rates(at(sample), AngularRate::zero(), gravity, dt);
///
/// // The barometer in the window is what fixes the reference the fusion below is
/// // relative to, and its scatter is how well: one reading held across the window has
/// // none, and fixes no reference. Without one the first altitude is spent reading a
/// // reference from the estimate instead, and tests nothing.
/// let mut window = StaticWindow::new();
/// for i in 1..=800 {
/// window.push(StaticSample {
/// imu: still(i),
/// baro: Some(Altitude::from_meters(if i % 2 == 1 { 112.25 } else { 111.75 })),
/// ..StaticSample::default()
/// })?;
/// }
/// filter.initialize(&window)?;
/// assert_eq!(filter.predict(still(801)), Propagation::Propagated);
///
/// // An IMU that has stopped producing numbers is refused rather than propagated: a
/// // non-finite sample reaches the quaternion and then the covariance, and never leaves.
/// let spinning = AngularRate::body(f32::NAN, 0.0, 0.0);
/// let broken = ImuSample::from_rates(at(802), spinning, gravity, dt);
/// assert_eq!(filter.predict(broken), Propagation::NotFinite);
///
/// // Two meters above the reference the window fixed. An altitude far from what the
/// // filter expects is `Fusion::Rejected` instead — the gate of (37) runs on every
/// // measurement, and a 52 m step from a vehicle that has not moved does not pass it.
/// let outcome = filter.fuse_baro_altitude(
/// at(802),
/// Altitude::from_meters(114.0),
/// AltitudeNoise::from_sigma(2.0),
/// );
/// assert!(outcome.is_accepted());
///
/// // `R` describes that one measurement, so it travels with it — and it has to be a
/// // variance some sensor could have. Zero or negative is refused, not fused.
/// assert_eq!(
/// filter.fuse_baro_altitude(
/// at(802),
/// Altitude::from_meters(114.0),
/// AltitudeNoise::from_variance(0.0),
/// ),
/// Fusion::InvalidNoise,
/// );
/// # Ok::<(), InitError>(())
/// ```
///
/// Deliberately excluded, because their names are too generic to glob-import safely:
/// [`Frame`], [`STATES`] and [`Quaternion`], the last of which `nalgebra` and most algebra
/// crates also export. Import those by path, as
/// [`ConfigBound`], which a caller reads off a [`ConfigError`] far more often than it names.