pamoja_kit/odometry.rs
1//! Dead-reckoning a robot's pose from its motion.
2
3use crate::motion::{clamp, magnitude, wrap_pi, Pose};
4use crate::DiffDrive;
5use libm::{cosf, sinf};
6
7/// Tracks a robot's [`Pose`] by accumulating its motion over time (odometry).
8///
9/// With no GPS indoors, a robot estimates where it is by adding up where it has been: each small
10/// move is integrated onto the running pose. This uses the exact arc model rather than a straight-
11/// line step, so a robot that drives and turns at once follows the curve it actually traces
12/// instead of cutting the corner; over many steps that is markedly more accurate. Feed it either a
13/// body motion (forward speed and yaw rate over a time step) or wheel-distance deltas through a
14/// [`DiffDrive`] model. Dead reckoning drifts, so correct the heading from an absolute source with
15/// [`fuse_heading`](Odometry::fuse_heading) when one is available.
16///
17/// # Examples
18///
19/// ```
20/// use pamoja_kit::{Odometry, Pose};
21///
22/// // Drive a quarter circle of radius 1 m: forward 1 m/s, turning left at 1 rad/s, for pi/2 s.
23/// let mut odom = Odometry::at_origin();
24/// let pose = odom.integrate(1.0, 1.0, core::f32::consts::FRAC_PI_2);
25///
26/// // It ends about (1, 1) facing 90 degrees, the far corner of the arc.
27/// assert!((pose.x - 1.0).abs() < 1e-5);
28/// assert!((pose.y - 1.0).abs() < 1e-5);
29/// assert!((pose.theta - core::f32::consts::FRAC_PI_2).abs() < 1e-5);
30/// ```
31#[derive(Clone, Copy, Debug)]
32pub struct Odometry {
33 pose: Pose,
34}
35
36impl Odometry {
37 /// Creates an estimator starting from a known pose.
38 ///
39 /// # Arguments
40 ///
41 /// * `start` - the initial pose.
42 ///
43 /// # Returns
44 ///
45 /// The estimator.
46 pub fn new(start: Pose) -> Self {
47 Self { pose: start }
48 }
49
50 /// Creates an estimator starting at the origin facing along the x axis.
51 ///
52 /// # Returns
53 ///
54 /// The estimator.
55 pub fn at_origin() -> Self {
56 Self {
57 pose: Pose::origin(),
58 }
59 }
60
61 /// Returns the current pose estimate.
62 ///
63 /// # Returns
64 ///
65 /// The pose accumulated so far.
66 pub fn pose(&self) -> Pose {
67 self.pose
68 }
69
70 /// Resets the estimate to a known pose.
71 ///
72 /// # Arguments
73 ///
74 /// * `pose` - the pose to set.
75 pub fn reset(&mut self, pose: Pose) {
76 self.pose = pose;
77 }
78
79 /// Integrates a body motion over a time step and returns the new pose.
80 ///
81 /// A speed, rate, or time step that is not a finite number is ignored and the pose
82 /// returned as it stood, so one bad sample cannot poison the pose for good.
83 ///
84 /// # Arguments
85 ///
86 /// * `linear` - the forward speed.
87 /// * `angular` - the yaw rate, positive turning left.
88 /// * `dt` - the length of the time step.
89 ///
90 /// # Returns
91 ///
92 /// The updated pose.
93 pub fn integrate(&mut self, linear: f32, angular: f32, dt: f32) -> Pose {
94 self.advance(linear * dt, angular * dt);
95 self.pose
96 }
97
98 /// Integrates wheel-distance deltas through a differential-drive model.
99 ///
100 /// A distance that is not a finite number is ignored and the pose returned as it stood.
101 ///
102 /// # Arguments
103 ///
104 /// * `left` - the distance the left wheel rolled since the last update.
105 /// * `right` - the distance the right wheel rolled since the last update.
106 /// * `drive` - the [`DiffDrive`] model giving the track between the wheels.
107 ///
108 /// # Returns
109 ///
110 /// The updated pose. The wheel deltas are turned into a forward distance and a heading
111 /// change by [`DiffDrive::body_motion`], then integrated as one arc.
112 pub fn integrate_wheels(&mut self, left: f32, right: f32, drive: &DiffDrive) -> Pose {
113 let (distance, heading_change) = drive.body_motion(left, right);
114 self.advance(distance, heading_change);
115 self.pose
116 }
117
118 /// Corrects the heading toward an absolute measurement, the way a compass tames gyro drift.
119 ///
120 /// This is the angular cousin of [`Complementary`](crate::Complementary): it nudges the
121 /// estimated heading along the shortest arc toward an absolute reading (an IMU yaw, a
122 /// magnetometer, a GPS course) by a blend weight, leaving the position untouched.
123 ///
124 /// # Arguments
125 ///
126 /// * `measured` - the absolute heading in the pose's own frame: radians counter-clockwise
127 /// from the world x axis. A compass course, clockwise from north, is not in that frame;
128 /// with the world x axis pointing east it converts as `FRAC_PI_2 - course`.
129 /// * `weight` - how strongly to trust the measurement, clamped to `[0, 1]`; zero keeps the
130 /// dead-reckoned heading, one snaps to `measured`.
131 ///
132 /// A measurement that is not a finite number, or a weight that is not a number, leaves the
133 /// heading as it was.
134 pub fn fuse_heading(&mut self, measured: f32, weight: f32) {
135 if !measured.is_finite() || weight.is_nan() {
136 return;
137 }
138 let w = clamp(weight, 0.0, 1.0);
139 let error = wrap_pi(measured - self.pose.theta);
140 self.pose.theta = wrap_pi(self.pose.theta + w * error);
141 }
142
143 // Advances the pose by an arc of forward distance `distance` and heading change
144 // `heading_change`, using the exact integration for a constant-curvature segment.
145 fn advance(&mut self, distance: f32, heading_change: f32) {
146 if !distance.is_finite() || !heading_change.is_finite() {
147 return;
148 }
149 let theta = self.pose.theta;
150 if magnitude(heading_change) < 1e-6 {
151 self.pose.x += distance * cosf(theta);
152 self.pose.y += distance * sinf(theta);
153 self.pose.theta = wrap_pi(theta + heading_change);
154 } else {
155 let radius = distance / heading_change;
156 let new_theta = theta + heading_change;
157 self.pose.x += radius * (sinf(new_theta) - sinf(theta));
158 self.pose.y += radius * (cosf(theta) - cosf(new_theta));
159 self.pose.theta = wrap_pi(new_theta);
160 }
161 }
162}
163
164#[cfg(test)]
165mod tests {
166 use super::*;
167 use core::f32::consts::{FRAC_PI_2, PI};
168
169 #[test]
170 fn driving_straight_moves_along_the_heading() {
171 let mut odom = Odometry::new(Pose::new(0.0, 0.0, FRAC_PI_2)); // facing +y
172 let pose = odom.integrate(2.0, 0.0, 1.0);
173 assert!(pose.x.abs() < 1e-5);
174 assert!((pose.y - 2.0).abs() < 1e-5);
175 assert!((pose.theta - FRAC_PI_2).abs() < 1e-6);
176 }
177
178 #[test]
179 fn a_quarter_circle_lands_at_the_arc_corner() {
180 let mut odom = Odometry::at_origin();
181 let pose = odom.integrate(1.0, 1.0, FRAC_PI_2);
182 assert!((pose.x - 1.0).abs() < 1e-5);
183 assert!((pose.y - 1.0).abs() < 1e-5);
184 assert!((pose.theta - FRAC_PI_2).abs() < 1e-5);
185 }
186
187 #[test]
188 fn wheel_deltas_match_a_spin_in_place() {
189 let drive = DiffDrive::new(0.5);
190 let mut odom = Odometry::at_origin();
191 // Equal and opposite wheel deltas: turn in place, no translation.
192 let pose = odom.integrate_wheels(-0.25, 0.25, &drive);
193 assert!(pose.x.abs() < 1e-6 && pose.y.abs() < 1e-6);
194 assert!((pose.theta - 1.0).abs() < 1e-6); // (0.25 - -0.25) / 0.5 = 1 rad
195 }
196
197 #[test]
198 fn fuse_heading_blends_along_the_shortest_arc() {
199 let mut odom = Odometry::new(Pose::new(0.0, 0.0, 0.1));
200 odom.fuse_heading(0.5, 0.5); // halfway from 0.1 toward 0.5
201 assert!((odom.pose().theta - 0.3).abs() < 1e-6);
202 }
203
204 #[test]
205 fn fuse_heading_takes_the_short_way_across_pi() {
206 let mut odom = Odometry::new(Pose::new(0.0, 0.0, 3.0));
207 // Measured just past pi: the shortest arc wraps through pi, not the long way back.
208 odom.fuse_heading(-3.0, 1.0);
209 assert!((odom.pose().theta - -3.0).abs() < 1e-6);
210 assert!(odom.pose().theta.abs() <= PI);
211 }
212
213 #[test]
214 fn a_sample_that_is_not_a_number_leaves_the_pose_as_it_was() {
215 let mut odometry = Odometry::at_origin();
216 let moved = odometry.integrate(1.0, 0.0, 1.0);
217 assert_eq!(odometry.integrate(f32::NAN, 0.0, 1.0), moved);
218 assert_eq!(odometry.integrate(1.0, 0.0, f32::INFINITY), moved);
219 assert_eq!(
220 odometry.integrate_wheels(f32::NAN, 1.0, &DiffDrive::new(0.5)),
221 moved
222 );
223 odometry.fuse_heading(f32::NAN, 1.0);
224 odometry.fuse_heading(1.0, f32::NAN);
225 assert_eq!(odometry.pose(), moved);
226 }
227}