From f7b15dcaf935bb56ab2222d1b2e63cffdb3e0e90 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 18:48:45 +0000 Subject: [PATCH 1/5] build: bump numerical-toolbox for ResidualKalmanUpdate (NT74) Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- CMakeLists.txt | 2 +- ROADMAP.md | 56 +++++++++++++++++++++++++------------------------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 28fda9e..a4c4770 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,7 @@ if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG dc147fb8672e983e5935290b94cc45ff00cbee29 # main + GIT_TAG c1a669bd94ebad3a7922dbc3865af0e02a5905f4 # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/ROADMAP.md b/ROADMAP.md index 2adbcc7..f31b626 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -40,33 +40,33 @@ this roadmap needs that upstream does not ship yet are specified and built upstr Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT** plus its master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a -numerical-toolbox pin that ships it. NT53–NT66 have shipped upstream, and their links point to the upstream -docs; the current pin (`dc147fb`) includes them. - -| # | Component | Upstream module | Spec / doc | Needed by | -|------|-------------------------------------------------------|----------------------|-------------------------------------|----------------------------------------------------------------------------| -| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 | -| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, E1, E3, S1 | -| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 | -| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 | -| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) | -| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 | -| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 | -| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) | -| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 | -| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14, H5 | -| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 | -| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 | -| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 | -| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] | F1, W1–W10, M24, E1, E2, E3, E5 | -| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] (spec) | E5, C1 | -| NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) | -| NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 | -| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [ParticleFilter][nt70] (spec) | E4 | -| NT71 | Error-state EKF on Lie groups | `filters/active` | [master list][ntrm], no spec yet | L5 | -| NT72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` | [master list][ntrm], no spec yet | A7 | -| NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 | -| NT74 | Residual-form Kalman update with χ² gating | `filters/active` | [ResidualKalmanUpdate][nt74] (spec) | E1, E3 | +numerical-toolbox pin that ships it. NT53–NT66 and NT74 have shipped upstream, and their links point to the +upstream docs; the current pin (`c1a669b`) includes them. + +| # | Component | Upstream module | Spec / doc | Needed by | +|------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------| +| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 | +| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, E1, E3, S1 | +| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 | +| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 | +| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) | +| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 | +| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 | +| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) | +| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 | +| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14, H5 | +| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 | +| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 | +| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 | +| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] | F1, W1–W10, M24, E1, E2, E3, E5 | +| NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] (spec) | E5, C1 | +| NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) | +| NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 | +| NT70 | Particle filter (bounded, systematic resampling) | `filters/active` | [ParticleFilter][nt70] (spec) | E4 | +| NT71 | Error-state EKF on Lie groups | `filters/active` | [master list][ntrm], no spec yet | L5 | +| NT72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` | [master list][ntrm], no spec yet | A7 | +| NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 | +| NT74 | Residual-form Kalman update with χ² gating | `filters/active` | [ResidualKalmanUpdate][nt74] | E1, E3 | [ntrm]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/ROADMAP.md#master-list-by-priority [nt53]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md @@ -85,7 +85,7 @@ docs; the current pin (`dc147fb`) includes them. [nt66]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE2Transform.md [nt67]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/PointSetAlignment/implementation.md [nt70]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ParticleFilter/implementation.md -[nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ResidualKalmanUpdate/implementation.md +[nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/filters/active/ResidualKalmanUpdate.md ## Robot manipulators & other manipulator types From f4d8d37e35e1fb2aba2b4a92e7385f1c024d83f1 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 18:49:01 +0000 Subject: [PATCH 2/5] feat(localization): add OdometryFusion (E1) Wheel and gyroscope EKF on SE(2) with state (x, y, heading, gyro bias): the mean follows the exact SE(2) arc, the covariance uses midpoint Jacobians with speed-scaled wheel noise and a bias random walk, and the wheel yaw-rate, heading and position updates go through the upstream residual-form Kalman update, so slip is caught by the chi-square gate and heading residuals are wrapped. The roadmap canonical example now points at MonteCarloLocalization. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 5 - doc/localization/OdometryFusion.md | 94 +++++++++++ doc/localization/README.md | 1 + roadmap/README.md | 4 +- .../OdometryFusion/explanation.md | 32 ---- .../OdometryFusion/implementation.md | 100 ------------ roadmap/localization/OdometryFusion/tests.md | 50 ------ robotics/localization/CMakeLists.txt | 3 + robotics/localization/OdometryFusion.cpp | 6 + robotics/localization/OdometryFusion.hpp | 152 ++++++++++++++++++ robotics/localization/test/CMakeLists.txt | 1 + .../localization/test/TestOdometryFusion.cpp | 112 +++++++++++++ 13 files changed, 372 insertions(+), 190 deletions(-) create mode 100644 doc/localization/OdometryFusion.md delete mode 100644 roadmap/localization/OdometryFusion/explanation.md delete mode 100644 roadmap/localization/OdometryFusion/implementation.md delete mode 100644 roadmap/localization/OdometryFusion/tests.md create mode 100644 robotics/localization/OdometryFusion.cpp create mode 100644 robotics/localization/OdometryFusion.hpp create mode 100644 robotics/localization/test/TestOdometryFusion.cpp diff --git a/README.md b/README.md index 6a59ef7..6a6654a 100644 --- a/README.md +++ b/README.md @@ -42,7 +42,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect, Dubins Shortest Paths, Dynamic Window Approach, Reeds–Shepp Shortest Paths | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | | [Mobile Robots](doc/mobile/README.md) | Planar Odometry, Differential-Drive Kinematics, Car-Like Kinematics, Omnidirectional Kinematics, Skid-Steer Kinematics, Pure-Pursuit Path Tracking, Kanayama Trajectory Tracking, Stanley Path Tracking | -| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping | +| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion | | [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | Each category page lists its algorithms with a brief description and links to the detailed diff --git a/ROADMAP.md b/ROADMAP.md index f31b626..c3e28ff 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -290,15 +290,10 @@ All planning items have shipped: see [Planning](doc/planning/README.md). | # | Component | Target module | Depends on | Difficulty | |----|--------------------------------------------------------|----------------|------------------|------------| -| E1 | Wheel + gyro odometry fusion (EKF on SE(2)) | `localization` | F1, NT66, NT74 | ★★★☆☆ | | E3 | EKF landmark localization (known map, range–bearing) | `localization` | F1, NT66, NT74 | ★★★☆☆ | | E4 | Monte Carlo localization | `localization` | NT70, E2, F1 | ★★★★☆ | | E5 | 2-D scan matching (point-to-point / point-to-line ICP) | `localization` | NT67, NT64, NT66 | ★★★★☆ | -**E1. Odometry fusion.** EKF on `(x, y, θ, gyro bias)`: wheel speed and gyro rate drive the prediction, the wheel yaw rate is a χ²-gated bias measurement (a rejected update flags slip), and optional heading and position fixes share the same update. -- *Algorithm / paper:* S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 5 and 7; J. Borenstein, L. Feng, "Gyrodometry," *ICRA* 1996. -- *Reuses / builds on:* F1, NT66, NT74 (upstream `ExtendedKalmanFilter` cannot wrap residuals or gate). - **E3. EKF landmark localization.** Pose tracking against a known landmark map with range–bearing measurements and gated data association. - *Algorithm / paper:* Thrun et al. (2005), Ch. 7. - *Reuses / builds on:* F1 (odometry noise), NT66, NT74. diff --git a/doc/localization/OdometryFusion.md b/doc/localization/OdometryFusion.md new file mode 100644 index 0000000..14daf0a --- /dev/null +++ b/doc/localization/OdometryFusion.md @@ -0,0 +1,94 @@ +# Odometry Fusion (Wheel and Gyroscope EKF on SE(2)) + +## Overview & Motivation + +Wheel odometry measures distance well but heading poorly: a few millimetres of wheel-radius error or a little slip turn into large heading drift. A MEMS gyroscope measures rotation well but has a bias that drifts with temperature, and integrating that bias produces a steadily growing heading error. + +This filter combines the two in the way each sensor is good at: **the gyroscope for rotation, the wheels for distance**. It estimates the gyroscope bias from the agreement between the wheel and gyroscope yaw rates, and refuses that comparison when the wheels slip. Optional absolute heading (compass, dual-antenna GNSS) and position (GNSS, scan matching) measurements go through the same update path. + +## Mathematical Theory + +### State and Motion Model + +The state is the planar pose and the gyroscope bias, $x = (p_x, p_y, \theta, b_g)$. Given the forward speed $v$ from a wheel model and the raw gyroscope rate $\omega_g$, the corrected yaw rate is $\omega = \omega_g - b_g$. The mean moves along the exact SE(2) arc: + +$$T_{k+1} = T_k \exp\!\big((v, 0, \omega)\,\Delta t\big), \qquad b_{g,k+1} = b_{g,k}$$ + +### Covariance Propagation + +The Jacobians use the midpoint heading $\theta_m = \theta + \omega\Delta t/2$. With respect to the state: + +$$F = \begin{pmatrix} 1 & 0 & -v\Delta t\sin\theta_m & \tfrac{1}{2}v\Delta t^2\sin\theta_m \\ 0 & 1 & v\Delta t\cos\theta_m & -\tfrac{1}{2}v\Delta t^2\cos\theta_m \\ 0 & 0 & 1 & -\Delta t \\ 0 & 0 & 0 & 1 \end{pmatrix}$$ + +The inputs $(v, \omega_g)$ enter through $G = \partial f / \partial(v, \omega_g)$. The speed noise grows with speed, $\sigma_v = \sigma_{v,0} + k_v |v|$, and the bias follows a random walk: + +$$P \leftarrow F P F^T + G\,\mathrm{diag}(\sigma_v^2, \sigma_g^2)\,G^T + \mathrm{diag}(0, 0, 0, \sigma_b^2 \Delta t)$$ + +### Measurement Updates + +Every update is a residual-form Kalman update with a Joseph-form covariance and an optional χ² gate. The heading is re-wrapped afterwards. + +| Update | Residual | Jacobian $H$ | Noise | Gate | +|-------------------|------------------------------------------|-----------------------|---------------------------|-----------| +| Wheel yaw rate | $\omega_w - (\omega_g - b_g)$ | $(0, 0, 0, -1)$ | $\sigma_w^2 + \sigma_g^2$ | slip gate | +| Absolute heading | $\operatorname{wrap}(\theta_m - \theta)$ | $(0, 0, 1, 0)$ | $\sigma_\theta^2$ | none | +| Absolute position | $p_m - (p_x, p_y)$ | first two rows of $I$ | $\Sigma_p$ | none | + +Both yaw rates measure the same true rotation, so their difference observes the bias. The gyroscope sample appears in the predicted rate, which is why its variance is added to the wheel noise. + +### Slip Detection + +When the wheels slip, for example spinning on a smooth floor or with a stuck caster, the wheel yaw rate disagrees with the gyroscope far beyond its noise. The normalised innovation squared then exceeds the χ²(1) gate. The update is rejected, the state is untouched, and the returned status lets the caller flag the slip. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------------|--------|--------|---------------------------------------------------------| +| Predict | $O(1)$ | $O(1)$ | One SE(2) exponential and a $4 \times 4$ congruence | +| Scalar update | $O(1)$ | $O(1)$ | One $1 \times 1$ Cholesky and a $4 \times 1$ gain | +| Position update | $O(1)$ | $O(1)$ | One $2 \times 2$ Cholesky and a $4 \times 2$ gain | +| State | — | $O(1)$ | 20 scalars of state and covariance plus the noise block | + +## Step-by-Step Walkthrough + +Noise floor $0.01$ m/s, speed gain $5$ %, gyroscope noise $0.01$ rad/s, bias random walk $10^{-4}$, wheel yaw-rate noise $0.02$ rad/s, slip gate $6.63$ (99 %), starting at the origin with variances $(0.01, 0.01, 0.01, 0.0025)$: + +1. **Straight drive**: one metre at $1$ m/s. The along-track variance barely grows, but the cross-track variance doubles to $0.0206$ through the heading uncertainty, which itself grows to $0.0125$ from the bias. +2. **Arc**: $1$ m/s and $0.5$ rad/s for $2$ s end at $(1.683, 0.919, 1.0)$, exactly the SE(2) exponential of the twist. +3. **Standstill**: a gyroscope with a $0.02$ rad/s bias, compared with the zero wheel yaw rate at $100$ Hz, gives a bias estimate of $0.019992$ after $5$ s. The heading error stays below $10^{-4}$ rad instead of drifting to $0.1$ rad. +4. **Slip**: the wheels report $1$ rad/s while the gyroscope reads zero. The NIS is $333$, far beyond the gate, so the update is rejected. +5. **Heading across $\pm\pi$**: an estimate of $3.1$ rad corrected by a compass reading of $-3.1$ rad moves through the seam to $-3.117$ rad. +6. **Position fix**: a GNSS fix at $(0.1, -0.05)$ with the same variance as the estimate pulls the position halfway and halves its variance, with NIS $0.625$. + +## Pitfalls & Edge Cases + +- **Bias observability**: the bias is only observable through the wheel comparison or absolute heading. Long stretches of slip leave it unobserved, and its variance grows with the random walk. +- **Gate locking**: after a long slip the filter may disagree with the wheels for a while. Monitor the gating rate, and raise the speed noise gain while slipping. +- **Wheel model errors**: a miscalibrated track width biases the wheel yaw rate, which the filter then reads as gyroscope bias. Calibrate the wheel model first. +- **Lateral slip**: the motion model has no lateral velocity. Skid-steer platforms turning hard violate it; an accelerometer test of $a_y \approx v\omega$ can flag that. +- **Time alignment**: the wheel and gyroscope samples must refer to the same interval, or the bias estimate absorbs the delay. + +## Variants & Generalizations + +- **Error-state filters**: propagate the error on the SE(2) manifold for large uncertainties. +- **Accelerometer aiding**: lateral-slip detection and pitch compensation on slopes. +- **Wheel calibration states**: estimating wheel radii and track width online. +- **Smoothing**: fixed-lag smoothers for post-processing logged runs. + +## Applications + +- Dead reckoning for indoor service robots and warehouse vehicles. +- Heading stabilisation for agricultural and outdoor robots between GNSS fixes. +- The motion model for landmark-based and Monte Carlo localization. + +## Connections to Other Algorithms + +- [Planar Odometry](../mobile/PlanarOdometry.md): the exact SE(2) arc integration used for the mean. +- [Differential-Drive Kinematics](../mobile/DifferentialDrive.md) and [Skid-Steer Kinematics](../mobile/SkidSteerKinematics.md): provide the wheel speed and yaw rate. +- The residual-form Kalman update, SE(2) transform and angle wrapping come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Borenstein, J. and Feng, L. (1996). "Gyrodometry: a new method for combining data from gyros and odometry in mobile robots." *IEEE International Conference on Robotics and Automation*, 423–428. +- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapters 3 and 5. +- Barfoot, T.D. (2017). *State Estimation for Robotics*. Cambridge University Press. Chapter 4. diff --git a/doc/localization/README.md b/doc/localization/README.md index 9dd0b5a..4a8ac2b 100644 --- a/doc/localization/README.md +++ b/doc/localization/README.md @@ -7,3 +7,4 @@ Maps, state estimation and localization for mobile robots moving in the plane. | Algorithm | Description | |-----------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------| | [Occupancy-Grid Mapping](OccupancyGridMap.md) | Bounded grid of clamped log-odds cells updated from range scans along integer Bresenham rays, with max-range clearing — $O(\sum \rho / r)$ | +| [Odometry Fusion](OdometryFusion.md) | Wheel and gyroscope EKF on SE(2) with gyro-bias estimation, χ²-gated slip detection and optional heading and position updates — $O(1)$ | diff --git a/roadmap/README.md b/roadmap/README.md index 5361f5c..b9b6bdf 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -23,7 +23,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[localization/OdometryFusion](localization/OdometryFusion/implementation.md). +[localization/MonteCarloLocalization](localization/MonteCarloLocalization/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md) (NT53), and planar @@ -92,4 +92,4 @@ prerequisites. `TimeOptimalPathParameterization` (M27) ### `localization` -`OdometryFusion` (E1) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) +`LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) diff --git a/roadmap/localization/OdometryFusion/explanation.md b/roadmap/localization/OdometryFusion/explanation.md deleted file mode 100644 index de1f807..0000000 --- a/roadmap/localization/OdometryFusion/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Wheel + Gyro Odometry Fusion — Overview - -## What it is -A small Kalman filter that combines a wheeled robot's wheel encoders with a yaw-rate gyro. It gives a -better pose estimate than either alone, learns the gyro's bias as it goes, and notices when the wheels -are slipping. - -## Why it matters (embedded) -Wheel odometry is good at distance but bad at heading: slip and uneven floors twist the estimated path. -A MEMS gyro is good at short-term heading but drifts with its bias. Fusing them on the robot's -microcontroller, at the control rate and with a 4-element state, gives a dead-reckoning estimate that -holds its heading far longer. It also feeds localization (E3, E4) a better motion estimate. - -## How it works (intuition) -Every cycle the robot moves forward by the wheel-measured speed and turns by the gyro rate minus the -current bias estimate, along an exact arc. The uncertainty grows with distance and with the gyro noise. -The wheels also report a turn rate; when it agrees with the bias-corrected gyro, the difference refines -the bias, which is easiest at standstill. When the two disagree beyond what noise can explain, the wheels -are slipping, and the comparison is skipped and reported. Absolute heading or position fixes, when -available, correct the pose through the same update. - -## Key parameters -- **speed noise** (floor plus a fraction of speed), **gyro noise**, **bias random walk**. -- **wheel yaw-rate noise** and the **slip gate** (χ² threshold). - -## Reference -S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 3 and 5; J. Borenstein, L. Feng, -"Gyrodometry: a new method for combining data from gyros and odometry in mobile robots," *ICRA* 1996. - -## See also -`PlanarOdometry` (F1), `DifferentialDrive` (W1), `SkidSteerKinematics` (W4); upstream `ResidualKalmanUpdate` -(NT74) and `SE2Transform` (NT66). diff --git a/roadmap/localization/OdometryFusion/implementation.md b/roadmap/localization/OdometryFusion/implementation.md deleted file mode 100644 index 9255453..0000000 --- a/roadmap/localization/OdometryFusion/implementation.md +++ /dev/null @@ -1,100 +0,0 @@ -# Wheel + Gyro Odometry Fusion (EKF on SE(2)) — Implementation Pseudocode - -> Roadmap ref: #E1 (★★★☆☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) - -Dead reckoning that trusts the gyro for rotation and the wheels for distance. It estimates the gyro -bias from the agreement between wheel and gyro yaw rates, and refuses that comparison when the wheels -slip. State `(x, y, θ, b_g)`: planar pose plus gyro bias. The mean is propagated with the exact SE(2) -arc (NT66, as in F1). Every measurement update goes through upstream `ResidualKalmanUpdate` (NT74), so -heading residuals are wrapped and slip shows up as a χ²-gated wheel update. Optional absolute heading -(compass, dual-antenna GNSS) and position (GNSS, scan matching E5) updates share the same path. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct OdometryFusionNoise: - T speedNoiseFloor # σ_v at standstill, m/s - T speedNoiseGain # σ_v grows by this fraction of |v| - T gyroNoise # σ_g, rad/s per sample - T biasRandomWalk # σ_b, rad/s/√s - T wheelYawRateNoise # σ_w, rad/s (wheel-derived yaw rate) - T slipGate # χ²(1) gate for the wheel update, e.g. 6.63 (99 %) - -template -class OdometryFusion: - math::Vector state # (x, y, θ, b_g), θ wrapped to (−π, π] - math::SquareMatrix covariance - OdometryFusionNoise noise -``` - -## Interface - -```cpp -OdometryFusion(const math::SE2Transform& pose, const math::SquareMatrix& covariance, const OdometryFusionNoise& noise) -void Predict(T speed, T gyroRate, T dt) # hot path -filters::KalmanUpdateResult UpdateWheelYawRate(T wheelYawRate, T gyroRate) # gated: status gated ⇒ slip -filters::KalmanUpdateResult UpdateHeading(T heading, T sigma) -filters::KalmanUpdateResult UpdatePosition(const math::Vector2& position, const math::SquareMatrix& covariance) -math::SE2Transform Pose() const; T GyroBias() const -const math::Vector& State() const; const math::SquareMatrix& Covariance() const -``` - -`speed` is the forward speed from a wheel model (W1 `Forward`, W4), `gyroRate` the raw z-gyro, and -`wheelYawRate` the wheel model's `ω`. - -## Algorithm (pseudocode) - -```text -function Predict(v, ω_g, dt): # OPTIMIZE_FOR_SPEED - ω = ω_g − b; θm = θ + ω·dt/2 # midpoint heading for the Jacobians - pose = (Pose() * SE2Transform::Exp((v, 0, ω), dt)) # exact arc (NT66) - F = I₄ with F[0,2] = −v·dt·sin θm, F[0,3] = v·dt²/2·sin θm, - F[1,2] = v·dt·cos θm, F[1,3] = −v·dt²/2·cos θm, F[2,3] = −dt - G = [[dt·cos θm, −v·dt²/2·sin θm], [dt·sin θm, v·dt²/2·cos θm], [0, dt], [0, 0]] # ∂f/∂(v, ω_g) - σ_v = speedNoiseFloor + speedNoiseGain·|v| - Q = G·diag(σ_v², σ_g²)·Gᵀ + diag(0, 0, 0, σ_b²·dt) - P = CongruenceTransform(F, P) + Q; state = (pose.x, pose.y, pose.Heading(), b) - -function UpdateWheelYawRate(ω_w, ω_g): # both rates measure the true ω; the difference observes b - r = (ω_w − (ω_g − b)); H = [0, 0, 0, −1]; R = σ_w² + σ_g² - return Apply(state, P, r, H, R, slipGate); wrap θ # NT74; gated ⇒ wheels slipping, nothing applied - -function UpdateHeading(θ_m, σ): - r = AngleDifference(θ_m, θ); H = [0, 0, 1, 0]; return Apply(state, P, r, H, σ², ∞); wrap θ - -function UpdatePosition(p, Σ): - r = p − (x, y); H = [[1, 0, 0, 0], [0, 1, 0, 0]]; return Apply(state, P, r, H, Σ, ∞); wrap θ -``` - -## Complexity & memory - -- `Predict`: one `Exp`, one 4×4 congruence and a 4×2 noise map. Updates: NT74 with `M ≤ 2`. -- Memory: 20 scalars of state plus the noise block. No heap. - -## Numerical / embedded notes - -- **Why a bias state.** An uncorrected 0.02 rad/s gyro bias turns 5 s at standstill into a 0.1 rad heading - error. With the wheel update the estimated bias reaches 0.019992 and the heading error stays below - `4e-5` (test). Standstill, where the wheel yaw rate is exactly 0, is the strongest bias observation. -- **Slip awareness.** During slip (spinning on carpet, a stuck caster) the wheel yaw rate disagrees with - the gyro far beyond `σ_w`. The gate rejects the update, and the returned status lets the caller flag - slip or raise `speedNoiseGain` for the following predicts. -- **Linearisation.** The mean uses the exact arc; the Jacobians use the midpoint heading, whose difference - from the exact arc's Jacobian is `O(v·ω²·dt³)`. -- `R = σ_w² + σ_g²` in the wheel update because the gyro sample enters the predicted rate. -- An accelerometer can later add a lateral-slip test (`a_y ≈ v·ω`). It is left out to keep the state at 4. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/localization/OdometryFusion.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Predict`, - and `extern template class OdometryFusion;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/localization/OdometryFusion.cpp` → `template class OdometryFusion;` -- Test: `robotics/localization/test/TestOdometryFusion.cpp` -- Doc: `doc/localization/OdometryFusion.md` (per `doc/TEMPLATE.md`) -- CMake: new module `robotics/localization` (`robotics.localization`, test target - `robotics.localization_test`), created by the first `localization` item deployed. -- Depends on: F1 (motion integration convention), NT66 (`SE2Transform`), NT54 (`AngleDifference`, `WrapToPi`), - NT74 (`ResidualKalmanUpdate`); upstream `MatrixOperations::CongruenceTransform`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/OdometryFusion/tests.md b/roadmap/localization/OdometryFusion/tests.md deleted file mode 100644 index d4cda47..0000000 --- a/roadmap/localization/OdometryFusion/tests.md +++ /dev/null @@ -1,50 +0,0 @@ -# Odometry Fusion — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestOdometryFusion : public ::testing::Test: - OdometryFusionNoise noise{ 0.01, 0.05, 0.01, 1e-4, 0.02, 6.63 } - SquareMatrix P0{ diag(0.01, 0.01, 0.01, 0.0025) } - OdometryFusion fusion{ Identity, P0, noise } -# each case below is a TEST_F(TestOdometryFusion, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -straight_drive_grows_the_covariance_along_and_across: - Act: 10 × Predict(1, 0, 0.1) - Assert: State() ≈ (1, 0, 0, 0); diag(P) ≈ (0.010360, 0.020628, 0.012510, 0.0025); P(1,2) ≈ 0.011255 -arc_uses_the_exact_se2_exponential: - Act: 20 × Predict(1, 0.5, 0.1) - Assert: Pose().Pose() ≈ (1.682942, 0.919395, 1.0) == Exp((1, 0, 0.5), 2) -standstill_estimates_the_gyro_bias: - Act: 500 × { Predict(0, 0.02, 0.01); UpdateWheelYawRate(0, 0.02) } - Assert: GyroBias() ≈ 0.019992 (tol 1e-4); |θ| < 1e-4; P(3,3) ≈ 1.02e-6 - # after the first update the bias is already 0.016667 -slip_is_gated_and_leaves_the_state_unchanged: - Arrange: Predict(1, 0, 0.1) - Act: result = UpdateWheelYawRate(1.0, 0.0) - Assert: result.status == gated, nis ≈ 333.33; State() and Covariance() unchanged -heading_update_wraps_the_residual: - Arrange: fusion at FromPose(0, 0, 3.1) - Act: UpdateHeading(−3.1, 0.05) - Assert: status applied; θ ≈ −3.116637 # K = 0.8 on the wrapped residual 0.083185 -position_update_pulls_the_pose_and_shrinks_the_covariance: - Act: UpdatePosition((0.1, −0.05), diag(0.01, 0.01)) - Assert: State() ≈ (0.05, −0.025, 0, 0); P(0,0) ≈ P(1,1) ≈ 0.005; nis ≈ 0.625 -``` - -## Reference vectors - -- Gyro bias 0.02 rad/s for 5 s without wheel updates ⇒ heading 0.1 rad; with them ⇒ `4e-5` rad. -- Straight drive (v = 1, dt = 0.1, 10 steps): the cross-track variance doubles (`0.01 → 0.020628`) through - the heading uncertainty. - -## Edge cases - -- `v = 0` and `ω_g − b = 0` ⇒ `F = I` except the bias column; covariance grows only by `Q`. -- A singular update (all-zero covariance and noise) returns `singular` and changes nothing. diff --git a/robotics/localization/CMakeLists.txt b/robotics/localization/CMakeLists.txt index 7731246..26705f0 100644 --- a/robotics/localization/CMakeLists.txt +++ b/robotics/localization/CMakeLists.txt @@ -7,15 +7,18 @@ target_include_directories(robotics.localization ${ROBOTICS_VISIBILITY} target_link_libraries(robotics.localization ${ROBOTICS_VISIBILITY} infra.util + numerical.filters.active numerical.math ) target_sources(robotics.localization PRIVATE OccupancyGridMap.hpp + OdometryFusion.hpp ) robotics_add_coverage_sources(robotics.localization OccupancyGridMap.cpp + OdometryFusion.cpp ) add_subdirectory(test) diff --git a/robotics/localization/OdometryFusion.cpp b/robotics/localization/OdometryFusion.cpp new file mode 100644 index 0000000..77af146 --- /dev/null +++ b/robotics/localization/OdometryFusion.cpp @@ -0,0 +1,6 @@ +#include "robotics/localization/OdometryFusion.hpp" + +namespace localization +{ + template class OdometryFusion; +} diff --git a/robotics/localization/OdometryFusion.hpp b/robotics/localization/OdometryFusion.hpp new file mode 100644 index 0000000..f1edb2a --- /dev/null +++ b/robotics/localization/OdometryFusion.hpp @@ -0,0 +1,152 @@ +#pragma once + +#include "numerical/filters/active/ResidualKalmanUpdate.hpp" +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/MatrixOperations.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include +#include + +namespace localization +{ + template + struct OdometryFusionNoise + { + static_assert(std::is_floating_point_v, "OdometryFusionNoise supports floating-point types"); + + T speedNoiseFloor{}; + T speedNoiseGain{}; + T gyroNoise{}; + T biasRandomWalk{}; + T wheelYawRateNoise{}; + T slipGate{}; + }; + + template + class OdometryFusion + { + static_assert(std::is_floating_point_v, "OdometryFusion supports floating-point types"); + + public: + using StateVector = math::Vector; + using CovarianceMatrix = math::SquareMatrix; + + OdometryFusion(const math::SE2Transform& pose, const CovarianceMatrix& covariance, const OdometryFusionNoise& noise); + + OPTIMIZE_FOR_SPEED void Predict(T speed, T gyroRate, T dt); + filters::KalmanUpdateResult UpdateWheelYawRate(T wheelYawRate, T gyroRate); + filters::KalmanUpdateResult UpdateHeading(T heading, T sigma); + filters::KalmanUpdateResult UpdatePosition(const math::Vector2& position, const math::SquareMatrix& covariance); + + math::SE2Transform Pose() const; + T GyroBias() const; + const StateVector& State() const; + const CovarianceMatrix& Covariance() const; + + private: + template + filters::KalmanUpdateResult Correct(const math::Vector& residual, const math::Matrix& jacobian, + const math::SquareMatrix& measurementNoise, T gate); + + StateVector state; + CovarianceMatrix covariance; + OdometryFusionNoise noise; + }; + + template + OdometryFusion::OdometryFusion(const math::SE2Transform& pose, const CovarianceMatrix& covariance, const OdometryFusionNoise& noise) + : state{ pose.translation[0], pose.translation[1], pose.Heading(), T{} } + , covariance{ covariance } + , noise{ noise } + {} + + template + OPTIMIZE_FOR_SPEED void OdometryFusion::Predict(T speed, T gyroRate, T dt) + { + const T rate{ gyroRate - state[3] }; + const T midpoint{ state[2] + rate * dt / T{ 2 } }; + const T cosine{ math::Cos(midpoint) }; + const T sine{ math::Sin(midpoint) }; + const T arc{ speed * dt }; + const T lever{ speed * dt * dt / T{ 2 } }; + const auto pose{ Pose() * math::SE2Transform::Exp(math::Vector3{ speed, T{}, rate }, dt) }; + + auto transition{ CovarianceMatrix::Identity() }; + transition.at(0, 2) = -arc * sine; + transition.at(0, 3) = lever * sine; + transition.at(1, 2) = arc * cosine; + transition.at(1, 3) = -lever * cosine; + transition.at(2, 3) = -dt; + + const math::Matrix inputMap{ { dt * cosine, -lever * sine }, { dt * sine, lever * cosine }, { T{}, dt }, { T{}, T{} } }; + const T speedSigma{ noise.speedNoiseFloor + noise.speedNoiseGain * math::Abs(speed) }; + const math::SquareMatrix inputNoise{ { speedSigma * speedSigma, T{} }, { T{}, noise.gyroNoise * noise.gyroNoise } }; + + covariance = math::CongruenceTransform(transition, covariance) + math::CongruenceTransform(inputMap, inputNoise); + covariance.at(3, 3) += noise.biasRandomWalk * noise.biasRandomWalk * dt; + state = StateVector{ pose.translation[0], pose.translation[1], pose.Heading(), state[3] }; + } + + template + filters::KalmanUpdateResult OdometryFusion::UpdateWheelYawRate(T wheelYawRate, T gyroRate) + { + const T variance{ noise.wheelYawRateNoise * noise.wheelYawRateNoise + noise.gyroNoise * noise.gyroNoise }; + return Correct<1>(math::Vector{ wheelYawRate - (gyroRate - state[3]) }, math::Matrix{ { T{}, T{}, T{}, T{ -1 } } }, math::SquareMatrix{ { variance } }, noise.slipGate); + } + + template + filters::KalmanUpdateResult OdometryFusion::UpdateHeading(T heading, T sigma) + { + return Correct<1>(math::Vector{ math::AngleDifference(heading, state[2]) }, math::Matrix{ { T{}, T{}, T{ 1 }, T{} } }, math::SquareMatrix{ { sigma * sigma } }, + std::numeric_limits::infinity()); + } + + template + filters::KalmanUpdateResult OdometryFusion::UpdatePosition(const math::Vector2& position, const math::SquareMatrix& positionCovariance) + { + const math::Matrix jacobian{ { T{ 1 }, T{}, T{}, T{} }, { T{}, T{ 1 }, T{}, T{} } }; + return Correct<2>(math::Vector{ position[0] - state[0], position[1] - state[1] }, jacobian, positionCovariance, std::numeric_limits::infinity()); + } + + template + math::SE2Transform OdometryFusion::Pose() const + { + return math::SE2Transform::FromPose(state[0], state[1], state[2]); + } + + template + T OdometryFusion::GyroBias() const + { + return state[3]; + } + + template + auto OdometryFusion::State() const -> const StateVector& + { + return state; + } + + template + auto OdometryFusion::Covariance() const -> const CovarianceMatrix& + { + return covariance; + } + + template + template + filters::KalmanUpdateResult OdometryFusion::Correct(const math::Vector& residual, const math::Matrix& jacobian, + const math::SquareMatrix& measurementNoise, T gate) + { + const auto result{ filters::ResidualKalmanUpdate::Apply(state, covariance, residual, jacobian, measurementNoise, gate) }; + state[2] = math::WrapToPi(state[2]); + return result; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class OdometryFusion; +#endif +} diff --git a/robotics/localization/test/CMakeLists.txt b/robotics/localization/test/CMakeLists.txt index 12ab4f3..3c19853 100644 --- a/robotics/localization/test/CMakeLists.txt +++ b/robotics/localization/test/CMakeLists.txt @@ -9,4 +9,5 @@ target_link_libraries(robotics.localization_test PUBLIC target_sources(robotics.localization_test PRIVATE TestOccupancyGridMap.cpp + TestOdometryFusion.cpp ) diff --git a/robotics/localization/test/TestOdometryFusion.cpp b/robotics/localization/test/TestOdometryFusion.cpp new file mode 100644 index 0000000..dba8655 --- /dev/null +++ b/robotics/localization/test/TestOdometryFusion.cpp @@ -0,0 +1,112 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/localization/OdometryFusion.hpp" +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Fusion = localization::OdometryFusion; + using filters::KalmanUpdateStatus; + + class TestOdometryFusion + : public ::testing::Test + { + protected: + static void ExpectState(const Fusion::StateVector& actual, float x, float y, float heading, float bias) + { + EXPECT_NEAR(actual[0], x, 1e-5f); + EXPECT_NEAR(actual[1], y, 1e-5f); + EXPECT_NEAR(actual[2], heading, 1e-5f); + EXPECT_NEAR(actual[3], bias, 1e-5f); + } + + static void ExpectEqual(const Fusion::CovarianceMatrix& actual, const Fusion::CovarianceMatrix& expected) + { + for (std::size_t r = 0; r < 4; ++r) + for (std::size_t c = 0; c < 4; ++c) + EXPECT_EQ(actual.at(r, c), expected.at(r, c)); + } + + localization::OdometryFusionNoise noise{ 0.01f, 0.05f, 0.01f, 1e-4f, 0.02f, 6.63f }; + Fusion::CovarianceMatrix initial{ { 0.01f, 0.0f, 0.0f, 0.0f }, { 0.0f, 0.01f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.01f, 0.0f }, { 0.0f, 0.0f, 0.0f, 0.0025f } }; + Fusion fusion{ Transform::Identity(), initial, noise }; + }; +} + +TEST_F(TestOdometryFusion, straight_drive_grows_the_covariance_along_and_across) +{ + for (std::size_t k = 0; k < 10; ++k) + fusion.Predict(1.0f, 0.0f, 0.1f); + + ExpectState(fusion.State(), 1.0f, 0.0f, 0.0f, 0.0f); + EXPECT_NEAR(fusion.Covariance().at(0, 0), 0.010360f, 1e-6f); + EXPECT_NEAR(fusion.Covariance().at(1, 1), 0.020628f, 1e-6f); + EXPECT_NEAR(fusion.Covariance().at(2, 2), 0.012510f, 1e-6f); + EXPECT_NEAR(fusion.Covariance().at(3, 3), 0.0025f, 1e-6f); + EXPECT_NEAR(fusion.Covariance().at(1, 2), 0.011255f, 1e-6f); +} + +TEST_F(TestOdometryFusion, arc_uses_the_exact_se2_exponential) +{ + for (std::size_t k = 0; k < 20; ++k) + fusion.Predict(1.0f, 0.5f, 0.1f); + + const auto pose{ fusion.Pose().Pose() }; + const auto exact{ Transform::Exp(math::Vector3{ 1.0f, 0.0f, 0.5f }, 2.0f).Pose() }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(pose[i], exact[i], 1e-5f); + EXPECT_NEAR(pose[0], 1.682942f, 1e-5f); + EXPECT_NEAR(pose[1], 0.919395f, 1e-5f); + EXPECT_NEAR(pose[2], 1.0f, 1e-5f); +} + +TEST_F(TestOdometryFusion, standstill_estimates_the_gyro_bias) +{ + for (std::size_t k = 0; k < 500; ++k) + { + fusion.Predict(0.0f, 0.02f, 0.01f); + fusion.UpdateWheelYawRate(0.0f, 0.02f); + } + + EXPECT_NEAR(fusion.GyroBias(), 0.019992f, 1e-4f); + EXPECT_LT(std::abs(fusion.State()[2]), 1e-4f); + EXPECT_NEAR(fusion.Covariance().at(3, 3), 1.02e-6f, 2e-8f); +} + +TEST_F(TestOdometryFusion, slip_is_gated_and_leaves_the_state_unchanged) +{ + fusion.Predict(1.0f, 0.0f, 0.1f); + const auto state{ fusion.State() }; + const auto covariance{ fusion.Covariance() }; + + const auto result{ fusion.UpdateWheelYawRate(1.0f, 0.0f) }; + + EXPECT_EQ(result.status, KalmanUpdateStatus::Gated); + EXPECT_NEAR(result.nis, 333.33f, 0.01f); + ExpectState(fusion.State(), state[0], state[1], state[2], state[3]); + ExpectEqual(fusion.Covariance(), covariance); +} + +TEST_F(TestOdometryFusion, heading_update_wraps_the_residual) +{ + Fusion reversed{ Transform::FromPose(0.0f, 0.0f, 3.1f), initial, noise }; + + const auto result{ reversed.UpdateHeading(-3.1f, 0.05f) }; + + EXPECT_EQ(result.status, KalmanUpdateStatus::Applied); + EXPECT_NEAR(reversed.State()[2], -3.116637f, 1e-5f); +} + +TEST_F(TestOdometryFusion, position_update_pulls_the_pose_and_shrinks_the_covariance) +{ + const math::SquareMatrix gnss{ { 0.01f, 0.0f }, { 0.0f, 0.01f } }; + + const auto result{ fusion.UpdatePosition(math::Vector2{ 0.1f, -0.05f }, gnss) }; + + ExpectState(fusion.State(), 0.05f, -0.025f, 0.0f, 0.0f); + EXPECT_NEAR(fusion.Covariance().at(0, 0), 0.005f, 1e-6f); + EXPECT_NEAR(fusion.Covariance().at(1, 1), 0.005f, 1e-6f); + EXPECT_NEAR(result.nis, 0.625f, 1e-5f); +} From 3b64372c5fcc6b61c79e59f022aa2ce8f63ac1ec Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 18:52:06 +0000 Subject: [PATCH 3/5] feat(localization): add LandmarkLocalization (E3) Range-bearing EKF against a known landmark map: body-frame odometry increments composed onto the pose with the F1 increment noise rotated into the world, observations matched by identity or by minimum Mahalanobis distance, chi-square gated and applied through the upstream residual-form Kalman update with the bearing residual wrapped. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 4 +- ROADMAP.md | 5 - doc/localization/LandmarkLocalization.md | 92 +++++++ doc/localization/README.md | 9 +- roadmap/README.md | 2 +- .../LandmarkLocalization/explanation.md | 33 --- .../LandmarkLocalization/implementation.md | 113 --------- .../LandmarkLocalization/tests.md | 53 ---- robotics/localization/CMakeLists.txt | 3 + .../localization/LandmarkLocalization.cpp | 7 + .../localization/LandmarkLocalization.hpp | 230 ++++++++++++++++++ robotics/localization/test/CMakeLists.txt | 1 + .../test/TestLandmarkLocalization.cpp | 139 +++++++++++ 13 files changed, 480 insertions(+), 211 deletions(-) create mode 100644 doc/localization/LandmarkLocalization.md delete mode 100644 roadmap/localization/LandmarkLocalization/explanation.md delete mode 100644 roadmap/localization/LandmarkLocalization/implementation.md delete mode 100644 roadmap/localization/LandmarkLocalization/tests.md create mode 100644 robotics/localization/LandmarkLocalization.cpp create mode 100644 robotics/localization/LandmarkLocalization.hpp create mode 100644 robotics/localization/test/TestLandmarkLocalization.cpp diff --git a/README.md b/README.md index 6a6654a..5a63f33 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ## Overview This library provides robotics algorithms — today robot-manipulator kinematics, rigid-body dynamics, -trajectory generation, manipulator control, motion planning, wheeled mobile robots and occupancy-grid +trajectory generation, manipulator control, motion planning, wheeled mobile robots, localization and mapping; aerial, legged and marine robots are on the [roadmap](ROADMAP.md) — designed for robust and efficient real-time use on resource-constrained embedded systems (no heap, deterministic, float-only). It builds on the shared numerical primitives (linear algebra, solvers, controllers) provided by @@ -42,7 +42,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect, Dubins Shortest Paths, Dynamic Window Approach, Reeds–Shepp Shortest Paths | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | | [Mobile Robots](doc/mobile/README.md) | Planar Odometry, Differential-Drive Kinematics, Car-Like Kinematics, Omnidirectional Kinematics, Skid-Steer Kinematics, Pure-Pursuit Path Tracking, Kanayama Trajectory Tracking, Stanley Path Tracking | -| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion | +| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion, EKF Landmark Localization | | [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | Each category page lists its algorithms with a brief description and links to the detailed diff --git a/ROADMAP.md b/ROADMAP.md index c3e28ff..ade1512 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -290,14 +290,9 @@ All planning items have shipped: see [Planning](doc/planning/README.md). | # | Component | Target module | Depends on | Difficulty | |----|--------------------------------------------------------|----------------|------------------|------------| -| E3 | EKF landmark localization (known map, range–bearing) | `localization` | F1, NT66, NT74 | ★★★☆☆ | | E4 | Monte Carlo localization | `localization` | NT70, E2, F1 | ★★★★☆ | | E5 | 2-D scan matching (point-to-point / point-to-line ICP) | `localization` | NT67, NT64, NT66 | ★★★★☆ | -**E3. EKF landmark localization.** Pose tracking against a known landmark map with range–bearing measurements and gated data association. -- *Algorithm / paper:* Thrun et al. (2005), Ch. 7. -- *Reuses / builds on:* F1 (odometry noise), NT66, NT74. - **E4. Monte Carlo localization.** Particle-filter localization in an occupancy grid with the odometry motion model and a likelihood-field sensor model (two-pass chamfer distance map); samplers injected by the caller. - *Algorithm / paper:* F. Dellaert, D. Fox, W. Burgard, S. Thrun, *ICRA* 1999. - *Reuses / builds on:* NT70, E2, F1. diff --git a/doc/localization/LandmarkLocalization.md b/doc/localization/LandmarkLocalization.md new file mode 100644 index 0000000..47c72c8 --- /dev/null +++ b/doc/localization/LandmarkLocalization.md @@ -0,0 +1,92 @@ +# EKF Landmark Localization (Known Map, Range–Bearing) + +## Overview & Motivation + +Odometry drifts without bound. To keep a robot localised over hours, its pose must be corrected against fixed features of the environment: reflectors on warehouse walls, fiducial markers, poles or trees. +When the positions of these **landmarks** are known and a sensor (laser scanner, camera, radar) measures their **range and bearing**, an extended Kalman filter tracks the pose accurately at very low cost. + +Each motion step composes the odometry increment with noise that grows with distance and rotation. Each observation is matched to a landmark, either by a known identity or by the smallest Mahalanobis distance, then checked against a χ² gate and applied with the bearing residual wrapped. Matching is also called data association, and the gate is what rejects spurious detections. + +## Mathematical Theory + +### Motion Step + +The state is the pose $x = (p_x, p_y, \theta)$. The odometry increment $\Delta = (d_x, d_y, d_\theta)$ in the body frame is composed onto the pose, $T \leftarrow T\,\Delta$. Its Jacobians with respect to the pose and to the increment are + +$$F = \begin{pmatrix} 1 & 0 & -d_x\sin\theta - d_y\cos\theta \\ 0 & 1 & d_x\cos\theta - d_y\sin\theta \\ 0 & 0 & 1 \end{pmatrix}, \qquad J = \begin{pmatrix} R(\theta) & 0 \\ 0 & 1 \end{pmatrix}$$ + +The increment noise $\Sigma_\Delta = \mathrm{diag}(\sigma_{xy}^2, \sigma_{xy}^2, \sigma_\theta^2)$ comes from the odometry noise model, with deviations that grow with the distance and rotation travelled. Then + +$$P \leftarrow F P F^T + J\,\Sigma_\Delta\,J^T$$ + +The noise is added in the body frame and rotated into the world, so forward and reverse motion are treated alike. + +### Range–Bearing Measurement + +For a landmark at $m$, with $d = m - (p_x, p_y)$ and $q = d^T d$: + +$$\hat{z} = \begin{pmatrix} \sqrt{q} \\ \operatorname{atan2}(d_y, d_x) - \theta \end{pmatrix}, \qquad H = \begin{pmatrix} -d_x/\sqrt{q} & -d_y/\sqrt{q} & 0 \\ d_y/q & -d_x/q & -1 \end{pmatrix}$$ + +The residual is $r = (z_r - \hat{z}_r,\; \operatorname{wrap}(z_b - \hat{z}_b))$, with noise $R = \mathrm{diag}(\sigma_r^2, \sigma_b^2)$. + +### Data Association and Gating + +A labelled observation (a fiducial with an identity) uses its landmark directly. An unlabelled one is matched to the landmark with the smallest normalised innovation squared, + +$$\text{NIS}_k = r_k^T \left(H_k P H_k^T + R\right)^{-1} r_k$$ + +and then gated against the χ²(2) quantile, for example $9.21$ at 99 %. The update uses the Joseph form, and observations taken at the same time are applied one after another. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------|--------|--------|-------------------------------------------------------------| +| Predict | $O(1)$ | $O(1)$ | Two $3 \times 3$ congruences | +| Labelled update | $O(L)$ | $O(1)$ | Identity search over $L$ landmarks, one $2 \times 2$ update | +| Unlabelled update | $O(L)$ | $O(1)$ | One NIS per landmark, each a $2 \times 2$ Cholesky solve | +| Map and state | — | $O(L)$ | Three scalars per landmark and 12 for the state | + +## Step-by-Step Walkthrough + +Four landmarks at $(5, 0)$, $(0, 5)$, $(5, 5)$ and $(-3, 2)$. The estimate starts at $(1, 1, 0.3)$ with variances $(0.05, 0.05, 0.01)$, while the true pose is $(1.1, 0.9, 0.32)$. The noise is $\sigma_r = 0.1$ m and $\sigma_b = 0.02$ rad, with a 99 % gate of $9.21$: + +1. **Predict**: an increment of $0.5$ m forward and $0.1$ rad moves the estimate to $(1.478, 1.148, 0.4)$ and grows the variances to $(0.0628, 0.0649, 0.013)$. +2. **Reverse motion**: from zero covariance, $0.5$ m backwards gives the same covariance as forwards, $(0.0125, 0.0125, 0.0025)$. +3. **Association**: an unlabelled observation of landmark 2 has NIS $0.002$ against it, against $119.6$, $118.1$ and $371.8$ for the others. It is matched to landmark 2 and moves the estimate to $(1.002, 0.996, 0.296)$. +4. **Convergence**: four labelled observations bring the estimate to $(1.093, 0.904, 0.319)$, close to the truth, and shrink the variances to $(0.0027, 0.0021, 0.00012)$. +5. **Outlier**: a detection at range $0.5$ and bearing $2.5$ is closest to landmark 3 with NIS $219$. It is gated, and the estimate is unchanged. +6. **Bearing seam**: a robot at $(6, 4.9)$ sees landmark 1 almost straight behind, at bearing $3.125$, and measures $-\pi + 0.01$. The wrapped residual is $0.027$ rad instead of $-6.26$, so the correction stays small (NIS $0.06$). + +## Pitfalls & Edge Cases + +- **Ambiguous association**: two landmarks inside the gate at once can be swapped. Prefer labelled observations, or space landmarks wider than the 3σ position uncertainty. +- **Bearing wrap**: an unwrapped bearing residual near $\pm\pi$ throws the filter off by a full turn. Always wrap. +- **Large initial uncertainty**: the linearisation fails when the heading is unknown. Initialise from a global method, such as Monte Carlo localization, first. +- **Unknown landmarks**: an identity missing from the map, or an empty map, returns a singular result and leaves the state unchanged. +- **Overconfidence**: too little motion noise makes the gate reject valid observations after a disturbance. Monitor the gating rate. + +## Variants & Generalizations + +- **EKF SLAM**: adding the landmark positions to the state to build the map while localising. +- **Unscented and particle filters**: for strongly nonlinear measurements or multimodal pose beliefs. +- **Joint compatibility association**: testing sets of matches together instead of one at a time. +- **Bearing-only and range-only variants**: for cameras without depth or radio beacons. + +## Applications + +- Automated guided vehicles navigating with wall-mounted reflectors. +- Robots localising with fiducial markers such as AprilTags. +- Agricultural and forestry robots using trees or poles as landmarks. + +## Connections to Other Algorithms + +- [Odometry Fusion](OdometryFusion.md): the wheel and gyroscope filter that can supply the odometry increments. +- [Planar Odometry](../mobile/PlanarOdometry.md): the increment convention and the odometry noise model. +- [Occupancy-Grid Mapping](OccupancyGridMap.md): a dense map representation, as opposed to sparse landmarks. +- The residual-form Kalman update, SE(2) transform and angle wrapping come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Leonard, J.J. and Durrant-Whyte, H.F. (1991). "Mobile robot localization by tracking geometric beacons." *IEEE Transactions on Robotics and Automation*, 7(3), 376–382. +- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapter 7. +- Bar-Shalom, Y., Li, X.-R. and Kirubarajan, T. (2001). *Estimation with Applications to Tracking and Navigation*. Wiley. Chapter 5. diff --git a/doc/localization/README.md b/doc/localization/README.md index 4a8ac2b..2d73569 100644 --- a/doc/localization/README.md +++ b/doc/localization/README.md @@ -4,7 +4,8 @@ Maps, state estimation and localization for mobile robots moving in the plane. ## Algorithms -| Algorithm | Description | -|-----------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------| -| [Occupancy-Grid Mapping](OccupancyGridMap.md) | Bounded grid of clamped log-odds cells updated from range scans along integer Bresenham rays, with max-range clearing — $O(\sum \rho / r)$ | -| [Odometry Fusion](OdometryFusion.md) | Wheel and gyroscope EKF on SE(2) with gyro-bias estimation, χ²-gated slip detection and optional heading and position updates — $O(1)$ | +| Algorithm | Description | +|------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------| +| [Occupancy-Grid Mapping](OccupancyGridMap.md) | Bounded grid of clamped log-odds cells updated from range scans along integer Bresenham rays, with max-range clearing — $O(\sum \rho / r)$ | +| [Odometry Fusion](OdometryFusion.md) | Wheel and gyroscope EKF on SE(2) with gyro-bias estimation, χ²-gated slip detection and optional heading and position updates — $O(1)$ | +| [EKF Landmark Localization](LandmarkLocalization.md) | Range–bearing EKF against a known landmark map, with body-frame odometry noise, Mahalanobis association, χ² gating and wrapped bearings — $O(L)$ | diff --git a/roadmap/README.md b/roadmap/README.md index b9b6bdf..937cd16 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,4 +92,4 @@ prerequisites. `TimeOptimalPathParameterization` (M27) ### `localization` -`LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) +`MonteCarloLocalization` (E4) · `ScanMatcher` (E5) diff --git a/roadmap/localization/LandmarkLocalization/explanation.md b/roadmap/localization/LandmarkLocalization/explanation.md deleted file mode 100644 index 03780db..0000000 --- a/roadmap/localization/LandmarkLocalization/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# EKF Landmark Localization — Overview - -## What it is -A Kalman-filter localizer for a robot that knows where certain landmarks are — reflective posts, -fiducial markers, poles — and measures their distance and direction. It keeps a pose estimate and its -uncertainty, moves it with odometry, and corrects it with every landmark sighting. - -## Why it matters (embedded) -Many industrial AGVs, lawn robots and warehouse vehicles navigate this way: a laser picks out -reflectors, or a camera picks out markers, at surveyed positions. With a 3-element state the filter is -tiny, so it runs at sensor rate on a microcontroller and gives a consistent uncertainty for safety checks -and planners. - -## How it works (intuition) -Between sightings the pose moves with the odometry increment, and the uncertainty grows in proportion to -how far and how much the robot turned. When a landmark is seen, the filter predicts where it should -appear given the current estimate. The difference between seen and predicted range and bearing corrects -the pose, weighted by how uncertain each is, and the bearing difference is taken the short way round the -circle. If a sighting carries no identity, the landmark it most plausibly belongs to is chosen; sightings -too far from every landmark are rejected as clutter. - -## Key parameters -- **motion noise** — growth of position and heading uncertainty with distance and rotation. -- **range and bearing noise** of the sensor. -- **gate** — how implausible a sighting may be before it is rejected. - -## Reference -S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 7; J. J. Leonard, H. F. Durrant-Whyte, -"Mobile robot localization by tracking geometric beacons," *IEEE T-RA*, 7(3), 1991. - -## See also -`PlanarOdometry` (F1, the increment), `OdometryFusion` (E1); upstream `ResidualKalmanUpdate` (NT74), -`SE2Transform` (NT66). diff --git a/roadmap/localization/LandmarkLocalization/implementation.md b/roadmap/localization/LandmarkLocalization/implementation.md deleted file mode 100644 index 3152db5..0000000 --- a/roadmap/localization/LandmarkLocalization/implementation.md +++ /dev/null @@ -1,113 +0,0 @@ -# EKF Landmark Localization (Known Map, Range–Bearing) — Implementation Pseudocode - -> Roadmap ref: #E3 (★★★☆☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) - -Pose tracking against a known map of point landmarks (reflectors, fiducials, poles) seen as range and -bearing. The motion step composes the odometry increment (F1) with noise proportional to translation -and rotation. Each observation is either labelled with a landmark id or associated by minimum -Mahalanobis distance. It is χ²-gated and applied with upstream `ResidualKalmanUpdate` (NT74), with the -bearing residual wrapped. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct Landmark: - math::Vector2 position # world frame - std::uint16_t id - -template -struct RangeBearing: - T range, bearing # sensor = robot frame - std::optional id # known identity (fiducial) or nullopt (association needed) - -template -struct LandmarkLocalizationNoise: - mobile::OdometryNoise motion # F1: α₁…α₄ - T rangeNoise, bearingNoise # σ_r (m), σ_b (rad) - T gate # χ²(2), e.g. 9.21 (99 %) - -template -struct LandmarkUpdate: - filters::KalmanUpdateResult result - std::optional landmark # index used (nullopt if none in the map) - -template -class LandmarkLocalization: - std::array, MaxLandmarks> map; std::size_t count - math::Vector3 state # (x, y, θ) - math::SquareMatrix covariance - LandmarkLocalizationNoise noise -``` - -## Interface - -```cpp -LandmarkLocalization(const math::SE2Transform& pose, const math::SquareMatrix& covariance, const LandmarkLocalizationNoise& noise) -bool SetMap(std::span> landmarks) # false if more than MaxLandmarks -void Predict(const math::SE2Transform& increment) # body-frame motion since the last call; hot path -LandmarkUpdate Update(const RangeBearing& observation) # hot path -std::optional Nis(const RangeBearing& observation, std::size_t landmark) const -math::SE2Transform Pose() const; const math::SquareMatrix& Covariance() const -``` - -`increment` is `previousOdometry.Inverse() * currentOdometry`, i.e. `Exp(F1::Increment(...), 1)`. - -## Algorithm (pseudocode) - -```text -function Predict(Δ): # OPTIMIZE_FOR_SPEED - (dx, dy, dθ) = Δ.Pose(); c, s = cos θ, sin θ; Δs² = dx² + dy² - F = [[1, 0, −s·dx − c·dy], [0, 1, c·dx − s·dy], [0, 0, 1]] # ∂(pose ⊕ Δ)/∂pose - J = blockdiag(R(θ), 1) # ∂(pose ⊕ Δ)/∂Δ - σ = mobile::IncrementDeviation(Δ, motion) # F1: (σ_xy, σ_xy, σ_θ) - P = CongruenceTransform(F, P) + CongruenceTransform(J, diag(σ²)) - state = (Pose() * Δ).Pose() - -function Expected(m): # predicted measurement and Jacobian for landmark m - d = m.position − (x, y); q = d·d; ρ = √q - ẑ = (ρ, AngleDifference(atan2(d.y, d.x), θ)) - H = [[−d.x/ρ, −d.y/ρ, 0], [d.y/q, −d.x/q, −1]] - return ẑ, H - -function Residual(z, m): (ẑ, H) = Expected(m); return ((z.range − ẑ.range, AngleDifference(z.bearing, ẑ.bearing)), H) - -function Update(z): - k = z.id ? index of the landmark with that id : argmin over map of Nis(z, k) # nearest neighbour in Mahalanobis distance - if no such landmark: return { singular, nullopt } - (r, H) = Residual(z, map[k]); R = diag(σ_r², σ_b²) - result = ResidualKalmanUpdate::Apply(state, P, r, H, R, gate); θ = WrapToPi(θ) - return { result, k } -``` - -## Complexity & memory - -- `Predict`: two 3×3 congruences. `Update`: `O(MaxLandmarks)` NIS evaluations (2×2 Cholesky each) for an - unlabelled observation, then one NT74 update. -- Memory: `3·MaxLandmarks` scalars of map plus 12 of state. No heap. - -## Numerical / embedded notes - -- **Increment noise in the body frame.** F1's `IncrementDeviation` adds the noise to the body-frame - increment, which `J` rotates into the world. Forward and backward motion are treated alike (test: a - reverse step gives the same covariance as a forward one), unlike the rotate–translate–rotate split. -- **Bearing wrap.** A landmark straight behind the robot sits on the ±π seam. The wrapped residual is - 0.026665 where the raw difference is −6.256520; the test checks the update stays small. -- **Association.** Nearest neighbour by NIS with a gate is the classic low-cost choice. Where two - landmarks are inside the gate together, prefer labelled observations, or space landmarks wider than - the 3σ position uncertainty. -- Observations taken at one time are applied one after another; each update refines `P` for the next. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/localization/LandmarkLocalization.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `Predict`/`Update`, and `extern template class LandmarkLocalization;` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/localization/LandmarkLocalization.cpp` → `template class LandmarkLocalization;` -- Test: `robotics/localization/test/TestLandmarkLocalization.cpp` -- Doc: `doc/localization/LandmarkLocalization.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.localization ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.localization_test`. -- Depends on: F1 (`OdometryNoise`, `IncrementDeviation`, increment convention), NT66, NT54, NT74 (`ResidualKalmanUpdate`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/LandmarkLocalization/tests.md b/roadmap/localization/LandmarkLocalization/tests.md deleted file mode 100644 index 74fd688..0000000 --- a/roadmap/localization/LandmarkLocalization/tests.md +++ /dev/null @@ -1,53 +0,0 @@ -# EKF Landmark Localization — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestLandmarkLocalization : public ::testing::Test: - std::array, 4> landmarks{ { (5, 0), 0 }, { (0, 5), 1 }, { (5, 5), 2 }, { (−3, 2), 3 } } - LandmarkLocalizationNoise noise{ { 0.05, 0.01, 0.05, 0.01 }, 0.1, 0.02, 9.21 } - LandmarkLocalization filter{ FromPose(1, 1, 0.3), diag(0.05, 0.05, 0.01), noise } # SetMap(landmarks) - Vector3 truth{ 1.1, 0.9, 0.32 } - # Measure(truth, k) = exact range and bearing of landmark k from the true pose -# each case below is a TEST_F(TestLandmarkLocalization, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -predict_composes_the_increment_and_grows_the_covariance: - Act: Predict(FromPose(0.5, 0, 0.1)) - Assert: Pose().Pose() ≈ (1.477668, 1.147760, 0.4); diag(P) ≈ (0.062818, 0.064882, 0.013) - P(0,1) ≈ −0.000706; P(1,2) ≈ 0.004777 -reverse_motion_has_the_same_noise_as_forward: - Arrange: filter at Identity with P = 0 - Act: Predict(FromPose(−0.5, 0, 0)) - Assert: Pose().Pose() ≈ (−0.5, 0, 0); diag(P) ≈ (0.0125, 0.0125, 0.0025) -unlabelled_observation_is_associated_by_mahalanobis_distance: - Act: Update({ Measure(truth, 2), nullopt }) - Assert: landmark == 2; status applied, nis ≈ 0.002138; Pose().Pose() ≈ (1.001568, 0.996349, 0.295825) - # NIS against landmarks 0, 1, 3: 119.6, 118.1, 371.8 -labelled_observations_converge_towards_the_truth: - Act: Update({ Measure(truth, 2), 2 }), then { 0 }, { 1 }, { 3 } - Assert: Pose().Pose() ≈ (1.092522, 0.904252, 0.318948); diag(P) ≈ (0.002722, 0.002077, 0.000122) -outlier_is_gated: - Act: Update({ (0.5, 2.5), nullopt }) - Assert: status gated (best nis ≈ 219.48 against landmark 3); pose and covariance unchanged -bearing_residual_is_wrapped_across_pi: - Arrange: filter at FromPose(6, 4.9, 0), P = diag(0.05, 0.05, 0.01); landmark 1 at bearing +3.124928 - Act: Update({ (6.0, −π + 0.01), 1 }) - Assert: status applied, nis ≈ 0.060327; Pose().Pose() ≈ (5.999620, 4.918856, −0.022620) -``` - -## Reference vectors - -- From `(1, 1, 0.3)`: landmark 2 predicted at `(5.656854, 0.485398)`; measured from the truth at - `(5.658622, 0.490393)`. The Jacobian is `[[−0.707107, −0.707107, 0], [0.125, −0.125, −1]]`. -- The wrapped bearing residual is `0.026665`; unwrapped it would be `−6.256520`. - -## Edge cases - -- Empty map ⇒ `Update` returns `{ singular, nullopt }`. -- An `id` that is not in the map ⇒ `{ singular, nullopt }`, state unchanged. diff --git a/robotics/localization/CMakeLists.txt b/robotics/localization/CMakeLists.txt index 26705f0..3873cdf 100644 --- a/robotics/localization/CMakeLists.txt +++ b/robotics/localization/CMakeLists.txt @@ -9,14 +9,17 @@ target_link_libraries(robotics.localization ${ROBOTICS_VISIBILITY} infra.util numerical.filters.active numerical.math + robotics.mobile ) target_sources(robotics.localization PRIVATE + LandmarkLocalization.hpp OccupancyGridMap.hpp OdometryFusion.hpp ) robotics_add_coverage_sources(robotics.localization + LandmarkLocalization.cpp OccupancyGridMap.cpp OdometryFusion.cpp ) diff --git a/robotics/localization/LandmarkLocalization.cpp b/robotics/localization/LandmarkLocalization.cpp new file mode 100644 index 0000000..ff29f9f --- /dev/null +++ b/robotics/localization/LandmarkLocalization.cpp @@ -0,0 +1,7 @@ +#include "robotics/localization/LandmarkLocalization.hpp" + +namespace localization +{ + template class LandmarkLocalization; + template class LandmarkLocalization; +} diff --git a/robotics/localization/LandmarkLocalization.hpp b/robotics/localization/LandmarkLocalization.hpp new file mode 100644 index 0000000..65cdb09 --- /dev/null +++ b/robotics/localization/LandmarkLocalization.hpp @@ -0,0 +1,230 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/filters/active/ResidualKalmanUpdate.hpp" +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/MatrixOperations.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/PlanarOdometry.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace localization +{ + template + struct Landmark + { + static_assert(std::is_floating_point_v, "Landmark supports floating-point types"); + + math::Vector2 position{}; + std::uint16_t id{}; + }; + + template + struct RangeBearing + { + static_assert(std::is_floating_point_v, "RangeBearing supports floating-point types"); + + T range{}; + T bearing{}; + std::optional id{}; + }; + + template + struct LandmarkLocalizationNoise + { + static_assert(std::is_floating_point_v, "LandmarkLocalizationNoise supports floating-point types"); + + mobile::OdometryNoise motion{}; + T rangeNoise{}; + T bearingNoise{}; + T gate{}; + }; + + template + struct LandmarkUpdate + { + static_assert(std::is_floating_point_v, "LandmarkUpdate supports floating-point types"); + + filters::KalmanUpdateResult result{}; + std::optional landmark{}; + }; + + template + class LandmarkLocalization + { + static_assert(std::is_floating_point_v, "LandmarkLocalization supports floating-point types"); + static_assert(MaxLandmarks > 0, "LandmarkLocalization needs room for at least one landmark"); + + public: + using CovarianceMatrix = math::SquareMatrix; + + LandmarkLocalization(const math::SE2Transform& pose, const CovarianceMatrix& covariance, const LandmarkLocalizationNoise& noise); + + bool SetMap(std::span> landmarks); + OPTIMIZE_FOR_SPEED void Predict(const math::SE2Transform& increment); + OPTIMIZE_FOR_SPEED LandmarkUpdate Update(const RangeBearing& observation); + std::optional Nis(const RangeBearing& observation, std::size_t landmark) const; + + math::SE2Transform Pose() const; + const CovarianceMatrix& Covariance() const; + + private: + using Correction = filters::ResidualKalmanUpdate; + + struct Linearization + { + typename Correction::Residual residual; + typename Correction::Jacobian jacobian; + }; + + Linearization Linearize(const RangeBearing& observation, const Landmark& landmark) const; + std::optional Associate(const RangeBearing& observation) const; + std::optional FindById(std::uint16_t id) const; + std::optional Nearest(const RangeBearing& observation) const; + typename Correction::MeasurementNoise MeasurementNoise() const; + + std::array, MaxLandmarks> map{}; + std::size_t count{ 0 }; + math::Vector3 state; + CovarianceMatrix covariance; + LandmarkLocalizationNoise noise; + }; + + template + LandmarkLocalization::LandmarkLocalization(const math::SE2Transform& pose, const CovarianceMatrix& covariance, const LandmarkLocalizationNoise& noise) + : state{ pose.Pose() } + , covariance{ covariance } + , noise{ noise } + {} + + template + bool LandmarkLocalization::SetMap(std::span> landmarks) + { + if (landmarks.size() > MaxLandmarks) + return false; + + std::copy(landmarks.begin(), landmarks.end(), map.begin()); + count = landmarks.size(); + return true; + } + + template + OPTIMIZE_FOR_SPEED void LandmarkLocalization::Predict(const math::SE2Transform& increment) + { + const auto delta{ increment.Pose() }; + const T cosine{ math::Cos(state[2]) }; + const T sine{ math::Sin(state[2]) }; + const CovarianceMatrix transition{ { T{ 1 }, T{}, -sine * delta[0] - cosine * delta[1] }, { T{}, T{ 1 }, cosine * delta[0] - sine * delta[1] }, { T{}, T{}, T{ 1 } } }; + const CovarianceMatrix incrementMap{ { cosine, -sine, T{} }, { sine, cosine, T{} }, { T{}, T{}, T{ 1 } } }; + const auto deviation{ mobile::IncrementDeviation(increment, noise.motion) }; + const CovarianceMatrix incrementNoise{ { deviation[0] * deviation[0], T{}, T{} }, { T{}, deviation[1] * deviation[1], T{} }, { T{}, T{}, deviation[2] * deviation[2] } }; + + covariance = math::CongruenceTransform(transition, covariance) + math::CongruenceTransform(incrementMap, incrementNoise); + state = (Pose() * increment).Pose(); + } + + template + OPTIMIZE_FOR_SPEED LandmarkUpdate LandmarkLocalization::Update(const RangeBearing& observation) + { + const auto landmark{ Associate(observation) }; + if (!landmark) + return LandmarkUpdate{ filters::KalmanUpdateResult{ filters::KalmanUpdateStatus::Singular, T{} }, std::nullopt }; + + const auto linearization{ Linearize(observation, map[*landmark]) }; + const auto result{ Correction::Apply(state, covariance, linearization.residual, linearization.jacobian, MeasurementNoise(), noise.gate) }; + state[2] = math::WrapToPi(state[2]); + return LandmarkUpdate{ result, landmark }; + } + + template + std::optional LandmarkLocalization::Nis(const RangeBearing& observation, std::size_t landmark) const + { + really_assert(landmark < count); + + const auto linearization{ Linearize(observation, map[landmark]) }; + return Correction::Nis(linearization.residual, linearization.jacobian, covariance, MeasurementNoise()); + } + + template + math::SE2Transform LandmarkLocalization::Pose() const + { + return math::SE2Transform::FromPose(state[0], state[1], state[2]); + } + + template + auto LandmarkLocalization::Covariance() const -> const CovarianceMatrix& + { + return covariance; + } + + template + auto LandmarkLocalization::Linearize(const RangeBearing& observation, const Landmark& landmark) const -> Linearization + { + const T dx{ landmark.position[0] - state[0] }; + const T dy{ landmark.position[1] - state[1] }; + const T squared{ dx * dx + dy * dy }; + const T range{ math::Sqrt(squared) }; + const T bearing{ math::AngleDifference(math::Atan2(dy, dx), state[2]) }; + + return Linearization{ typename Correction::Residual{ observation.range - range, math::AngleDifference(observation.bearing, bearing) }, + typename Correction::Jacobian{ { -dx / range, -dy / range, T{} }, { dy / squared, -dx / squared, T{ -1 } } } }; + } + + template + std::optional LandmarkLocalization::Associate(const RangeBearing& observation) const + { + return observation.id ? FindById(*observation.id) : Nearest(observation); + } + + template + std::optional LandmarkLocalization::FindById(std::uint16_t id) const + { + const auto first{ map.begin() }; + const auto last{ map.begin() + static_cast(count) }; + const auto found{ std::find_if(first, last, [id](const Landmark& landmark) + { + return landmark.id == id; + }) }; + if (found == last) + return std::nullopt; + return static_cast(found - first); + } + + template + std::optional LandmarkLocalization::Nearest(const RangeBearing& observation) const + { + std::optional best{}; + T bestNis{ std::numeric_limits::infinity() }; + for (std::size_t k = 0; k < count; ++k) + { + const T nis{ Nis(observation, k).value_or(std::numeric_limits::infinity()) }; + if (nis < bestNis) + { + best = k; + bestNis = nis; + } + } + return best; + } + + template + auto LandmarkLocalization::MeasurementNoise() const -> typename Correction::MeasurementNoise + { + return typename Correction::MeasurementNoise{ { noise.rangeNoise * noise.rangeNoise, T{} }, { T{}, noise.bearingNoise * noise.bearingNoise } }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class LandmarkLocalization; + extern template class LandmarkLocalization; +#endif +} diff --git a/robotics/localization/test/CMakeLists.txt b/robotics/localization/test/CMakeLists.txt index 3c19853..295b3fe 100644 --- a/robotics/localization/test/CMakeLists.txt +++ b/robotics/localization/test/CMakeLists.txt @@ -8,6 +8,7 @@ target_link_libraries(robotics.localization_test PUBLIC ) target_sources(robotics.localization_test PRIVATE + TestLandmarkLocalization.cpp TestOccupancyGridMap.cpp TestOdometryFusion.cpp ) diff --git a/robotics/localization/test/TestLandmarkLocalization.cpp b/robotics/localization/test/TestLandmarkLocalization.cpp new file mode 100644 index 0000000..f433143 --- /dev/null +++ b/robotics/localization/test/TestLandmarkLocalization.cpp @@ -0,0 +1,139 @@ +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/localization/LandmarkLocalization.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Point = math::Vector2; + using Vector3 = math::Vector3; + using Landmark = localization::Landmark; + using Observation = localization::RangeBearing; + using Localization = localization::LandmarkLocalization; + using filters::KalmanUpdateStatus; + + constexpr float pi{ std::numbers::pi_v }; + + class TestLandmarkLocalization + : public ::testing::Test + { + protected: + TestLandmarkLocalization() + { + filter.SetMap(landmarks); + } + + Observation Measure(std::size_t landmark, std::optional id) const + { + const float dx{ landmarks[landmark].position[0] - truth[0] }; + const float dy{ landmarks[landmark].position[1] - truth[1] }; + return Observation{ std::hypot(dx, dy), math::WrapToPi(std::atan2(dy, dx) - truth[2]), id }; + } + + static void ExpectPose(const Localization& actual, float x, float y, float heading) + { + const auto pose{ actual.Pose().Pose() }; + EXPECT_NEAR(pose[0], x, 1e-5f); + EXPECT_NEAR(pose[1], y, 1e-5f); + EXPECT_NEAR(pose[2], heading, 1e-5f); + } + + static void ExpectDiagonal(const Localization& actual, float xx, float yy, float headingVariance) + { + EXPECT_NEAR(actual.Covariance().at(0, 0), xx, 1e-6f); + EXPECT_NEAR(actual.Covariance().at(1, 1), yy, 1e-6f); + EXPECT_NEAR(actual.Covariance().at(2, 2), headingVariance, 1e-6f); + } + + std::array landmarks{ Landmark{ Point{ 5.0f, 0.0f }, 0 }, Landmark{ Point{ 0.0f, 5.0f }, 1 }, Landmark{ Point{ 5.0f, 5.0f }, 2 }, Landmark{ Point{ -3.0f, 2.0f }, 3 } }; + localization::LandmarkLocalizationNoise noise{ { 0.05f, 0.01f, 0.05f, 0.01f }, 0.1f, 0.02f, 9.21f }; + Localization::CovarianceMatrix initial{ { 0.05f, 0.0f, 0.0f }, { 0.0f, 0.05f, 0.0f }, { 0.0f, 0.0f, 0.01f } }; + Localization filter{ Transform::FromPose(1.0f, 1.0f, 0.3f), initial, noise }; + Vector3 truth{ 1.1f, 0.9f, 0.32f }; + }; +} + +TEST_F(TestLandmarkLocalization, predict_composes_the_increment_and_grows_the_covariance) +{ + filter.Predict(Transform::FromPose(0.5f, 0.0f, 0.1f)); + + ExpectPose(filter, 1.477668f, 1.147760f, 0.4f); + ExpectDiagonal(filter, 0.062818f, 0.064882f, 0.013f); + EXPECT_NEAR(filter.Covariance().at(0, 1), -0.000706f, 1e-6f); + EXPECT_NEAR(filter.Covariance().at(1, 2), 0.004777f, 1e-6f); +} + +TEST_F(TestLandmarkLocalization, reverse_motion_has_the_same_noise_as_forward) +{ + Localization origin{ Transform::Identity(), Localization::CovarianceMatrix{}, noise }; + + origin.Predict(Transform::FromPose(-0.5f, 0.0f, 0.0f)); + + ExpectPose(origin, -0.5f, 0.0f, 0.0f); + ExpectDiagonal(origin, 0.0125f, 0.0125f, 0.0025f); +} + +TEST_F(TestLandmarkLocalization, unlabelled_observation_is_associated_by_mahalanobis_distance) +{ + const auto observation{ Measure(2, std::nullopt) }; + EXPECT_NEAR(*filter.Nis(observation, 0), 119.6f, 0.1f); + EXPECT_NEAR(*filter.Nis(observation, 1), 118.1f, 0.1f); + EXPECT_NEAR(*filter.Nis(observation, 3), 371.8f, 0.1f); + + const auto update{ filter.Update(observation) }; + + ASSERT_TRUE(update.landmark.has_value()); + EXPECT_EQ(*update.landmark, 2u); + EXPECT_EQ(update.result.status, KalmanUpdateStatus::Applied); + EXPECT_NEAR(update.result.nis, 0.002138f, 1e-5f); + ExpectPose(filter, 1.001568f, 0.996349f, 0.295825f); +} + +TEST_F(TestLandmarkLocalization, labelled_observations_converge_towards_the_truth) +{ + for (const std::uint16_t id : { 2, 0, 1, 3 }) + filter.Update(Measure(id, id)); + + ExpectPose(filter, 1.092522f, 0.904252f, 0.318948f); + ExpectDiagonal(filter, 0.002722f, 0.002077f, 0.000122f); +} + +TEST_F(TestLandmarkLocalization, outlier_is_gated) +{ + const auto covariance{ filter.Covariance() }; + + const auto update{ filter.Update(Observation{ 0.5f, 2.5f, std::nullopt }) }; + + EXPECT_EQ(update.result.status, KalmanUpdateStatus::Gated); + EXPECT_NEAR(update.result.nis, 219.48f, 0.01f); + ASSERT_TRUE(update.landmark.has_value()); + EXPECT_EQ(*update.landmark, 3u); + ExpectPose(filter, 1.0f, 1.0f, 0.3f); + ExpectDiagonal(filter, covariance.at(0, 0), covariance.at(1, 1), covariance.at(2, 2)); + + const auto unknown{ filter.Update(Observation{ 1.0f, 0.0f, std::uint16_t{ 9 } }) }; + EXPECT_EQ(unknown.result.status, KalmanUpdateStatus::Singular); + EXPECT_FALSE(unknown.landmark.has_value()); + + const std::array tooMany{}; + EXPECT_FALSE(filter.SetMap(tooMany)); +} + +TEST_F(TestLandmarkLocalization, bearing_residual_is_wrapped_across_pi) +{ + Localization nearSeam{ Transform::FromPose(6.0f, 4.9f, 0.0f), initial, noise }; + nearSeam.SetMap(landmarks); + + const auto update{ nearSeam.Update(Observation{ 6.0f, -pi + 0.01f, std::uint16_t{ 1 } }) }; + + EXPECT_EQ(update.result.status, KalmanUpdateStatus::Applied); + EXPECT_NEAR(update.result.nis, 0.060327f, 1e-5f); + ExpectPose(nearSeam, 5.999620f, 4.918856f, -0.022620f); +} From c91acfcc7439e0d6f8d3891a56eaf58d27be116e Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 19:03:33 +0000 Subject: [PATCH 4/5] build: pin numerical-toolbox to the NT74 merge commit Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- CMakeLists.txt | 2 +- ROADMAP.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a4c4770..4ef8b73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,7 @@ if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG c1a669bd94ebad3a7922dbc3865af0e02a5905f4 # main + GIT_TAG b93d6e074d6aeee4f9395feb54429d55954ebd45 # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/ROADMAP.md b/ROADMAP.md index ade1512..0b543bf 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -41,7 +41,7 @@ this roadmap needs that upstream does not ship yet are specified and built upstr Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT** plus its master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a numerical-toolbox pin that ships it. NT53–NT66 and NT74 have shipped upstream, and their links point to the -upstream docs; the current pin (`c1a669b`) includes them. +upstream docs; the current pin (`b93d6e0`) includes them. | # | Component | Upstream module | Spec / doc | Needed by | |------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------| From 41ebb5f5f8bb83451374ecdb6fcb6296cd9363af Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 19:07:04 +0000 Subject: [PATCH 5/5] refactor(localization): match the UpdatePosition parameter name in declaration and definition The declaration named the position covariance 'covariance', which also shadowed the filter's covariance member. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- robotics/localization/OdometryFusion.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotics/localization/OdometryFusion.hpp b/robotics/localization/OdometryFusion.hpp index f1edb2a..9f3d5bb 100644 --- a/robotics/localization/OdometryFusion.hpp +++ b/robotics/localization/OdometryFusion.hpp @@ -40,7 +40,7 @@ namespace localization OPTIMIZE_FOR_SPEED void Predict(T speed, T gyroRate, T dt); filters::KalmanUpdateResult UpdateWheelYawRate(T wheelYawRate, T gyroRate); filters::KalmanUpdateResult UpdateHeading(T heading, T sigma); - filters::KalmanUpdateResult UpdatePosition(const math::Vector2& position, const math::SquareMatrix& covariance); + filters::KalmanUpdateResult UpdatePosition(const math::Vector2& position, const math::SquareMatrix& positionCovariance); math::SE2Transform Pose() const; T GyroBias() const;