<figure><img src="image_1.png" /></figure>

HAL
open science

# Drift-Free Limb Angles Reconstruction for FSM-Based Gait Phases Supervision

Mario Spirito, Ardito Annavaleria, Vaccaro Riccardo, Traverso Simone, Boccardo Nicolò, Laffranchi Matteo

► To cite this version:

Mario Spirito, Ardito Annavaleria, Vaccaro Riccardo, Traverso Simone, Boccardo Nicolò, et al. Drift-Free Limb Angles Reconstruction for FSM-Based Gait Phases Supervision. 2026. hal-05520409

HAL Id: hal-05520409

https://hal.science/hal-05520409v1

Preprint submitted on 20 Feb 2026

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
# Drift-Free Limb Angles Reconstruction for FSM-Based Gait Phases Supervision

Mario Spirito*

Rehab Technologies Lab

Italian Institute of Technologies-CCT

Genoa, Italy

mario.spirito@iit.it

Annavaleria Ardito

Rehab Technologies Lab

Italian Institute of Technologies-CCT

Genoa, Italy

annavaleria.ardito@iit.it

Riccardo Vaccaro

Rehab Technologies Lab

Italian Institute of Technologies-CCT

Genoa, Italy

riccardo.vaccaro@iit.it

Simone Traverso¹

Rehab Technologies Lab

Italian Institute of Technologies-CCT

Genoa, Italy

simone.traverso@iit.it

Nicolò Boccardo¹

Rehab Technologies Lab

Italian Institute of Technologies-CCT

Genoa, Italy

nicolo.boccardo@iit.it

Matteo Laffranchi

Rehab Technologies Lab

Italian Institute of Technologies-CCT

Genoa, Italy

matteo.laffranchi@iit.it

**Abstract**—Synchronizing assistance in soft exosuits requires precise gait phase detection derived from reliable kinematic data. This paper presents a six-phase Finite State Machine utilizing a unilateral modeling approach, which treats each limb as an independent system to ensure a lightweight and modular control architecture. To achieve robust orientation estimation, we implement a signal processing pipeline featuring gravity-based orientation mapping and trajectory unwrapping to manage mathematical singularities. A key contribution of this work is the integration of recursive high-pass filtering for drift suppression, enabling the reconstruction of accurate, drift-free limb angles from Inertial Measurement Unit data. The FSM utilizes deterministic transition logic—relying on threshold analysis and zero-crossing detection—to identify fundamental gait events such as foot impact, heel-off, and swing dynamics. Experimental results demonstrate that the system accurately supervises gait progression with high reliability, providing a scalable and computationally efficient foundation for the control of wearable robotic systems across varied locomotor activities.

**Index Terms**—Gait Phase Detection, Finite State Machine, Inertial Measurement Units, Drift Compensation, Wearable Robotics, Soft Exosuits.

## I. INTRODUCTION

Human gait analysis has become a cornerstone in clinical diagnostics, rehabilitation monitoring, and the development of assistive technologies [1], [2]. Traditional gold-standard systems, such as optoelectronic motion capture, offer high precision but are restricted to laboratory environments due to their lack of portability and high cost [3]. To overcome these limitations, wearable Inertial Measurement Units (IMUs) have emerged as a versatile alternative, enabling long-term, out-of-the-lab monitoring of gait parameters in both healthy and pathological populations [4]–[6].

Despite their advantages, reconstructing limb link angles from raw IMU data remains a significant challenge. The integration of gyroscope signals is inherently prone to “drift”—a cumulative error caused by sensor noise and bias that leads to inaccurate orientation estimates over time [7]. Addressing this drift is critical for obtaining reliable kinematic data, which serves as the foundation for high-level control systems in wearable robotics [8].

In the context of lower-limb exoskeletons, the ability to accurately detect gait phases in real-time is essential for providing synchronized and intuitive assistance [9], [10]. Finite State Machines (FSMs) have been widely adopted for this purpose due to their computational efficiency and robustness in handling the cyclical nature of human locomotion [11], [12]. By partitioning the gait cycle into discrete states— of the Stance and Swing phases—FSMs allow for the implementation of event-driven control strategies that adapt to the user's intended movement [13], [14].

The utility of FSM-based control is particularly evident in rehabilitation robotics, where hierarchical control strategies are employed to manage the interaction between the user and the device [15], [16]. Research into exoskeletons for specific populations, such as children with cerebral palsy, highlights the need for control algorithms that are not only robust but also capable of recognizing various locomotion modes [17], [18]. By utilizing reconstructed link angles to drive an FSM, researchers can develop model-based control strategies that provide more natural assistance compared to traditional time-triggered methods [19], [20].

The remainder of this paper is organized as follows. Section II establishes the theoretical foundation by redefining a unilateral, six-phase gait cycle suitable for real-time applications. Section III details the materials and methods used, encompassing the experimental setup, data acquisition using spherical coordinates, signal conditioning with the discrete-time drift filter design, and the architecture of the Finite State Machine.

This research was funded by the PR23-RR-P1 FeatherEXO INAIL project.

* Corresponding author.

¹ Open University Affiliated Research Centre at Istituto Italiano di Tecnologia (ARC@IIT), via Morego 30, Genova, 16163, Italy
Section IV presents the validation results, including the effec-
tiveness of the signal filtering and a preliminary comparison
of the FSM performance against gait events retrieved via a
Vicon motion capture system. Finally, Section V provides a
discussion of these findings, concludes the work, and outlines
future directions for advanced exoskeleton control integration.

II. GAIT PHASES AND EVENTS REDEFINITION

Gait analysis is traditionally decomposed into distinct
phases and transition events [21]. However, classical defini-
tions often rely on bilateral events that couple the movements
of both limbs (e.g., double support or initial contact of the
contralateral limb). Our approach is motivated by the fact that
both legs undergo an identical sequence of phases; therefore,
they can be analyzed as independent unilateral systems and,
if needed, synchronized a posteriori.

Analyzing each limb as an independent unilateral system,
the stance period is subdivided into three phases:

*   **Early Stance:** Commences at the instant of initial contact between the heel and the ground (Event: Heel Strike);

*   **Mid-Stance:** Initiated when the foot achieves full contact with the ground through a rapid, controlled descent (Event: Foot Impact);

*   **Late Stance:** Begins as the heel loses contact with the ground to initiate forward progression (Event: Heel Off).

The swing period subsequently comprises:

*   **Initial Swing:** Begins when the foot loses all contact with the ground (Event: Toe Off);

*   **Mid-Swing:** Commences when the swinging ankle crosses the sagittal plane of the body, facilitating forward limb advancement (Event: Ankle Crossing);

*   **Terminal Swing:** Initiated when the tibia reaches a vertical orientation as the limb prepares for the subsequent heel strike (Event: Tibia Vertical).

This sequence of phases and transition events is formally
mapped within the FSM architecture illustrated in Fig. 2.

*Remark on Heel-to-Toe Progression:* Although the “heel-to-toe roll”, which is a continuous motion that spans the duration of the *Stance* phase, our FSM architecture requires discrete triggers for state synchronization. We define the *Foot Impact* event as the specific instant when the foot completes its controlled descent to reach maximum surface contact.

III. MATERIALS & METHODS

A. *Experimental Setup and Sensors*

The experimental hardware platform is centered around a
Raspberry Pi 4 Model B single-board computer, which serves
as the central processing and data aggregation unit. Data
acquisition is performed by eight custom-designed interface
boards, each integrating a BNO055 9-axis IMU (Bosch Sen-
sortec, Kusterdingen, Germany). These sensors deliver real-
time sensor fusion data, specifically providing the gravity
vector, quaternions, and Euler angles at a uniform sampling
rate of 100 Hz. The system is powered by an INIU BI-
B64 high-capacity portable power unit (27,000 mAh / 99.9

Wh), providing a stable 5V/3A output to ensure operational
longevity and system stability during mobile testing.

The power supply unit and the Raspberry Pi 4 are securely
enclosed within a robust protective case, which is affixed to the
subject's back using a dedicated elastic harness. Each of the
eight sensor boards is housed in its own custom-designed min-
imal enclosure. These IMU units are strategically positioned
and secured to the body using adjustable straps at the following
anatomical locations: the lateral sides of the central unit case
(on the back), the anterior aspect of the thigh and tibia, and
the heel of the shoe. Sensor data from all eight IMUs are
transmitted to the Raspberry Pi via a unified Controller Area
Network (CAN) bus architecture. The Raspberry Pi processes
the collected data to obtain the orientation evolution of the
segments in real time during testing, while logging all raw and
processed data into structured .txt files for subsequent offline
analysis and verification.

B. Spherical Coordinates Applied to the Gravity Vector Signal

To determine the absolute orientation relative to the vertical
axis within the sagittal plane, we utilize the gravity vector pro-
vided by the onboard sensor fusion algorithm of the BNO055
IMU. The processing of orientation data is based on a spherical
coordinate system defined within the IMU-fixed body frame,
as illustrated in Fig.1.

In this coordinate system, the gravity vector $g = (g_x, g_y, g_z)$
is expressed by the following spherical coordinates

$$
\begin{cases}
\rho = \sqrt{g_x^2 + g_y^2 + g_z^2} \\
\varphi = \arctan\left(\frac{g_z}{g_y}\right) \\
\theta = \arctan\left(\frac{\operatorname{sign}(\cos \varphi) \sqrt{g_y^2 + g_z^2}}{g_x}\right).
\end{cases}
$$

Under the first-order approximation that the lower limbs
move within a plane parallel to the sagittal plane, the orienta-
tion of the segments relative to the vertical axis<fnref n="1" /> is represented
by the angle ϕ. This orientation angle is computed using the
atan2 function of the NumPy library, utilizing the z and y
components of the gravity vector as inputs.

C. *Drift Removal Filter*

During dynamic repetitive motions, the angle reconstructed
from the IMU data is affected by low-frequency disturbances.
This artifact mimics a drift typically associated with numer-
ical integration issues. The amplitude of this spurious low-
frequency content is non-negligible; it significantly distorts
the orientation reconstruction, occasionally pushing the signal
beyond the ±180° range.

To address this, we defined a custom digital high-pass filter,
whose discrete-time difference equation reads as

$$
\varphi^*(k) = \exp(\omega_{hp} \cdot T_s) \cdot \varphi^*(k-1) + K_{hp} (\varphi(k) - \varphi(k-1)),
$$

<footnote n="1">¹Defined as the intersection of the frontal and sagittal planes.</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 1: Representation of the adopted spherical coordinate in IMU fixed frame. The *x* and *y* axes of the IMU board span the *xy*-plane, corresponding to the board plane. The *z*-axis corresponds to the direction on motion.</figcaption></figure>

where $\omega_{hp}$ and $K_{hp}$ represent the cutoff frequency and the static gain, respectively. The designed filter is a recursive, first-order high-pass operator characterized by a zero at the origin and a pole at $\omega_{hp}$ in the frequency domain$^2$.

In the high-frequency asymptotic limit ($\omega \gg \omega_{hp}$), the gain is governed by $K_{hp}$; we set this to unity to maintain a transparent passband. To effectively suppress the low-frequency drift while preserving gait dynamics, the cutoff frequency is set to $\omega_{hp} = 0.1$ rad/s.

Further details on the rationale behind the filter design will be provided in the extended journal version of this paper. Although a preliminary validation comparing this reconstruction method with a Vicon motion capture system is available in [22].

## D. Finite State Machine

Building upon the unilateral gait subdivision defined in the previous section, we implement a Finite State Machine to track these phases in real-time. The FSM operates as a deterministic transition logic where the system resides in one of the six gait states ($S_1...S_6$) for each leg $j \in \{\text{left, right}\}$.

The transition between states is triggered by monitoring kinematic signatures from three segments: the femur, the tibia, and the foot. We primarily utilize two mathematical operators for event detection:

1. **Threshold Crossing:** Transitions occur when a filtered angular velocity ($\omega$) or segment angle ($\phi$) exceeds or falls below a predefined biomechanical constant ($Th_{event}$).

2. **Zero-Crossing Detection:** By evaluating the sign of the product between the current and previous sample (e.g., $\text{sign}(x(k) \cdot x(k-1)) < 0$), the algorithm identifies the exact moment a segment changes direction or crosses a reference plane, such as the vertical axis.

The logic for each transition event is summarized as follows:

<figure><img src="image_4.png" /><figcaption>Fig. 2: Diagram of the FSM logic for single-limb gait analysis. The six distinct functional phases (Early Stance, Mid-Stance, Late Stance, Initial Swing, Mid-Swing, Late Swing) are represented by the states, with transitions triggered by the specific unilateral events (*Heel Strike*, *Foot Impact*, *Heel Off*, *Toe Off*, *Ankle Crossing*, and *Tibia Vertical*).</figcaption></figure>

* **Foot Impact (1 → 2):** Detected via a sign change in the foot's angular velocity relative to the impact threshold ($Th_{impact}$), indicating the end of the foot's descent.

* **Heel Off (2 → 3):** Triggered when the foot's angular velocity crosses a threshold ($Th_{off}$) indicating the start of the push-off.

* **Toe Off (3 → 4):** Identified by a zero-crossing in the foot's angular velocity, marking the reversal of motion as the foot leaves the ground.

* **Ankle Crossing (4 → 5):** Calculated by summing the sines of the femur and tibia angles to find the moment the ankle joint passes the body's frontal plane.

* **Tibia Vertical (5 → 6):** Detected when the tibia angle crosses the zero-degree vertical reference.

* **Heel Strike (6 → 1):** Identified by a specific negative zero-crossing in foot angular velocity, signaling the impact of the heel with the ground.

# IV. RESULTS

## A. Removing the Signal Drift

The angle reconstructed from IMU data during repetitive dynamic motions is affected by a low-frequency disturbance that mimics a numerical integration drift artifact. The amplitude of this spurious content is non-negligible, significantly distorting orientation reconstruction and occasionally pushing the signal beyond the ±180° range. As illustrated in Figure 3a, this condition, combined with the limited codomain of the atan2 function, results in frequent discontinuities in the angle trajectory at the boundaries. To address these discontinuities and facilitate linear filtering, the reconstructed signal is first

<footnote>$^2$The zero and pole locations are defined in the continuous frequency domain (s-plane) and mapped to the discrete-time domain via the matched Z-transform.</footnote>
unwrapped to span a continuous range in the real field, as shown in Figure 3b.

The first-order high-pass filter described in Section III-C, with a unitary static gain $K_{hp}$ and cutoff frequency set to $\omega_{hp} = 0.1$ rad/s, was applied to the unwrapped signal to suppress the low-frequency disturbance while preserving gait dynamics. The resulting filtered angle is presented in Figure 3c. Visual analysis confirms the effective removal of the low-frequency drift, yielding a repetitive, stable signal suitable for subsequent analysis and definition of transition conditions for the Finite State Machine described in later sections. The inset in Figure 3c provides a detailed zoom of the stabilized, repetitive waveform during consistent gait cycles.

## B. Preliminary Validation of the FSM

The results of the developed Finite State Machine were compared against a gold-standard Vicon motion capture system. Key gait events were manually retrieved from video records over ten continuous walking cycles. A temporal match was performed between the FSM transition outputs and these Vicon-derived events.

The plot in Figure 4 illustrates the FSM performance over several gait cycles. The y-axis maps the FSM output to the six operational gait phases: Early Stance ($S_1$), Mid-Stance ($S_2$), Late Stance ($S_3$), Early Swing ($S_4$), Mid Swing ($S_5$), and Late Swing ($S_6$). The vertical dashed lines represent the corresponding events detected via the Vicon analysis: **HS** (Heel Strike, green), **FI** (Foot Impact, blue), **HO** (Heel Off, magenta), and **TO** (Toe Off, red).

Visual analysis confirms a high success rate for the FSM in detecting these events across all cycles. The temporal mismatch tolerance observed between the FSM output and the Vicon events was a maximum of 2 samples, corresponding to an error of approximately 20 ms (assuming a 100 Hz sampling rate). This preliminary validation demonstrates the reliability of the IMU-based FSM approach for real-time gait phase supervision.

## V. DISCUSSION AND CONCLUSIONS

In this work, we presented a comprehensive framework for real-time gait phase supervision designed for wearable assistive technologies. By redefining the gait cycle into a unilateral, six-phase model, we addressed the limitations of classical bilateral definitions that complicate real-time control in independent-limb systems.

The integration of custom IMU interface boards with a high-performance Raspberry Pi 4 allowed for reliable, high-frequency (100 Hz) data acquisition. Crucially, the signal conditioning pipeline—specifically the unwrapping of spherical coordinates and the application of a recursive high-pass filter—proved effective in eliminating low-frequency disturbances. This reliable kinematic input enabled the FSM to accurately distinguish between stance and swing sub-phases using deterministic triggers, such as the *Foot Impact* and *Ankle Crossing* events.

<figure><img src="image_5.png" /><figcaption>(a) Wrapped angle.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>(b) Unwrapped angle.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>(c) Filtered angle with zoom detail.</figcaption></figure>

Fig. 3: Analysis of the foot angle: comparisons between wrapped, unwrapped, and filtered signals with a detailed zoom.
<figure><img src="image_8.png" /><figcaption>Fig. 4: This figure displays the output of the FSM algorithm used for real-time gait phase detection, plotted over a short time interval.</figcaption></figure>

By identifying specific transition points such as *Foot Impact* and *Toe Off*, the proposed FSM provides the granularity required for a soft exosuits assistance control architecture.

## A. Future Works

While the current FSM effectively monitors steady-state walking, several avenues for further development remain. We aim to provide a more rigorous quantitative validation of the detected events. This will involve comparing the FSM’s real-time state estimation against ground-truth data from optical motion capture (Vicon) and force plates, with a specific focus on the precision of swing phase timing.

To provide a more complete supervision layer, the FSM will be refined to include non-walking states. We intend to integrate detection for transitions toward non-dynamical conditions such as sitting, standing up, and standing still.

By evolving toward a multi-modal activity recognition system, this platform will serve as a robust foundation for truly versatile, daily-wear assistive exosuits.

# VI. ACKNOWLEDGMENT

The corresponding author, M. Spirito made substantial contributions to conceptualization, data analysis, and manuscript preparation. The remaining authors provided project management support and secured funding. All authors approved the final version of the manuscript.

During the preparation of this work, the authors used Gemini (by Google) to refine the English phrasing and enhance the clarity of the technical descriptions. All final content was reviewed and edited by the authors, who take full responsibility for the integrity of the work.

## REFERENCES

[1] M. Iosa, P. Picerno, S. Paolucci, and G. Morone, "Inertial measurement units: A brief state of the art on gait analysis," *Sensors*, vol. 16, no. 12, p. 2002, 2016.

[2] A. Muro-de-la Heran, B. Garcia-Zapirain, and A. Mendez-Zorrilla, "Gait analysis using wearable sensors," *Sensors*, vol. 14, no. 2, pp. 3362-3394, 2014.

[3] H. Prasanth, M. Cabas, A. M. Blanco, S. Tedesco, O. Giggins, J. Barton, and B. O'Flynn, "Gait analysis in a box: A system based on magnetometer-free imus or clusters of optical markers with automatic event detection," *Sensors*, vol. 20, no. 12, p. 3338, 2020.

[4] E. Digo, V. Agostini, L. Gastaldi, and E. Panero, "Inertial measurement unit sensor-based gait analysis in adults and older adults: A cross-sectional study," *Journal of NeuroEngineering and Rehabilitation*, vol. 21, no. 1, p. 15, 2024.

[5] L. Donisi, G. Cesarelli, G. Piscopo, F. Amitrano, G. Pagano, and M. Cesarelli, "Validation of a wearable imu system for gait analysis: Protocol and application to a new system," *Applied Sciences*, vol. 11, no. 11, p. 4851, 2021.

[6] L. Shu, T. Hua, Y. Wang, Q. Li, J. Feng, and X. M. Tao, "Gait analysis using imu sensor," in *2021 International Conference on Sensing, Measurement & Data Analytics (ICSMD)*, pp. 1-6, 2021.

[7] T. Seel, J. Raisch, and T. Schauer, "Imu-based joint angle measurement for gait analysis," *Sensors*, vol. 14, no. 4, pp. 6891-6909, 2014.

[8] C. Y. Kian, S. C. Liew, and C. S. Liew, "Thigh motion-based gait analysis for human identification using inertial measurement units (imus)," *Informatics in Medicine Unlocked*, vol. 32, p. 101037, 2022.

[9] R. Luo, S. Sun, X. Zhang, Z. Wang, and Q. Wang, "Gait event detection of a lower extremity exoskeleton robot by an intelligent imu," *IEEE Access*, vol. 8, pp. 18469-18478, 2020.

[10] J. Taborri, E. Palermo, S. Rossi, and P. Cappa, "Wearable sensor-based real-time gait detection: A systematic review," *Sensors*, vol. 16, no. 5, p. 631, 2016.

[11] C. Bazzicalupo, A. Castelli, M. Carli, and M. Schmid, "U2fsm: Un-supervised square finite state machine for gait events estimation from instrumented insoles," *Sensors*, vol. 23, no. 3, p. 1254, 2023.

[12] I. P. Pappas, M. R. Popovic, T. Keller, V. Dietz, and M. Morari, "Heuristic real-time detection of temporal gait events for lower limb amputees," *IEEE Transactions on Neural Systems and Rehabilitation Engineering*, vol. 9, no. 2, pp. 113-123, 2001.

[13] M. S. Aung, S. B. Thies, L. P. Kenney, D. Howard, R. W. Selles, J. Hellums, and J. Y. Goulermas, "Inertial measurement unit-based real-time adaptive algorithm for human walking pattern and gait event detection," *IEEE Transactions on Neural Systems and Rehabilitation Engineering*, vol. 21, no. 4, pp. 601-612, 2013.

[14] T. Yan, M. Cempini, M. Calabrò, N. Hummel, N. Vitiello, and M. C. Carrozza, "An event-driven control to achieve adaptive walking assist with gait primitives," *IEEE Transactions on Neural Systems and Rehabilitation Engineering*, vol. 23, no. 5, pp. 875-885, 2015.

[15] R. Baud, A. Binboogaard, A. Ijspeert, and M. Bouri, "Review on control strategies for lower limb rehabilitation exoskeletons," *Frontiers in Robotics and AI*, vol. 8, p. 632326, 2021.

[16] M. R. Tucker, J. Olivier, A. Pagel, H. Bleuler, M. Bouri, O. Lambercy, J. d. R. Millán, R. Riener, H. Vallery, and R. Gassert, "Control schemes for lower limb exoskeleton: A systematic review," *Journal of NeuroEngineering and Rehabilitation*, vol. 12, no. 1, pp. 1-29, 2015.

[17] D. Shi, J. Zhao, and Z. Wang, "A review of hierarchical control strategies for lower-limb exoskeletons in children with cerebral palsy," *Sensors*, vol. 24, no. 4, p. 1204, 2024.

[18] M. Li, G. Xu, J. Xie, and Z. G. Hou, "Fsm-hsvm-based locomotion mode recognition for exoskeleton robot," *Cybernetics and Systems*, vol. 50, no. 2, pp. 121-138, 2019.

[19] B. Chen, X. Zhao, H. Ma, and L. Qin, "Development of ankle-less active lower-limb exoskeleton controlled using finite leg function state machine," *Frontiers in Neurorobotics*, vol. 15, p. 638780, 2021.

[20] P. L. Melo, M. T. Silva, J. M. Martins, and D. J. Newman, "Experimental validation of a finite state controlled functional electrical stimulation walking system with real-time gait phase detection using a single wearable imu sensor," *Medical Engineering & Physics*, vol. 78, pp. 31-40, 2020.

[21] J. Perry, *Gait Analysis: Normal and Pathological Function*. Thorofare, NJ: SLACK Incorporated, 1992.

[22] A. Ardito, M. Spirito, L. Cavallaro, R. Vaccaro, S. Traverso, N. Boccardo, and M. Laffranchi, "Imu-based joint trajectories and gait contact event detection: A vicon-referenced analysis," *Submitted to the International Conference of the IEEE Engineering in Medicine and Biology Society (EMBC)*, 2026.