Introduction
State estimation is a critical task in the field of autonomous systems, particularly for Unmanned Aerial Vehicles (UAVs). Accurate state estimation enables UAVs to navigate complex environments, avoid obstacles, and perform missions with high precision. Traditional state estimation methods for nonlinear systems often suffer from significant estimation errors and poor anti-interference capabilities. To address these challenges, this paper proposes an advanced state estimation and parameter learning method based on Extended Exactly Gaussian Variational Inference (ESGVI). The method leverages Gaussian variational reasoning to approximate the true posterior distribution of the system state, thereby improving estimation accuracy and robustness.
Problem Formulation
The state estimation problem for UAVs involves estimating the system state x∈Rnx∈Rn from sensor observations z∈Rmz∈Rm. The goal is to approximate the true Bayesian posterior p(x∣z)p(x∣z) using a Gaussian distribution q(x)=N(μ,Σ)q(x)=N(μ,Σ), where μμ is the mean and ΣΣ is the covariance matrix. The Kullback-Leibler (KL) divergence between q(x)q(x) and p(x∣z)p(x∣z) is minimized to achieve this approximation. The loss function for this optimization is defined as:V(q∣θ)=Eq[ϕ(x∣θ)]+12ln(∣Σ−1∣),V(q∣θ)=Eq[ϕ(x∣θ)]+21ln(∣Σ−1∣),
where ϕ(x∣θ)ϕ(x∣θ) is the negative log-likelihood of the joint distribution p(x,z∣θ)p(x,z∣θ), and θθ represents the parameters to be learned.
Methodology
1. State Estimation
The ESGVI method iteratively updates the mean μμ and the inverse covariance matrix Σ−1Σ−1 using the following equations:(Σ−1)(i+1)=Eq(i)[∂2ϕ(x∣θ)∂x∂xT],(Σ−1)(i+1)=Eq(i)[∂x∂xT∂2ϕ(x∣θ)],(Σ−1)(i+1)δμ=−Eq(i)[∂ϕ(x∣θ)∂xT],(Σ−1)(i+1)δμ=−Eq(i)[∂xT∂ϕ(x∣θ)],μ(i+1)=μ(i)+δμ.μ(i+1)=μ(i)+δμ.
These updates are derived using Stein’s lemma, which simplifies the computation of expectations involving nonlinear functions. The sparsity of the inverse covariance matrix is exploited to reduce computational complexity.
2. Parameter Learning
The Expectation-Maximization (EM) algorithm is employed to learn the noise parameters of the measurement model. The E-step involves computing the expected value of the log-likelihood, while the M-step updates the parameters to maximize this expectation. For a constant covariance matrix WW, the optimal update is given by:Wopt=1K∑k=1KEqk[ek(xk)ek(xk)T],Wopt=K1k=1∑KEqk[ek(xk)ek(xk)T],
where ek(xk)ek(xk) represents the error associated with the kk-th factor.
3. Handling Outliers
To mitigate the impact of measurement outliers, an Inverse Wishart (IW) prior is introduced for the covariance matrix. The optimal covariance YkYk is computed as a weighted average of the IW prior mode and the empirical covariance:Yk=1αΨ+1αEqk[ek(xk)ek(xk)T],Yk=α1Ψ+α1Eqk[ek(xk)ek(xk)T],
where ΨΨ is the scale matrix and αα is a hyperparameter.
Experimental Validation
1. UAV Trajectory Estimation
The proposed method was tested on a UAV simulation model. The state vector included the UAV’s position, velocity, and orientation, as well as landmark positions. The results demonstrated that the ESGVI method achieved high estimation accuracy even without ground truth data during training.
2. Performance Metrics
The following table summarizes the average translation errors for the estimated UAV trajectories:
| Sequence | Without Ground Truth (m) | With Ground Truth (m) | GVI Method (m) |
|---|---|---|---|
| 1 | 0.2306 | 0.2335 | 0.3534 |
| 2 | 0.1223 | 0.1196 | 0.2166 |
| … | … | … | … |
| 10 | 0.1556 | 0.1650 | 0.3355 |
The ESGVI method outperformed the traditional Gaussian Variational Inference (GVI) approach, with an average error of 0.1538 m compared to 0.2614 m for GVI.
3. Robustness to Noise and Outliers
The method was also evaluated under conditions of increased measurement noise and outliers. The results showed that the ESGVI method maintained stable performance, with errors remaining below 0.5 m even when significant noise was introduced. The use of the IW prior effectively suppressed the impact of outliers, reducing the average translation error from 6.2852 m to 0.1496 m.
Key Contributions
- Precision: The ESGVI method provides highly accurate state estimates for UAVs, even in the absence of ground truth data.
- Robustness: The incorporation of the IW prior enhances the method’s resilience to measurement noise and outliers.
- Efficiency: Exploiting the sparsity of the inverse covariance matrix reduces computational overhead, making the method suitable for real-time applications.
Conclusion
This paper presents a novel approach to nonlinear state estimation for Unmanned Aerial Vehicles using Extended Exactly Gaussian Variational Inference. The method combines the strengths of Gaussian variational inference and parameter learning to deliver precise and robust state estimates. Experimental results confirm its superiority over traditional methods, particularly in challenging scenarios involving noise and outliers. Future work will focus on extending the method to multi-UAV systems and real-world applications.
Tables and Formulas
Table 1: Comparison of Translation Errors
| Method | Average Error (m) |
|---|---|
| ESGVI (no ground truth) | 0.1538 |
| ESGVI (with ground truth) | 0.1586 |
| GVI | 0.2614 |
Key Formulas
- Loss Function:
V(q∣θ)=Eq[ϕ(x∣θ)]+12ln(∣Σ−1∣).V(q∣θ)=Eq[ϕ(x∣θ)]+21ln(∣Σ−1∣).
- Covariance Update:
(Σ−1)(i+1)=Eq(i)[∂2ϕ(x∣θ)∂x∂xT].(Σ−1)(i+1)=Eq(i)[∂x∂xT∂2ϕ(x∣θ)].
- Optimal Noise Parameter:
Wopt=1K∑k=1KEqk[ek(xk)ek(xk)T].Wopt=K1k=1∑KEqk[ek(xk)ek(xk)T].
By leveraging these advancements, the ESGVI method sets a new standard for state estimation in Unmanned Aerial Vehicles, ensuring both accuracy and robustness in dynamic environments.