FR-MHE: Fast and Robust Quadrotor State Estimation With Design, Analysis, and Validation

Accurate and high-rate state estimation is crucial for quadrotors due to its central role in decision, planning and control, and its performance directly affects trajectory tracking accuracy, flight stability, and mission safety. However, lightweight UAVs are constrained by limited onboard computational and storage resources due to restrictions in size, payload, and cost. Meanwhile, internal and external disturbances—such as mass and lift‑coefficient mismatches, unmodeled dynamics, wind gusts, payload variations, impact forces, and propeller damage—can significantly degrade state estimation accuracy. The Extended Kalman Filter (EKF) offers high computational efficiency, but is susceptible to local linearization errors under strong nonlinearities and complex disturbances. Moving Horizon Estimation (MHE), on the other hand, leverages system dynamics, measurements, and constraints within a sliding time window and generally provides superior nonlinear estimation capability, yet requires repeatedly solving optimization problems, making it difficult to meet the 100 Hz real‑time requirements of micro‑UAV closed‑loop control. Balancing estimation accuracy, disturbance robustness, and computational efficiency on resource‑constrained hardware remains a key challenge for autonomous quadrotor flight.

To address these challenges, the research team of Professors Su Jinya and Li Shihua from the School of Automation at Southeast University (with first author Jiaheng Lu, a first-year master's student) , together with Professor Wen-Hua Chen from The Hong Kong Polytechnic University, conducted the study "FR-MHE: Fast and Robust Quadrotor State Estimation With Design, Analysis, and Validation" and proposed the Fast and Robust Moving Horizon Estimation (FR-MHE) method. This approach explicitly augments unknown lumped disturbances as system states and replaces the conventional iterative optimization of MHE with a single‑step gradient descent, compressing estimation computation to the sub‑millisecond level while preserving disturbance awareness. The findings have been published in IEEE Transactions on Industrial Electronics, a flagship journal in the field of industrial electronics (a CAS Zone 1 TOP journal). 

I. Protocol

This study first introduces a lumped disturbance force state into the quadrotor translational dynamics, incorporating parameter deviations, payload changes, wind disturbances, impacts, and propeller anomalies into a unified augmented model to enable joint estimation of position, velocity, and disturbance forces. Within each moving time window, the method uses the propagated result from the previous time step as a warm start, performing only a single local correction along the gradient direction of the current window's cost function. Intermediate Jacobian computation results are recursively reused to avoid redundant operations, reducing time complexity to a level comparable to that of the Robust Extended Kalman Filter (REKF). The paper further provides error contraction conditions and stability analysis. The estimated results are fed into the existing cascaded PID controller of the Crazyflie without altering the underlying control structure.

*Figure 1: Schematic diagram of the Crazyflie quadrotor and the overall experimental system.

Figure 2: FR-MHE state estimation and cascaded control framework diagram

Key Innovations and Advantages:

1. Unified characterization of internal and external uncertainties through disturbance state augmentation, improving position, velocity, and disturbance force estimation under complex disturbances;

2. Sub-millisecond MHE achieved via warm-started single-step gradient iteration and recursive Jacobian computation, with corresponding stability conditions provided;

3. Fully onboard estimation on the resource-constrained Crazyflie STM32F405 MCU, validated through 10 representative comparative experiments.

II.Validation

The experimental platform consists of a Crazyflie 2.1 micro-quadrotor, a four-camera motion capture system, and a host computer. The CHINGMU MC1300 optical motion capture system acquires the UAV's position with sub-millimeter accuracy and streams it to the host computer at 100 Hz via the VRPN protocol; the host computer then forwards the data to the Crazyflie via Crazyradio. State estimation is performed onboard on the STM32F405 MCU, and logged data—including position, velocity, disturbance forces, and timestamps—are transmitted back via the CRTP protocol. All data links operate at 100 Hz, providing a reliable measurement foundation for closed-loop flight and estimation performance comparison under various disturbance scenarios.

The study first compared the computational efficiency of FR-MHE, conventional Robust MHE (RMHE) , and Robust EKF (REKF) . With a horizon length of N = 8, FR-MHE required only 0.17 ms per estimation on a PC, while RMHE required 19.23 ms—overall, RMHE's computation time was 55–126 times that of FR-MHE. Deployed on the Crazyflie, FR-MHE achieved 0.499 ms per computation, with an estimation rate exceeding 500 Hz, while maintaining low memory and CPU usage.

Subsequently, the study introduced disturbances including model parameter mismatches, wind gusts, a 4.5 g additional payload, multidirectional impacts, propeller replacement, and propeller trimming, and conducted hovering tests as well as trajectory flights along circular, square, triangular, and figure-eight paths. Results showed that FR-MHE's estimation accuracy was generally comparable to RMHE and significantly outperformed REKF. Under wind disturbances, REKF's hovering performance degraded markedly, while FR-MHE remained stable. When one propeller was trimmed to three-quarters of its original length, the quadrotor became unstable and crashed under REKF control, whereas both FR-MHE and RMHE successfully completed the flight task.

Figure 3: Comparison of computation time for three algorithms

Figure 4: Computation time results of the three algorithms

Figure 5: Tracking results of circular, square, triangular, and figure‑eight trajectories under wind disturbances

III.Result

This study achieved fast and robust moving horizon estimation on resource-constrained micro quadrotors, obtaining stable and accurate closed-loop experimental data via the CHINGMU motion capture system. Ten comparative experiments validated the method's estimation accuracy, robustness, and real-time performance under diverse internal and external disturbances. Compared to conventional RMHE, FR-MHE achieved a 55–126× computational acceleration. Compared to REKF, FR-MHE offers similar computational complexity but superior estimation performance and generalization capability under complex disturbances—providing a viable path for deploying MHE on lightweight robotic platforms.

REFERENCES：

[1] Jiaheng Lu, Jinya Su, Wen-Hua Chen, Shihua Li, “FR-MHE: Fast and Robust Quadrotor State Estimation With Design, Analysis, and Validation,” IEEE Transactions on Industrial Electronics, 2026, doi: 10.1109/TIE.2026.3708941.

[2] Wen-Hua Chen, Jun Yang, Lei Guo, and Shihua Li. "Disturbance-observer-based control and related methods—An overview." IEEE Transactions on industrial electronics 63, no. 2 (2015): 1083-1095.

[3] Jinya Su, Wen-Hua Chen, and Jun Yang. "On relationship between time-domain and frequency-domain disturbance observers and its applications." Journal of Dynamic Systems, Measurement, and Control 138, no. 9 (2016): 091013.

......