Skip to content

feat(localization): add OdometryFusion (E1) and LandmarkLocalization (E3) #39

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
gabrielfrasantos merged 5 commits into from
Nov 5, 2026
Merged

feat(localization): add OdometryFusion (E1) and LandmarkLocalization (E3) #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f7b15dc
build: bump numerical-toolbox for ResidualKalmanUpdate (NT74)
claude Nov 5, 2026
f4d8d37
feat(localization): add OdometryFusion (E1)
claude Nov 5, 2026
3b64372
feat(localization): add LandmarkLocalization (E3)
claude Nov 5, 2026
c91acfc
build: pin numerical-toolbox to the NT74 merge commit
claude Nov 5, 2026
41ebb5f
refactor(localization): match the UpdatePosition parameter name in de…
claude Nov 5, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 b93d6e074d6aeee4f9395feb54429d55954ebd45 # main
)

set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE)
Expand Down
4 changes: 2 additions & 2 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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, 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
Expand Down
66 changes: 28 additions & 38 deletions ROADMAP.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 (`b93d6e0`) 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
Expand All @@ -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

Expand Down Expand Up @@ -290,19 +290,9 @@ 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.

**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.
Expand Down
Loading
Loading