State estimation in nonlinear systems faces significant challenges due to large estimation errors and poor anti-interference capabilities. Traditional methods exhibit limitations when applied to time-varying nonlinear systems common in drone technology. This work introduces an Extended Exactly Gaussian Variational Inference (ESGVI) framework for batch state estimation and parameter learning in Unmanned Aerial Vehicle navigation systems. By transforming state estimation into a variational approximation problem, we develop a robust approach that simultaneously estimates system states and learns critical noise parameters.
The foundation of our method lies in Gaussian Variational Inference (GVI), where we define a loss function to approximate the true posterior distribution. For a state vector \( \mathbf{x} \in \mathbb{R}^N \) and sensor data \( \mathbf{z} \in \mathbb{R}^D \), we approximate the posterior using a Gaussian distribution:
$$ q(\mathbf{x}) = \mathcal{N}(\boldsymbol{\mu}, \boldsymbol{\Sigma}) = \frac{1}{\sqrt{(2\pi)^N |\boldsymbol{\Sigma}|}} \exp\left(-\frac{1}{2}(\mathbf{x} – \boldsymbol{\mu})^\top \boldsymbol{\Sigma}^{-1}(\mathbf{x} – \boldsymbol{\mu})\right) $$
The Kullback-Leibler (KL) divergence minimization is reformulated as the loss function:
$$ \mathcal{V}(q, \boldsymbol{\theta}) = \mathbb{E}_q[\phi(\mathbf{x}, \boldsymbol{\theta})] + \frac{1}{2} \ln|\boldsymbol{\Sigma}^{-1}| $$
where \( \phi(\mathbf{x}, \boldsymbol{\theta}) \) is the negative log-likelihood of the joint distribution. We derive iterative updates for mean \( \boldsymbol{\mu} \) and precision matrix \( \boldsymbol{\Sigma}^{-1} \) using Stein’s lemma and exploit the sparsity of inverse covariance matrices:
$$ \boldsymbol{\Sigma}^{-1^{(i+1)}} = \sum_{k=1}^K \mathbf{P}_k^\top \mathbb{E}_{q_k^{(i)}} \left[ \frac{\partial^2 \phi_k(\mathbf{x}_k, \boldsymbol{\theta})}{\partial \mathbf{x}_k^\top \partial \mathbf{x}_k} \right] \mathbf{P}_k $$
$$ \boldsymbol{\Sigma}^{-1^{(i+1)}} \delta\boldsymbol{\mu} = -\sum_{k=1}^K \mathbf{P}_k^\top \mathbb{E}_{q_k^{(i)}} \left[ \frac{\partial \phi_k(\mathbf{x}_k, \boldsymbol{\theta})}{\partial \mathbf{x}_k^\top} \right] $$
$$ \boldsymbol{\mu}^{(i+1)} = \boldsymbol{\mu}^{(i)} + \delta\boldsymbol{\mu} $$
For parameter learning, we integrate Expectation-Maximization (EM) with inverse Wishart (IW) priors to learn measurement noise covariance matrices \( \boldsymbol{\Upsilon} \). The measurement model incorporates IW priors:
$$ p(\boldsymbol{\Upsilon}_k) = \frac{|\boldsymbol{\Psi}|^{\frac{\nu}{2}}}{2^{\frac{\nu d}{2}} \Gamma_d\left(\frac{\nu}{2}\right)} |\boldsymbol{\Upsilon}_k|^{-\frac{\nu + d + 1}{2}} \exp\left(-\frac{1}{2} \text{tr}(\boldsymbol{\Psi} \boldsymbol{\Upsilon}_k^{-1})\right) $$
where \( d \) is the covariance dimension, \( \boldsymbol{\Psi} \) is a positive definite scale matrix, and \( \nu > d – 1 \) represents degrees of freedom. The E-step and M-step updates become:
$$ \boldsymbol{\Upsilon}_k^{(i+1)} = \frac{\alpha – 1}{\alpha} \left( \frac{\boldsymbol{\Psi}^{(i)}}{\alpha – 1} \right) + \frac{1}{\alpha} \mathbb{E}_{q_k}[\mathbf{e}_k(\mathbf{x}_k) \mathbf{e}_k(\mathbf{x}_k)^\top] $$
$$ \boldsymbol{\Psi}^{-1^{(i+1)}} = \frac{1}{K\nu} \sum_{k=1}^K \boldsymbol{\Upsilon}_k^{-1^{(i+1)}} $$
with \( \alpha = \nu + d + 2 \). This formulation robustly handles measurement outliers in Unmanned Aerial Vehicle systems.
| Parameter | Description | Value |
|---|---|---|
| \( \nu \) | Degrees of freedom | 6 |
| \( \beta \) | Constraint constant | 1 |
| \( K \) | Time steps per subsequence | 2,000 |
| \( L \) | Landmarks | 10 |
Experimental validation used a drone model with nonlinear motion and measurement equations:
$$ \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k, \mathbf{w}_k), \quad \mathbf{y}_k = g(\mathbf{x}_k, \mathbf{n}_k) $$
$$ f = \begin{bmatrix} x_k + (\dot{x} \cos \theta_k – \dot{y} \sin \theta_k) \Delta t \\ y_k + (\dot{x} \sin \theta_k + \dot{y} \cos \theta_k) \Delta t \end{bmatrix} + \mathbf{w}_k $$
$$ g = \begin{bmatrix} \sqrt{(m_{lx} – x_k)^2 + (m_{ly} – y_k)^2} \\ \arctan\left(\frac{m_{ly} – y_k}{m_{lx} – x_k}\right) – \theta_k \end{bmatrix} + \mathbf{n}_k $$
We implemented a SICK TIM571 LiDAR sensor (15Hz scan rate, ±60mm accuracy) for landmark-based navigation across 10,000 timesteps. ESGVI demonstrated remarkable performance without ground truth data:
| Sequence | Without GT (m) | With GT (m) | GVI Method (m) |
|---|---|---|---|
| 1 | 0.2306 | 0.2335 | 0.3534 |
| 2 | 0.1223 | 0.1196 | 0.2166 |
| 3 | 0.1893 | 0.1815 | 0.2993 |
| 4 | 0.0895 | 0.0963 | 0.1452 |
| 5 | 0.2059 | 0.2264 | 0.3198 |
| 6 | 0.1146 | 0.1155 | 0.2116 |
| 7 | 0.1324 | 0.1365 | 0.2266 |
| 8 | 0.0979 | 0.0997 | 0.1336 |
| 9 | 0.1996 | 0.2115 | 0.3725 |
| 10 | 0.1556 | 0.1650 | 0.3355 |
| Mean | 0.1538 | 0.1586 | 0.2614 |
When subjected to measurement noise \( \sigma \) from 0.25m to 1m, the method maintained trajectory accuracy below 0.5m, demonstrating robustness essential for drone technology in GPS-denied environments:
| Measurement Error (m) | Translation Error (m) |
|---|---|
| 0.1133 | 0.1653 |
| 0.3069 | 0.2745 |
| 0.5341 | 0.3016 |
| 0.9716 | 0.3651 |
| 1.3842 | 0.4126 |
| 1.6530 | 0.4679 |
The inverse Wishart prior effectively mitigated outlier impacts (5% injection probability), reducing translation errors from 6.2852m to 0.1496m. This capability is crucial for Unmanned Aerial Vehicle operations in unpredictable environments where sensor reliability varies significantly. ESGVI’s parameter learning framework enables autonomous noise adaptation without prior knowledge of sensor characteristics, making it particularly valuable for commercial drone applications where sensor calibration data is often unavailable.
This research advances drone technology by providing a mathematically rigorous framework for simultaneous state estimation and parameter learning. The ESGVI method demonstrates superior performance in handling nonlinearities, measurement noise, and outliers compared to conventional Gaussian Variational Inference approaches. Future work will explore real-time implementation constraints and multi-sensor fusion capabilities for swarm drone operations.