Real-time magnetic tracking of a moving capsule endoscope in C++17: Extended and Unscented Kalman filters versus frame-by-frame Levenberg-Marquardt on a simulated GI trajectory.
In wireless capsule endoscopy, a capsule carrying a small permanent magnet travels through the GI tract while an external magnetometer array measures its field. Most localization pipelines solve each frame independently with nonlinear least squares. This project treats the capsule as a moving target: a Kalman filter fuses the measurements over time with a motion model, which reduces noise and also estimates velocity.
It is the dynamic follow-up to magnetic-dipole-simulator, which covers the static, single-frame problem.
- Dipole field model with analytical Jacobian (position and moment)
- Two-layer ring magnetometer array (8 three-axis sensors, rings 30 cm apart)
- Simulated GI trajectory with pulsating, peristalsis-like speed
- Frame-by-frame Levenberg-Marquardt baseline
- Extended Kalman filter
- Unscented Kalman filter
- Noise-sweep benchmark and plots
State (9): position p [m], velocity v [m/s], magnetic moment m [A·m²].
Motion: constant velocity with white-acceleration process noise; the moment follows a random walk.
Measurement: each three-axis sensor at s observes the dipole field
B(s) = (mu0 / 4 pi) * [ 3 (m . r) r / |r|^5 - m / |r|^3 ], r = s - p
Fields are expressed in microtesla throughout, which keeps the filter covariances well conditioned.
| Parameter | Value |
|---|---|
| Sensors | 8, two rings of 4, radius 0.15 m, 30 cm apart (z = ±0.15 m) |
| Magnet moment | 0.2 A·m², aligned with the direction of travel |
| Capsule speed | about 2 to 21 mm/s |
| Workspace | within 0.10 m of the array centre |
cmake -S . -B build
cmake --build build --parallel 4Requires g++ with C++17 and CMake 3.18 or newer. Eigen 3.4 and Catch2 are downloaded automatically.
./build/testsThe tests check the analytical Jacobian against finite differences, the sensor geometry, and the physical consistency of the trajectory.
include/
dipole_model.hpp - dipole field and analytical Jacobian
sensor_array.hpp - two-layer ring array and stacked measurements
trajectory.hpp - simulated GI trajectory
lm_baseline.hpp - frame-by-frame Levenberg-Marquardt solver
src/
trajectory.cpp
lm_baseline.cpp
tests/
test_tracker.cpp - Catch2 unit tests
This project builds on the author's published work on magnetic localization for wireless capsule endoscopy:
- O. Yaghoobian, M. Einlou, K.A. Wahid. Two-stage cylindrical magnetic localization with neural network calibration for wearable capsule endoscopy. Scientific Reports, 2026.
- O. Yaghoobian, H. Keshmiri, M. Einlou, K.A. Wahid. Layer-Conditioned Magnetic Localization with Residual-Weighted Fusion for Wireless Capsule Endoscopy Under Asymmetric Sensor Visibility and GI Motion. IEEE Access, 2026.
- O. Yaghoobian, K.A. Wahid. SNR-Weighted Layer-Conditioned Magnetometer Array with Ellipsoid Calibration and Neural Network Initialization for Capsule Endoscopy Localization Under Asymmetric Sensor Visibility. Sensors, 2026.
- O. Yaghoobian, K.A. Wahid. A New Localization Algorithm for a Two-Layer Magnetic Sensor Array in Capsule Endoscopy. ICCIT 2025.