Hand_EKF — Finger Pose Estimation with an Extended Kalman Filter

Estimating the joint angles of a two-link finger from a pair of inertial measurement units (IMUs), fusing a physics-based dynamic model with noisy accelerometer/gyroscope readings through an Extended Kalman Filter (EKF).

The finger is modeled as a planar 2-DOF pendulum (segments driven by joint angles θ₁, θ₂). A MuJoCo simulation provides ground truth and inverse dynamics, symbolic equations of motion are derived with SymPy, and real motion is captured both from a video (via Tracker) and from two MPU-6555 IMUs mounted on an instrumented glove.

Status: work in progress. The dynamic model, MuJoCo simulation, trajectory processing and noisy sensor simulation are complete; the EKF implementation (last section of the notebook) is still being finished.

---

Pipeline

 Video of finger motion                 Instrumented glove (2× MPU-6555)
          │                                          │
          ▼                                          ▼
  Tracker point tracking                    scripts/serial_reader_mpu.py
  (mass A/B/C coordinates)                  (serial capture → CSV)
          │                                          │
          ▼                                          ▼
  data/tracker/*.txt                        data/imu/*.csv
          │                                          │
          └──────────────► Main_notebook.ipynb ◄─────┘
                                   │
        ┌──────────────────────────┼───────────────────────────┐
        ▼                          ▼                            ▼
  SymPy Lagrangian          MuJoCo model +                 EKF fusion
  equations of motion       inverse dynamics            (θ₁, θ₂ estimation)

1. Motion capture — finger motion is recorded on video and tracked in Tracker, exporting segment endpoint coordinates. The relative positions are converted into the joint angles θ₁, θ₂ used as a reference trajectory.
2. Dynamic model — the equations of motion for the 2-link finger are derived symbolically (Lagrangian mechanics) and cross-checked against MuJoCo.
3. Inverse dynamics — MuJoCo computes the joint torques required to follow the reference trajectory.
4. Sensor simulation — the expected accelerometer/gyroscope signals are generated from the state, then corrupted with realistic noise/quantization.
5. State estimation — an EKF fuses the dynamic transition model with the (simulated or real) IMU measurements to estimate the finger pose.

Repository structure

Hand_EKF/
├── README.md
├── requirements.txt
├── notebooks/
│   └── Main_notebook.ipynb        # Main pipeline: model, simulation, EKF
├── scripts/
│   └── serial_reader_mpu.py       # Capture dual-IMU serial data to CSV
├── data/
│   ├── tracker/
│   │   ├── Finger_coordinates.txt          # Tracker export (masses A, B, C)
│   │   └── Finger_coordinates_mpu6555.txt  # Tracker export (masses A–D)
│   └── imu/
│       └── datos_IMU_22-40-05.csv          # Captured IMU session (2 IMUs, 12 ch)
└── media/
    ├── video_glove_mpu6555.mp4             # Source video of the glove
    └── tracker_project_mpu6555.trz         # Tracker project file

Data formats

• data/tracker/*.txt — Tracker point-mass exports. A 3-line header (#multi:, mass labels, t,x,y,...) followed by comma-separated rows of a time column and (x, y) coordinates per tracked mass.
• data/imu/*.csv — output of serial_reader_mpu.py. Columns: Tiempo(s) plus, for each of the two IMUs, Acc{X,Y,Z} and Gyr{X,Y,Z} (13 columns total).

Setup

Requires Python 3.10+.

python -m venv .venv
# Windows (PowerShell):
.venv\Scripts\Activate.ps1
# Linux/macOS:
# source .venv/bin/activate

pip install -r requirements.txt

Usage

Capture IMU data from the glove

Flash your microcontroller to stream 12 space-separated values per line (IMU1 acc x/y/z, gyro x/y/z, then IMU2 the same). Adjust the puerto (COM port) and baudrate at the top of the script, then run:

python scripts/serial_reader_mpu.py

Rows are printed live and written to a timestamped datos_IMU_HH-MM-SS.csv. Press Ctrl+C to stop and close the port cleanly.

Run the analysis notebook

jupyter notebook notebooks/Main_notebook.ipynb

The notebook loads the reference trajectory from ../data/tracker/Finger_coordinates.txt (path relative to the notebook), so run it from the notebooks/ directory.

Hardware

• 2× MPU-6555 IMUs (accelerometer + gyroscope) mounted on finger segments of an instrumented glove.
• Finger geometry used in the model (segment lengths): l₀ = 44 mm, l₁ = 26 mm, l₂ = 23 mm.