Attitude Estimation via EKF, UKF, ESEKF and EEKF
Attitude estimation refers to the problem of finding the angular position of an object with respect to a global frame. Estimating attitude is a fundamental task across many domains, from robotics to sports analytics. For instance, in the autonomous navigation of a quadrotor, the first requirement is to stabilize the vehicle, which in turn demands precise knowledge of its attitude. Among the various sensing modalities available for attitude estimation, the most widely used is the Inertial Measurement Unit (IMU). The core components of an IMU are gyroscopes, which measure the three-dimensional angular velocity of the object, and accelerometers, which measure the three-dimensional specific force acting on the object.
In this project, we designed and implemented four Kalman-type filters for the attitude estimation problem:
- Extended Kalman filter (EKF)
- Error State EKF (ESEKF)
- Unscented Kalman filter (UKF)
- Enhanced EKF (EEKF)
We proposed EEKF that keeps the filter statistically consistent by solving a small semidefinite program whenever its innovations look suspicious. We implement these filters on real-world data (EuRoC mav dataset) and on synthetic data (IMU plus magnetometer data).
The Enhanced EKF
A filter is consistent when the uncertainty it reports matches the errors it actually makes. This can be checked online through the innovations. At time step \(k\), the innovation \(z_k = y_k - h(\hat x_{k\mid k-1})\) is the difference between the measurement and its prediction, and \(S_k = H_k P_{k\mid k-1} H_k^\top + R\) is the innovation covariance predicted by the filter, where \(H_k\) is the measurement Jacobian, \(P_{k\mid k-1}\) the predicted state covariance and \(R\) the measurement noise covariance. The normalized innovation squared (NIS) and its running average (ANIS) are
\[\epsilon_k^2 = z_k^\top S_k^{-1} z_k, \qquad \bar\epsilon_k^2 = \frac{1}{k}\sum_{i=1}^{k} \epsilon_i^2 .\]For a consistent filter, \(\bar\epsilon_k^2\) is approximately normal with mean \(n_y\) (the measurement dimension) and variance \(2n_y/k\), so with \(95\%\) confidence it stays in the band
\[n_y - 2\sqrt{2n_y/k} \;\le\; \bar\epsilon_k^2 \;\le\; n_y + 2\sqrt{2n_y/k} .\]Since \(\bar\epsilon_k^2 = \big((k-1)\,\bar\epsilon_{k-1}^2 + \epsilon_k^2\big)/k\), this band translates into bounds \(\bar\tau_l \le \epsilon_k^2 \le \bar\tau_u\) on the current NIS, with
\[\bar\tau_u = k\Big(n_y + 2\sqrt{2n_y/k}\Big) - (k-1)\,\bar\epsilon_{k-1}^2\]and \(\bar\tau_l\) defined in the same way with a minus sign. When the EKF becomes overconfident, \(\epsilon_k^2\) exceeds \(\bar\tau_u\). The EEKF then replaces \(R\) with a new matrix \(X\) chosen so that \(z_k^\top (H_k P_{k\mid k-1} H_k^\top + X)^{-1} z_k \le \bar\tau_u\). By a Schur complement, this condition is a linear matrix inequality in \(X\), so \(X\) is found by solving a small semidefinite program at that time step:
\[\begin{bmatrix} H_k P_{k\mid k-1} H_k^\top + X & z_k \\ z_k^\top & \bar\tau_u \end{bmatrix} \succeq 0, \qquad X \succ 0 .\]A typical result is shown below for heavy-tailed (Student’s-t) magnetometer noise. The black lines show the ideal ANIS mean and its \(95\%\) band; the colored line and shaded region show each filter’s empirical mean and \(95\%\) spread. For the EKF, ESEKF and UKF the spread is far wider than the ideal band, whereas the EEKF’s spread closely matches it. Its mean sits slightly below the ideal value because the lower bound of the test is not enforced.




Click any figure to enlarge it.
Limitations. The lower bound of the consistency test is non-convex, so it is currently dropped: the EEKF corrects overconfidence but not underconfidence. Solving a semidefinite program at each corrected step is also computationally expensive, especially when the underlying EKF is often inconsistent, which limits its use on resource-constrained hardware.
Results on real flight data
On the EuRoC MAV dataset, which provides IMU data and motion-capture ground truth from a drone flying indoors but no magnetometer, yaw is unobservable and none of the filters can track it. Pitch and roll show chattering, and the EKF and UKF become inconsistent, because the drone’s abrupt maneuvers violate the assumption that body acceleration is negligible compared with gravity; the EEKF stays consistent. Removing the body acceleration from the data confirms this: the chattering disappears and all filters become consistent.








For the simulation results under Gaussian and heavy-tailed noise, comparisons with the posterior Cramér–Rao bound, filter efficiency, computation times and further discussion, see the full report.
Takeaways
- Enforcing consistency through convex optimization gives a filter that is both accurate and statistically trustworthy, at a computational price that depends on how often the base filter is inconsistent.
- The negligible-acceleration assumption is critical; in practice, full pose estimation (position and orientation) avoids it and yields better attitude estimates.
- Natural next steps are a convex relaxation of the lower consistency bound and replacing the feasibility problem with an optimization objective.
Citation
If you use this project in your research, please cite:
M. H. Yoosefian Nooshabadi, L. Lessard. “State Estimation for Linear Systems with Non-Gaussian Measurement Noise via Dynamic Programming.” IEEE 64th Conference on Decision and Control (CDC), pp. 5612–5617, 2025.
@inproceedings{nooshabadi2025state,
title={State Estimation for Linear Systems with Non-Gaussian Measurement Noise via Dynamic Programming},
author={Nooshabadi, Mohammad Hussein Yoosefian and Lessard, Laurent},
booktitle={2025 IEEE 64th Conference on Decision and Control (CDC)},
pages={5612--5617},
year={2025},
organization={IEEE}
}