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

Jiaheng Lu¹, Jinya Su¹, Senior Member, IEEE, Wen-Hua Chen², IEEE Fellow and Shihua Li¹, IEEE Fellow

$^{1}$Jiaheng Lu, Jinya Su, and Shihua Li are with School of Automation, Key Laboratory of Measurement and Control of CSE, Ministry of Education, and Institute of Intelligent Unmanned Systems, Southeast University, Nanjing 210096, China.
$^{2}$Wen-Hua Chen is with the Research Centre for Low Altitude Economy and the Department of Aeronautical and Aviation Engineering, The Hong Kong Polytechnic University, Hong Kong, China.

Video

Diagram of the proposed FR-MHE framework. The disturbance force $\boldsymbol{d}_f$ is explicitly augmented as states, enhancing the quadrotor dynamic model's ability and robustness in capturing both internal and external disturbances. Based on this augmented model and prior state $\boldsymbol{\bar{\xi}}_t$, FR-MHE adopts a fast single iteration gradient descent, which substantially accelerates MHE-based state estimation to meet the controller's high-frequency state feedback requirements. The estimated position and velocity are then transmitted to the cascaded PID controller of Crazyflie, while attitude and attitude rate are provided by the motion capture system and IMU, and the cascaded PID controller finally outputs the motor commands.

Summary Video

Abstract

Accurate and high-rate state estimation is crucial for quadrotors due to its central role in both planning and control, which is challenging due to the presence of unknown disturbances. Disturbances include internal ones caused by model uncertainties such as mass or lift-coefficient mismatch, and external ones caused by unknown wind or striking forces. To address these challenges, we propose a disturbance-resilient and computationally efficient state estimation method, termed Fast and Robust Moving Horizon Estimation (FR-MHE), and validate it through onboard closed-loop experiments on Crazyflie quadrotor with its resource-constrained STM32F405 MCU. Disturbances are first augmented into the quadrotor dynamics to enhance generalization and robustness. We then design a lightweight computation framework via a single-iteration gradient descent to overcome the low computational efficiency of MHE, which otherwise cannot meet the controller's 100Hz state estimation demand. Extensive experiments across 10 scenarios, including hovering under internal disturbances, wind gusts, payload changes, and striking forces, and flying 4 typical trajectories under wind gusts, changed paddles, and damaged paddles, demonstrate that, while maintaining the estimation performance of Robust MHE (RMHE), FR-MHE improves computational efficiency by 55-126$\times$ and reaches sub-millisecond, achieving a runtime comparable to Robust Extended Kalman Filter but with superior state estimation capability.

Contributions

(1) Disturbance Resilience: We introduce an explicit disturbance augmentation meachanism, enabling the proposed FR-MHE framework to more effectively capture various disturbances and maintain accurate state estimates under real-world uncertainties.

(2) Computation Efficiency: We adopt a single-iteration gradient descent in FR-MHE to reduce computation burden of MHE. The computational time complexity is only $\mathcal{O}(N \cdot n^3)$, and improves computation efficiency by 55-126x (i.e., to sub-millisecond).

(3) Experimental validation: We conduct comprehensive closed-loop evaluations of FR-MHE against SOTA (R)MHE and (R)EKF on Crazyflie 2.1 platform under 10 representative scenarios. The experimental results demonstrate that FR-MHE offers clear advantages in both estimation efficiency and robustness.

Crazyflie Experiments

1. Calculation Time

Computation Time: comparison of computation time for 3 algorithms

The results show that the computational efficiency of FR-MHE is comparable to that of REKF, which is consistent with our analysis in our paper. In contrast, because RMHE requires multiple iterations of a nonlinear optimization procedure, its computation time is 55-126$\times$ larger than that of FR-MHE. When the horizon length $N\geq6$, RMHE cannot satisfy the required 100 $Hz$ update rate of the state estimator, even when fully utilizing the computational resources of the PC. To improve efficiency, we neglect the process noise in RMHE ($N=8$), yielding a per estimation computation time of 5.21 $ms$. Finally, FR-MHE ($N=8$) and REKF are deployed on the Crazyflie for onboard computation, whereas RMHE ($N=8$) is executed on the PC and its estimates are transmitted to the Crazyflie.

2. Hovering with Internal Disturbances

FR-MHE

FR-MHE$^*$

FMHE

Internal: hovering height and disturbance forces estimation

Internal: estimated velocities and groundtruth values

Experimental results demonstrate that the proposed FR-MHE accurately estimates an internal disturbance of -0.16 $N$. During phases $S_1$ (take-off) and $S_2$ (hovering), the velocity estimates remain highly consistent with the groundtruth, and the quadrotor ultimately achieves stable hovering at the target altitude of 0.9 $m$. In contrast, FMHE, which lacks disturbance compensation, yields a spurious upward velocity estimate of 1.2 $m/s$ even though the actual velocity remains zero, so the vehicle fails to lift off.

3. Hovering with Internal and Wind Disturbances

FR-MHE (without wind)

RMHE (without wind)

REKF (without wind)

FR-MHE (with wind)

RMHE (with wind)

REKF (with wind)

Wind: Crazyflie hovering without wind

Wind: Crazyflie hovering with wind

The results show that REKF fails to accurately estimate the disturbance force along the Z-axis without wind, leading to a large overshoot during takeoff and a hovering envelope roughly twice that of FR-MHE and RMHE. When we add wind disturbances, RMHE achieves the best overall performance, with FR-MHE closely matching it, whereas REKF drops to the ground after about 23 $s$ and then pulls back up, resulting in unsatisfactory hovering performance.

4. Hovering with Internal and Payload Disturbances

FR-MHE

RMHE

FMHE

Payload: Z-axis position and estimated disturbance force

FR-MHE and RMHE still exhibit strong estimation capability, enabling the Crazyflie to smoothly hover at 0.9 $m$ after takeoff. In contrast, REKF produces inaccurate estimates of disturbance force along the Z-axis in the presence of complex ground-effect phenomena and additional payload during takeoff, resulting in a pronounced overshoot in altitude. In this test, the disturbance force estimates of all three methods converge to -0.23 $N$, whereas under internal disturbances only, the estimates converge to -0.16 $N$, as shown in figure. At first glance, this appears inconsistent with the added payload of 4.5 $g$. The reason is that the identified thrust coefficient is not exact: when an additional payload is attached, extra lift must be generated, so the magnitude of the resulting disturbance force becomes larger than the added payload.

5. Hovering with Internal and Striking Disturbances

FR-MHE

Striking force: velocity and disturbance forces estimation

After the Crazyflie completes the take-off phase and enters a 15 $s$ hover at an altitude of 0.9 $m$, we strike the vehicle with a stick from various directions. The results demonstrate that, despite the presence of significant disturbance forces (as indicated in figure, where the estimated disturbance force in the X-Y directions peaks at 0.15 $N$, approximately half of the gravitational force), the combination of the FR-MHE and a cascaded PID controller enables the Crazyflie to maintain a stable hover.

6. Flying with Internal and Wind Disturbances

FR-MHE (circle)

RMHE (circle)

REKF (circle)

FR-MHE (square)

RMHE (square)

REKF (square)

FR-MHE (triangle)

RMHE (triangle)

REKF (triangle)

FR-MHE (figure-eight)

RMHE (figure-eight)

REKF (figure-eight)

Fly/wind: tracking circle and square paths with 3 algorithms

Fly/wind: tracking triangle and figure-eight paths with 3 algorithms

In the path following tasks along these trajectories, the REKF exhibits pronounced performance degradation in complex scenarios due to the simplifications introduced by linearization, and its performance is inferior to that of FR-MHE and RMHE. Although FR-MHE adopts a lightweight computation strategy, it still maintains high estimation accuracy: for three out of the four trajectories, its position RMSE is lower than that of RMHE. This improvement is attributed to the fully onboard computation of FR-MHE, whereas RMHE requires additional data transmission, which introduces extra delay-related errors.

7. Flying with Internal and Paddle Disturbances

FR-MHE (change one paddle)

RMHE (change one paddle)

REKF (change one paddle)

FR-MHE (damage one paddle)

RMHE (damage one paddle)

REKF (damage one paddle)

Fly/Paddle: Path tracking with a paddle altered or trimmed

We further modify the paddle of the Crazyflie used in the previous tests by either replacing or damaging it, as illustrated in figure. Specifically, one propeller is either replaced with a different model or trimmed by approximately 1/4 of its original length. This modification introduces additional disturbance forces in the X-Y plane during flight due to the induced thrust asymmetry. Figure (a) shows that all three algorithms perform well under the paddle replacement scenario, highlighting the advantage of disturbance state augmentation. In contrast, when one propeller is trimmed by one quarter, figure (b) clearly shows that the Crazyflie becomes unstable and crashes under the REKF, whereas FR-MHE and RMHE complete the task successfully with comparable performance.