无人机非线性状态估计:扩展精确高斯变分推理学习方法

刘久富，Elishahidi S.B.Mvungi，汪恒宇，解晖，刘向武，王志胜; LIU Jiufu; Elishahidi S.B.Mvungi; WANG Hengyu; XIE Hui; LIU Xiangwu; WANG Zhisheng

无人机非线性状态估计：扩展精确高斯变分推理学习方法

doi: 10.11887/j.cn.202503015

刘久富， Elishahidi S.B.Mvungi ，汪恒宇，解晖，刘向武，王志胜

南京航空航天大学自动化学院，江苏南京 211106

基金项目: 国家自然科学基金资助项目（61473144）

详细信息

作者简介

刘久富（1970—），男，江苏淮安人，副教授，博士，硕士生导师，E-mail: liujiufu2@126.com

中图分类号: TP181

文献标识码: A

文章编号: 1001-2486(2025)03-141-10

Nonlinear state estimation for unmanned aerial vehicles: extended exactly Gaussian variational inference learning method

LIU Jiufu ， Elishahidi S.B.Mvungi ， WANG Hengyu ， XIE Hui ， LIU Xiangwu ， WANG Zhisheng

College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211106 , China

摘要

针对在对时变非线性系统进行状态估计以及参数学习时估计误差大、抗干扰能力差等问题，提出一种面向非线性系统的精确稀疏高斯变分推理的批量状态估计与参数学习方法。基于高斯变分推理提出损失函数，状态估计问题转化为对真实后验近似问题，并引入需要学习的参数。对状态概率分布的参数使用高斯-牛顿式优化器的方法进行迭代更新，利用Stein引理、协方差矩阵的稀疏性及高斯容积方法得到完整的状态估计迭代方案。使用期望最大化学习测量模型的噪声参数，同时引入逆Wishart先验减少测量噪声和离群值对参数学习以及状态估计结果的影响。通过对无人机仿真模型进行模拟实验，在不加入无人机运动以及测量噪声真实值的情况下，对无人机轨迹能够进行精确的估计，且有效抑制测量噪声和测量离群值对轨迹估计精度带来的影响。

关键词

精确稀疏高斯变分推理 / 非线性系统批量状态估计 / 参数学习 / 期望最大化方法 / 轨迹估计

Abstract

Aiming at the problems of large estimation error and poor anti-interference ability in state estimation and parameter learning of time-varying nonlinear systems, a batch state estimation and parameter learning method for accurate sparse Gaussian variational inference for nonlinear systems was proposed. A loss function was proposed based on Gaussian variational reasoning, and the state estimation problem was transformed into an approximation problem to the true posterior, and parameters that need to be learned were introduced. The parameters of the state probability distribution were iteratively updated using the Gauss-Newton optimizer method, and a complete state estimation iterative scheme was obtained by using Stein′s lemma, the sparsity of the covariance matrix and the Gaussian volume method. The noise parameters of the measurement model were learned through expectation maximization, and the inverse Wishart prior was introduced to reduce the influence of measurement noise and outliers on parameter learning and state estimation results. The simulation experiment was carried out on the UAV simulation model, and the UAV trajectory can be accurately estimated without adding the UAV movement and the real value of the measurement noise, and the impact of measurement noise and measurement outliers on trajectory estimation accuracy is effectively suppressed.

Keywords

exactly sparse Gaussian variational inference / batch state estimation for nonlinear systems / parameter learning / expectation maximization method / trajectory estimation

1 ESGVI状态估计方法 1.1 损失函数 1.2 状态估计 1.2.1 迭代更新 1.2.2 稀疏性与边缘采样 1.3 ESGVI状态估计算法具体实现过程 2 ESGVI参数学习方法 2.1 期望最大化分解 2.1.1 E步骤 2.1.2 M步骤 2.2 白噪声加速度先验参数学习 2.3 协方差的逆Wishart先验 2.4 ESGVI参数学习算法具体实现过程 3 实例分析与验证 3.1 无人机轨迹估计与噪声模型参数学习 3.2 轨迹生成 3.3 误差分析 3.4 测量噪声对参数学习的影响 3.5 测量离群值对轨迹估计的影响 4 结论

状态估计^[1-2]的目的是通过系统中传感器观测到的数据估计出无法被观测到的状态。当面对时变非线性系统时，传统的状态估计方法存在误差大、算法迭代次数过多等缺陷。本文方法拓展了精确高斯变分推理学习方法在非线性状态估计领域的理论研究，并对传统状态估计方法存在的缺陷进行改善。

时变非线性系统的状态估计^[3-7]可以看作是对非线性系统的识别问题。高斯变分推理（Gaussian variational inference，GVI）是将高斯推断以及变分推理结合的状态估计方法，GVI方法从待估计状态的先验概率分布出发，利用传感器的观测值修正先验概率分布，得到更加精确的后验概率分布，然后利用损失函数对状态概率分布的均值以及协方差的变分对该分布进行迭代优化，得到接近状态真实后验的概率分布。状态估计问题中，文献^[3]提出了一种基于在线的自适应滤波器（online-based adaptive filter， OBAF），用于状态滤波和对具有估计约束、非线性变换的干扰和噪声的非线性离散动态模型的状态进行估计。在OBAF中，将噪声和干扰离散化，通过置信度检测非期望状态估计值避免离群值的干扰，适用于缺少观测或估计约束的状态估计问题。文献^[4]使用无迹卡尔曼滤波器（unscented Kalman filter，UKF）对非线性系统进行状态估计，经典的UKF算法要求过程噪声和测量噪声相互独立，由于传感器精度等问题，会发生数据丢失或测量不可靠的情况，导致估计误差过大。文献^[5]针对非线性动态系统的状态估计问题，提出了一种基于扩展卡尔曼滤波器（extended Kalman filter，EKF）的连续离散状态估计器，对于高度非线性动态系统或中低采样频率，EKF提供了更好的精度和收敛性。文献^[6]考虑非线性状态估计受到线性和非线性矩阵不等式形式的不等式约束，重写用于导出卡尔曼滤波器的标准最大似然目标函数，通过求解具有受线性矩阵不等式约束的线性目标函数的约束优化问题来找到卡尔曼增益，所提出的约束估计方法应用于EKF和sigma点卡尔曼滤波器（sigma point Kalman filter，SPKF）框架。

本文在GVI方法的基础上提出精确稀疏高斯变分推理（exactly sparse Gaussian variational inference， ESGVI）方法对非线性系统进行批量状态估计。ESGVI方法利用Stein引理^[8]以及逆协方差矩阵的稀疏性，对所求期望进行边缘采样，降低均值以及协方差在迭代过程中的计算量，同时提高批量状态估计的精确性，进一步近似完整的贝叶斯后验。同时在ESGVI框架中对噪声模型进行参数学习^[9-14]，使用期望最大化方法从传感器数据中学习估计所需的模型参数，实现更加精确的状态估计。通过在协方差中引入逆Wishart先验对时变的协方差进行学习，并且抑制测量噪声过大以及离群值对状态估计精度带来的影响。

本文提出的ESGVI状态估计与参数学习方法应用到无人机轨迹估计中^[15-26]，通过学习传感器测量噪声参数减小轨迹估计误差。并且在仿真过程中测试了训练中是否添加真实值对估计结果的影响、算法在测量噪声过大时的估计精度表现以及对测量离群值影响的抑制能力。

1 ESGVI状态估计方法

1.1 损失函数

变分推理方法目的是将真实贝叶斯后验p（x|z）和后验的估计q（x）之间的库尔贝克-莱布勒（Kullback-Leibler，KL）散度最小化。x是系统状态（

x \in R^{N}

），z是传感器观测数据（

z \in R^{D}

）。假设状态x满足如下所示的高斯分布：

\begin{matrix} q (x) = N (μ, Σ) \\ = \frac{1}{\sqrt{(2 π)^{N} | Σ |}} e x p [- \frac{1}{2} (x - μ)^{T} Σ^{- 1} (x - μ)] \end{matrix}

(1)

式中，μ为均值，Σ为协方差矩阵。

使用S表示后验估计q（x）对真实贝叶斯后验p（x|z）的近似程度：

\begin{matrix} S = - \int_{- \infty}^{\infty} q (x) l n (\frac{p (x ∣ z)}{q (x)}) d x \\ = E_{p} [l n q (x) - l n p (x ∣ z)] \end{matrix}

(2)

式中，E_p[·]表示贝叶斯后验p（x|z）的期望。

将式（2）的KL散度写成：

\begin{matrix} S = E_{q} [- l n p (x, z)] - \\ \frac{1}{2} l n [(2 π e)^{N} | Σ |] + l n p (z) \end{matrix}

(3)

式中，lnp（z）是常数，E_q[·]表示后验估计q（x）的期望。

将KL散度最小化，转化为损失函数，定义为：

V (q) = E_{q} [ϕ (x)] + \frac{1}{2} l n (|Σ^{- 1}|)

(4)

引入系统中希望学习的参数θ，将损失函数写为：

V (q ∣ θ) = E_{q} [ϕ (x ∣ θ)] + \frac{1}{2} l n (|Σ^{- 1}|)

(5)

1.2 状态估计

1.2.1 迭代更新

式（5）的损失函数关于μ以及Σ^-1的微分如下：

\frac{\partial V (q ∣ θ)}{\partial μ^{T}} = Σ^{- 1} E_{q} [(x - μ) ϕ (x ∣ θ)]

(6)

\begin{matrix} \frac{\partial^{2} V (q ∣ θ)}{\partial μ^{T} \partial μ} = Σ^{- 1} E_{q} [(x - μ) (x - μ)^{T} ϕ (x ∣ θ)] Σ^{- 1} - \\ Σ^{- 1} E_{q} [ϕ (x ∣ θ)] \end{matrix}

(7)

\begin{matrix} \frac{\partial V (q ∣ θ)}{\partial Σ^{- 1}} = - \frac{1}{2} E_{q} [(x - μ) (x - μ)^{T} ϕ (x ∣ θ)] + \\ \frac{1}{2} Σ E_{q} [ϕ (x ∣ θ)] + \frac{1}{2} Σ \end{matrix}

(8)

联立式（7）~（8）得到：

\frac{\partial^{2} V (q ∣ θ)}{\partial μ^{T} \partial μ} = Σ^{- 1} - 2 Σ^{- 1} \frac{\partial V (q ∣ θ)}{\partial Σ^{- 1}} Σ^{- 1}

(9)

将式（9）中微分

\frac{\partial V （ q ∣ θ ）}{\partial Σ^{- 1}}

设为零，得到Σ^-1的更新：

{(Σ^{- 1})}^{(i + 1))} = {\frac{\partial^{2} V (q ∣ θ)}{\partial μ^{T} \partial μ}|}_{q^{(i)}}

(10)

式中，i表示迭代次数。

得到Σ^-1的变化量：

δ Σ^{- 1} = - {2 {(Σ^{- 1})}^{(i)} \frac{\partial V (q ∣ θ)}{\partial Σ^{- 1}}|}_{q^{(i)}} {(Σ^{- 1})}^{(i)}

(11)

假设f（·）是任意非线性二阶可微函数，引入Stein引理：

E_{q} [(x - μ) f (x)] \equiv Σ E_{q} [\frac{\partial f (x)}{\partial x^{T}}]

(12)

\begin{matrix} E_{q} [(x - μ) (x - μ)^{T} f (x)] \\ \equiv Σ E_{q} [\frac{\partial^{2} f (x)}{\partial x^{T} \partial x}] Σ + Σ E_{q} [f (x)] \end{matrix}

(13)

利用Stein引理代换式（6）~（8）中的损失函数微分项，得到迭代更新：

{(Σ^{- 1})}^{(i + 1)} = E_{q^{(i)}} [\frac{\partial^{2}}{\partial x^{T} \partial x} ϕ (x ∣ θ)]

(14)

{(Σ^{- 1})}^{(i + 1)} δ μ = - E_{q^{(i)}} [\frac{\partial}{\partial x^{T}} ϕ (x ∣ θ)]

(15)

μ^{(i + 1)} = μ^{(i)} + δ μ

(16)

1.2.2 稀疏性与边缘采样

假设φ（x|θ）可以被分解，将其负对数似然写为：

\{\begin{matrix} ϕ (x ∣ θ) = \sum_{k = 1}^{K} ϕ_{k} (x_{k} ∣ θ) \\ ϕ_{k} (x_{k} ∣ θ) = - l n p (x_{k}, z_{k} ∣ θ) \end{matrix}

(17)

其中，x_k是x中与第k个因子关联的状态子集，z_k是z中与第k个因子关联的数据子集。

引入投影矩阵P_k，其作用是从状态集x中提取状态子集x_k，映射关系为：

x_{k} = P_{k} x

(18)

高斯边缘化等价于投影，使得：

q_{k} (x_{k}) = N (μ_{k}, Σ_{k k}) = N (P_{k} μ, P_{k} Σ P_{k}^{T})

(19)

将Σ^-1写成：

Σ^{- 1} = \sum_{k = 1}^{K} P_{k}^{T} E_{q_{k}} [\frac{\partial^{2}}{\partial x_{k}^{T} \partial x_{k}} ϕ_{k} (x_{k} ∣ θ)] P_{k}

(20)

使用子块

Σ_{k k} = P_{k} Σ P_{k}^{T}

计算每个因子的期望，将结果返回到Σ^-1的对应元素中。

利用分解式，并且反向利用Stein引理以达到避免求微分、简化计算以及减少可微性要求的目的，最后使用高斯容积方法近似积分，得到迭代更新：

{(Σ^{- 1})}^{(i + 1)} = \sum_{k = 1}^{K} P_{k}^{T} E_{q_{k}^{(i)}} [\frac{\partial^{2} ϕ_{k} (x_{k} ∣ θ)}{\partial x_{k}^{T} \partial x_{k}}]

(21)

{(Σ^{- 1})}^{(i + 1)} δ μ = - \sum_{k = 1}^{K} P_{k}^{T} E_{q_{k}^{(i)}} [\frac{\partial ϕ_{k} (x_{k} ∣ θ)}{\partial x_{k}^{T}}]

(22)

μ^{(i + 1)} = μ^{(i)} + δ μ

(23)

1.3 ESGVI状态估计算法具体实现过程

ESGVI状态估计算法如算法1所示。

算法1 ESGVI状态估计算法

Alg.1 ESGVI state estimation algorithm

2 ESGVI参数学习方法

2.1 期望最大化分解

将数据的负对数似然进行期望最大化分解：

\begin{matrix} - l n p (z ∣ θ) = \int_{- \infty}^{\infty} q (x) l n [\frac{p (x ∣ z, θ)}{q (x)}] d x - \\ \int_{- \infty}^{\infty} q (x) l n [\frac{p (x, z ∣ θ)}{q (x)}] d x \end{matrix}

(24)

式中，

- \int_{- \infty}^{\infty} q （ x ） l n [\frac{p （ x ， z ∣ θ ）}{q （ x ）}] d x

为证据下界（evidence lower bound，ELBO）。

期望最大化（expectation maximization，EM）方法在两个步骤中进行迭代，即期望步骤（E步骤）以及最大化步骤（M步骤）。

2.1.1 E步骤

当式（5）中的参数θ为协方差矩阵W时，负对数似然变为：

ϕ (x ∣ W) = \frac{1}{2} [e (x)^{T} W^{- 1} e (x) - l n (|W^{- 1}|)]

(25)

式中，e表示待估计的状态向量。

利用Jensen不等式，并且去掉在E步骤中为常数的ln（|W^-1|）项，得到：

E_{q} [e (x)]^{T} W^{- 1} E_{q} [e (x)] ⩽ E_{q} [e (x)^{T} W^{- 1} e (x)]

(26)

利用式（26）中的关系，定义E步骤中的损失函数：

V^{'} (q) = \frac{1}{2} E_{q} [e (x)]^{T} W^{- 1} E_{q} [e (x)] + \frac{1}{2} l n (|Σ^{- 1}|)

(27)

2.1.2 M步骤

当协方差矩阵恒定时，定义M步骤的损失函数：

V (q ∣ W) = E_{q} [ϕ^{m} (x ∣ W)] + \frac{1}{2} l n (|Σ^{- 1}|)

(28)

将负对数似然

ϕ^{m} （ x ∣ W ）

分解为：

\begin{matrix} ϕ^{m} (x ∣ W) = \sum_{k = 1}^{K} ϕ_{k}^{m} (x_{k} ∣ W) \\ = \sum_{k = 1}^{K} \frac{1}{2} [e_{k} {(x_{k})}^{T} W^{- 1} e_{k} (x_{k}) - l n (|W^{- 1}|)] \end{matrix}

(29)

式中，K因子被未知参数W所影响，W是一个恒定的协方差矩阵。

对式（28）求微分得到式（30），并将微分设为零，计算出最优W的最小结果，如式（31）所示。

\begin{matrix} \frac{\partial V (q ∣ W)}{\partial W} = \frac{\partial E_{q} [ϕ^{m} (x ∣ W)]}{\partial W} \\ = \frac{\partial E_{q} [\sum_{k = 1}^{K} ϕ_{k}^{m} (x_{k} ∣ W)]}{\partial W} = \sum_{k = 1}^{K} E_{q_{k}} [\frac{\partial ϕ_{k}^{m} (x_{k} ∣ W)}{\partial W}] \end{matrix}

(30)

W_{m i n} = \frac{1}{K} \sum_{k = 1}^{K} E_{q_{k}} [e_{k} (x_{k}) e_{k} {(x_{k})}^{T}]

(31)

2.2 白噪声加速度先验参数学习

本节将通过间接估计因子协方差的方法以实现参数学习。以状态的白噪声加速度（white-noise-on-acceleration，WNOA）动力先验为例，希望估计的参数为功率谱密度矩阵Q_C。定义：

(32)

其中：

表示高斯过程，T（t）∈SE（3），是用于表示无人机姿态的特殊欧几里得群；

ω （ t ） \in R^{6}

，是物体中心的速度；

ω （ t ） \in R^{6}

，是一个零均值白噪声高斯过程，算子∧将

R^{6}

中的元素转换到李代数向量空间se（3）。t_k时刻的状态x_k=（T_k，ω_k）。

WNOA中的先验因子为：

\begin{matrix} ϕ^{p} (x ∣ Q_{C}) = \sum_{k = 2}^{K} ϕ_{k}^{p} (x_{k - 1, k} ∣ Q_{C}) \\ = \sum_{k = 2}^{K} \frac{1}{2} (e_{p, k}^{T} Q_{k}^{- 1} e_{p, k} + l n |Q_{k}|) \end{matrix}

(33)

其中，

e_{p, k} = [\begin{matrix} l n {(T_{k} T_{k - 1}^{- 1})}^{\lor} - (t_{k} - t_{k - 1}) ω_{k - 1} \\ J^{- 1} [\ln {(T_{k} T_{k - 1}^{- 1})}^{\lor}] ω_{k} - ω_{k - 1} \end{matrix}]

(34)

J

表示w（t）的协方差矩阵与标准差矩阵之积。

先验Q_k的协方差为：

(35)

其中，

\otimes

是克罗内克内积。

计算式（5）中损失函数关于

Q_{C_{i ， j}}

的微分，

Q_{C_{i ， j}}

是矩阵Q_C中位于（i，j）位置的元素，可以得到：

\begin{matrix} \frac{\partial V (q ∣ θ)}{\partial Q_{C_{i, j}}} = \frac{1}{2} t r [\sum_{k = 2}^{K} E_{q_{k - 1, k}} (e_{p, k} e_{p, k}^{T}) (Q_{Δ t}^{- 1} \otimes 1_{i, j})] - \\ \frac{1}{2} (K - 1) d i m (Q_{Δ t}) Q_{C_{i, j}} \end{matrix}

(36)

q_k_-1，_k是两个连续时刻t_k_-1和t_k的边缘后验。将微分设为零，得到参数的最优估计值：

Q_{C_{i, j}} = \frac{t r [\sum_{k = 2}^{K} E_{q_{k - 1, k}} (e_{p, k} e_{p, k}^{T}) (Q_{Δ t}^{- 1} \otimes 1_{i, j})]}{(K - 1) d i m (Q_{Δ t})}

(37)

2.3 协方差的逆Wishart先验

本节通过将先验合并进一步扩展协方差估计。将联合似然函数重新表达为：

p (x, z, A) = p (x, z ∣ A) p (A)

(38)

式中将协方差矩阵视为一个随机变量A=（A₁，A₂，···，A_K）。将后验估计重新表示为高斯分布q（x）和协方差s（A）后验分布的乘积：

q^{'} (x) = q (x) s (A)

(39)

式中协方差的后验为：

s (A) = δ (A - Υ)

(40)

其中，δ（·）是狄拉克δ分布函数，

Υ

=（

Υ

₁，

Υ

₂，···，

Υ

_K）是最优协方差的设置值。

将式（24）的ELBO项进行简化：

\begin{matrix} - \iint q (x) s (A) l n [\frac{p (x, z ∣ A) p (A)}{q (x) s (A)}] d x d A \\ = - \int q (x) l n [p (x, z ∣ Υ) p (A = Υ)] d x + \\ \int q (x) l n q (x) d x + \int s (A) l n s (A) d A \end{matrix}

(41)

式中最后一项为狄拉克函数的微分熵，因为它与

Υ

无关，故将其从损失函数中去掉。

假设p（Α=

Υ

）因子为：

p (A = Υ) = \prod_{k = 1}^{K} p (A_{k} = Υ_{k})

(42)

在分解后的协方差因子上应用逆威沙特（inverse Wishart，IW）先验：

p (A_{k} = Υ_{k}) = \frac{| Ψ |^{\frac{ν}{2}}}{2^{\frac{ν d}{2}} Γ_{d} (\frac{ν}{2})} {|Υ_{k}|}^{- \frac{ν + d + 1}{2}} e x p [- \frac{1}{2} t r (Ψ Υ_{k}^{- 1})]

(43)

式中，d是

Υ

_k的维数，

Ψ \in R^{6 \times 6}

为比例矩阵且正定，ν＞d-1为自由度，Γ_d（·）为多元伽马函数。对比例矩阵参数Ψ进行估计，并将自由度ν视为元参数。

将因子设定为：

\begin{matrix} - l n (A = Υ) = \sum_{k = 1}^{K} - l n p (A_{k} = Υ_{k}) \\ = \sum_{k = 1}^{K} ϕ_{k}^{w} (Υ_{k} ∣ Ψ) = ϕ^{w} (Υ ∣ Ψ) \end{matrix}

(44)

舍去常数项，将式（28）写成：

\begin{matrix} V (q^{'} ∣ Υ, Ψ) \\ = \sum_{k = 1}^{K} E_{q_{k}} [ϕ_{k}^{m} (x_{k} ∣ Υ_{k}) + ϕ_{k}^{w} (Υ_{k} ∣ Ψ)] + \frac{1}{2} l n (|Σ^{- 1}|) \\ = \sum_{k = 1}^{K} E_{q_{k}} \{\frac{1}{2} [e_{k} {(x_{k})}^{T} Υ_{k}^{- 1} e_{k} (x_{k}) - l n (|Υ_{k}^{- 1}|)] - \\ \frac{α - 1}{2} l n |Υ_{k}^{- 1}| - \frac{ν}{2} l n | Ψ | + \frac{1}{2} t r (Ψ Υ_{k}^{- 1})\} + \frac{1}{2} l n (|Σ^{- 1}|) \end{matrix}

(45)

式中，α满足α=ν+d+2。

在E步骤中，保持Ψ不变以优化

Υ

_k。对式（45）求微分，得到：

\begin{matrix} \frac{\partial V (q^{'} ∣ Υ, Ψ)}{\partial Υ_{k}^{- 1}} = \frac{1}{2} E_{q_{k}} [e_{k} (x_{k}) e_{k} {(x_{k})}^{T}] - \\ \frac{1}{2} α Υ_{k} + \frac{1}{2} Ψ \end{matrix}

(46)

将微分设为零得到

Υ

_k的最优值：

\begin{matrix} Υ_{k} = \frac{1}{α} Ψ + \frac{1}{α} E_{q_{k}} [e_{k} (x_{k}) e_{k} {(x_{k})}^{T}] \\ = \frac{α - 1}{α} (\frac{Ψ}{α - 1}) + \frac{1}{α} E_{q_{k}} [e_{k} (x_{k}) e_{k} {(x_{k})}^{T}] \end{matrix}

(47)

式（47）是IW分布的模

\frac{Ψ}{α - 1}

以及式（38）在单边际因子下的最优静态协方差估计的加权平均。

在M步骤中，保持

Υ

不变以优化Ψ。对式（46）的损失函数求微分，得到：

\frac{\partial V (q^{'} ∣ Υ, Ψ)}{\partial Ψ} = \sum_{k = 1}^{K} (- \frac{ν}{2} Ψ^{- 1} + \frac{1}{2} A_{k}^{- 1})

(48)

将微分设为零得到Ψ^-1的最优值：

Ψ^{- 1} = \frac{1}{K ν} \sum_{k = 1}^{K} Υ_{k}^{- 1}

(49)

加入约束常数β以确保协方差的估计值不会趋向于正定边界，在训练过程中利用运动先验的噪声模型确定β的具体值。更新方式为：

Ψ_{c} \leftarrow {(β | Ψ |^{- 1})}^{\frac{1}{d}} Ψ

(50)

根据E步骤的式（47）以及M步骤的式（49）即可得到协方差A的最优估计值。

2.4 ESGVI参数学习算法具体实现过程

ESGVI参数学习算法如算法2所示。

算法2 ESGVI参数学习算法

Alg.2 ESGVI parameter learning algorithms

3 实例分析与验证

使用无人机运动模型对ESGVI参数估计方法的表现进行评估，在对无人机进行批量轨迹估计的同时对噪声模型进行参数学习。仿真实验将在Ubuntu系统中使用Eigen、Sophus以及Pangolin等软件的帮助下进行。

系统的运动方程以及观测方程为：

\{\begin{matrix} x_{k} = f (x_{k - 1}, u_{k}, w_{k}) \\ y_{k} = g (x_{k}, n_{k}) \end{matrix}

(51)

其中：函数f（·）为非线性运动模型，u_k为输入，w_k~N（0，Ω_k）为过程噪声；函数g（·）为非线性观测模型，n_k~N（0，Ψ_k）为测量噪声。f（·）的非线性部分以及g（·）为：

f = [\begin{matrix} x_{k} + (\dot{x} c o s θ_{k} - \dot{y} s i n θ_{k}) Δ t \\ y_{k} + (\dot{x} s i n θ_{k} + \dot{y} c o s θ_{k}) Δ t \end{matrix}] + w_{k}

(52)

g = [\begin{matrix} \sqrt{{(m_{l x} - x_{k})}^{2} + {(m_{l y} - y_{k})}^{2}} \\ a r c t a n (\frac{m_{l y} - y_{k}}{m_{l x} - x_{k}}) - θ_{k} \end{matrix}] + n_{k}

(53)

3.1 无人机轨迹估计与噪声模型参数学习

本节将在训练中不添加无人机运动状态真实值的情况下对无人机进行轨迹估计，同时对无人机运动模型的过程噪声以及传感器模型的测量噪声的协方差进行学习以实现更加精确的轨迹估计。无人机设置为定高飞行模式，由无人机在飞行过程中构建地标地图，并且同时估计无人机自身位置以及速度。整个数据集包含10 000个时间步长，将其分为1 000个时间步长的10个子序列。图1为问题的因子图。

图1无人机轨迹估计与参数学习因子图

Fig.1Factor diagram of UAV trajectory estimation and parameter learning

图中方框表示需要估计的变量，包括无人机状态x、测量噪声协方差

Υ

与地标位置m，黑点表示数据以及状态联合似然的因子，二元运动先验因子φ^p（x_k_-1，_k|Q_C）由参数Q_C决定，一元真实姿态因子φ^m（x_k|W_gt）由参数W_gt决定，因子φ^m（x_k|

Υ

_k）和φ^w（

Υ

_k|Ψ）用于IW先验测量姿态协方差，

Υ

取决于参数Ψ。本文方法可以在不需要虚线框中的真实值因子的情况下学习参数Q_C和Ψ。仿真实验将分别在加入以及不加入真实值的情况下进行轨迹估计，并将两种方法的结果进行对比。

需要估计的状态x为：

(54)

其中：x_k是无人机状态，

x_{k} ， y_{k} ， θ_{k} ， {\dot{x}}_{k} ， {\dot{y}}_{k} ， {\dot{θ}}_{k}

分别为无人机的x轴位置、y轴位置、偏航角以及x和y方向对应的速度与角速度；m_l是地标位置。对于每一个子序列，K=2 000，L=10。

对于线性先验因子有：

ϕ_{k} = \{\begin{matrix} \frac{1}{2} {(x_{0} - {\overset{ˇ}{x}}_{0})}^{T} {\overset{ˇ}{P}}^{- 1} (x_{0} - {\overset{ˇ}{x}}_{0}), k = 0 \\ \frac{1}{2} {(x_{k} - A x_{k - 1})}^{T} Q^{- 1} (x_{k} - A x_{k - 1}), k > 0 \end{matrix}

(55)

式中，

(56)

其中，T是离散时间采样周期，Q_C，_i是功率谱密度，

σ_{x}^{2} ， σ_{y}^{2} ， σ_{θ}^{2} ， σ_{\dot{x}}^{2} ， σ_{\dot{y}}^{2} ， σ_{\dot{θ}}^{2}

是对应的方差。非线性里程因子为：

ψ_{k} = \frac{1}{2} {(v_{k} - C_{k} x_{k})}^{T} S^{- 1} (v_{k} - C_{k} x_{k})

(57)

式中，

(58)

其中：v_k是速度，包含x轴上的分量u_k、y轴上的分量v_k以及角速度ω_k；

σ_{u}^{2} ， σ_{v}^{2} ， σ_{w}^{2}

是各分量对应的测量噪声方差。

非线性方位测量因子为：

ψ_{l, k} = \frac{{[β_{l, k} - g (m_{l}, x_{k})]}^{2}}{2 σ_{r}^{2}}

(59)

式中，β_l_，_k是第k个无人机位姿与第l个地标位置之间的方位测量值，σ²_r是测量噪声方差。

g (m_{l}, x_{k}) = a t a n 2 (y_{l} - y_{k}, x_{l} - x_{k}) - θ_{k}

(60)

结合式（55）、式（57）、式（59）得到数据和状态的联合似然函数：

\begin{matrix} - l n p (x, z) = \sum_{k = 0}^{K} ϕ_{k} + \sum_{k = 0}^{K} ψ_{k} + \\ \sum_{k = 1}^{K} \sum_{l = 1}^{L} ψ_{l, k} + c o n s t \end{matrix}

(61)

参数学习对应的损失函数为：

\begin{matrix} V (q^{'} ∣ Υ, Ψ, W_{g t}, Q_{C}) \\ = E_{q^{'}} [ϕ^{p} (x ∣ Q_{C})] + ϕ^{m} (x ∣ W_{g t}) + \\ ϕ^{m} (x ∣ Υ) + ϕ^{w} (Υ ∣ Ψ) + \frac{1}{2} l n (|Σ^{- 1}|) \end{matrix}

(62)

式中：

ϕ^{p} (x ∣ Q_{C})

是WNOA先验因子，定义由式（33）给出；

ϕ^{m} (x ∣ W_{g t})

是真实值因子，定义由式（29）给出；φ^m（x|

Υ

）和φ^w（

Υ

|Ψ）是测量协方差的IW先验因子，定义由式（29）、式（43）~（44）给出。

式（34）为φ^p所需的WNOA误差函数，φ^m所需的姿态测量误差函数为：

e_{m, k} = l n (T_{k} T_{meas, k}^{- 1})

(63)

式中，T_k表示估计的k时刻的状态，T_meas，_k表示测量的k时刻状态。

令参数ν=6，β=1，对参数Ψ、Q_C以及W_gt进行学习。

3.2 轨迹生成

实验采用SICK TIM571红外雷达，扫描频率为15 Hz，测量量程为0.05~10 m，测量精度误差为±60 mm，角度分辨率为0.33°。

将无人机真实轨迹、有真实值估计轨迹以及无真实值估计轨迹叠加以进行比较，结果如图2所示。

图2无人机轨迹生成

Fig.2UAV trajectory generation

由图2可知，无论在训练中是否加入真实值，估计的轨迹与真实轨迹之间的拟合度都很好，验证了算法在缺少真实值情况下也能得出精确的估计值。

3.3 误差分析

无人机轨迹估计仿真中每个子序列的平移误差绝对值平均值如表1所示。

表1无人机轨迹估计平移误差绝对值平均值

Tab.1 Mean value of the absolute value of the translation error of UAV trajectory estimation

由表1可知，无真实值估计轨迹平移的平均误差为0.153 8 m，有真实值估计轨迹平移的平均误差为0.158 6 m，GVI方法估计轨迹平移的平均误差为0.261 4 m。本文所用的ESGVI状态估计与参数学习方法能在不加入真实值的情况下达到和加入真实值的情况下相同的算法性能。

无人机轨迹估计仿真中不加入真实值训练估计的轨迹在x轴和y轴方向上的误差以及偏航角误差如图3所示。

图3中不加入真实值训练估计的轨迹在x轴上的误差均保持在±0.1 m内，y轴上的误差均保持在±0.06 m内，并且波动小；偏航角误差波动稍大，但也保持在±6°内。图3中同时给出采用GVI方法进行估计的实验结果，可以看出，偏航角的误差与ESCVI方法相差不大，但在x轴和y轴上，最大误差均超过了0.15 m。结果表明，ESGIV方法能在不加入真实值的情况下对系统参数进行有效估计，并对无人机轨迹进行精确估计，且平动状态上的估计效果比GVI方法要好。

图3无人机轨迹误差分析

Fig.3UAV trajectory error analysis

3.4 测量噪声对参数学习的影响

为验证本文方法可以使用测量噪声进行训练，本节在训练过程中将测量噪声统计数据设为未知，并且在测量值中添加额外的噪声误差。在位姿测量部分注入噪声：

\{\begin{matrix} T_{noise} = e x p (ξ^{\land}) T_{means} \\ ξ = [\begin{matrix} ξ_{1 : 3} \\ 0 \end{matrix}] \\ ξ_{1 : 3} \sim N (0, σ^{2} I) \end{matrix}

(64)

其中，T_noise表示噪声状态，T_means表示测量状态的平均值，ξ_1:3表示噪声向量，ξ表示调整矩阵行数后的噪声向量，再使用∧算子得到ξ^∧。

调整σ的值为0.25~1 m，步长为0.2 m，在测试量和训练量中注入相同的噪声。估计平移误差的统计结果如表2所示。

从表2中可见，当σ增加时，测量误差显著增加，但轨迹估计的平移误差仍然保持在0~0.5 m之内。结果表明，本文方法能够在没有真实值以及噪声过大的情况下对系统参数进行学习，以达到对无人机轨迹的精确估计。

表2增加测量噪声时的轨迹估计平移误差

Tab.2 Trajectory estimation translation error when adding measurement noise

3.5 测量离群值对轨迹估计的影响

本节将在训练集和测试集中以5%的概率人为引入离群值，以测试IW先验能否有效抑制离群值对轨迹估计精度的影响。将以下的扰动应用到实际的姿态测量。

\{\begin{matrix} T_{outlier} = e x p (ξ^{\land}) T_{means} \\ ξ \in R^{6} \sim u (- 200,200) \end{matrix}

(65)

其中，T_outlier表示离群值的状态，ξ的每个分量从均匀分布

u

独立采样，单位为m。将测量协方差W作为静态参数学习，将每次测量协方差作为随机变量以对IW先验参数Ψ进行学习。

静态测量协方差对应的损失函数为：

\begin{matrix} V (q^{'} ∣ W, Q_{C}) = E_{q^{'}} [ϕ^{p} (x ∣ Q_{C})] + ϕ^{m} (x ∣ W) + \\ \frac{1}{2} l n (|Σ^{- 1}|) \end{matrix}

(66)

IW先验的测量协方差对应的损失函数为：

\begin{matrix} V (q^{'} ∣ Υ, Ψ, Q_{C}) = E_{q^{'}} [ϕ^{p} (x ∣ Q_{C})] + ϕ^{m} (x ∣ Υ) + \\ ϕ^{w} (Υ ∣ Ψ) + \frac{1}{2} l n (|Σ^{- 1}|) \end{matrix}

(67)

无人机轨迹估计仿真中，在训练集和测试集中加入离群值后，是否引入IW先验的轨迹估计平移误差如表3所示。

表3加入离群值后的轨迹估计平移误差

Tab.3 Trajectory estimation translation error after adding outliers

表3中，不引入IW先验的估计轨迹平移误差平均值达到6.285 2 m，而引入IW先验的估计轨迹平移误差仅为0.149 6 m。可见，在训练中引入IW先验能有效抑制离群值对参数学习以及轨迹估计结果产生的影响，提升参数学习以及轨迹估计的精确性。

4 结论

本文针对非线性系统中系统状态以及系统参数难以精确估计及学习的特性，结合高斯变分推理不需要对系统进行精确建模的特点，提出了精确稀疏高斯变分推理的非线性系统状态估计与参数学习方法，利用传感器数据对状态的概率分布进行迭代修正，使其更加近似于状态的真实分布，并且通过期望最大化方法对传感器参数进行学习，使状态估计更加精确。将算法应用于无人机模型的轨迹估计，结果表明：本文方法的估计轨迹准确性较好，并且在参数学习的训练中不加入无人机状态真实值也能实现轨迹精确估计；在人为增加测量噪声的情况下，本文算法也能在参数学习时有效缩小测量噪声带来的影响；在训练以及测试中人为加入测量离群值的情况下，本文算法通过引入IW先验，有效抑制了离群值对轨迹估计精度的影响。综上所述，本文提出的ESGVI参数学习与状态估计算法在精确性以及鲁棒性两个方面都有较好的性能表现。

图1无人机轨迹估计与参数学习因子图

Fig.1Factor diagram of UAV trajectory estimation and parameter learning

下载: 全尺寸图片

图2无人机轨迹生成

Fig.2UAV trajectory generation

下载: 全尺寸图片

图3无人机轨迹误差分析

Fig.3UAV trajectory error analysis

下载: 全尺寸图片

表1无人机轨迹估计平移误差绝对值平均值

下载: 全尺寸图片

表2增加测量噪声时的轨迹估计平移误差

下载: 全尺寸图片

表3加入离群值后的轨迹估计平移误差

下载: 全尺寸图片

图1无人机轨迹估计与参数学习因子图

Fig.1Factor diagram of UAV trajectory estimation and parameter learning

图2无人机轨迹生成

Fig.2UAV trajectory generation

图3无人机轨迹误差分析

Fig.3UAV trajectory error analysis

表1无人机轨迹估计平移误差绝对值平均值

表2增加测量噪声时的轨迹估计平移误差

表3加入离群值后的轨迹估计平移误差

引用提醒

图(3) / 表(3)

引用本文

刘久富, Elishahidi S.B.Mvungi, 汪恒宇, 等. 无人机非线性状态估计：扩展精确高斯变分推理学习方法[J]. 国防科技大学学报, 2025, 47(3): 141-150.

复制

LIU J F, MVUNGI E S B, WANG H Y, et al. Nonlinear state estimation for unmanned aerial vehicles: extended exactly Gaussian variational inference learning method [J]. Journal of National University of Defense Technology, 2025, 47(3): 141-150.

Copy

计量

图1无人机轨迹估计与参数学习因子图

Fig.1Factor diagram of UAV trajectory estimation and parameter learning

图2无人机轨迹生成

Fig.2UAV trajectory generation

图3无人机轨迹误差分析

Fig.3UAV trajectory error analysis

表1无人机轨迹估计平移误差绝对值平均值

表2增加测量噪声时的轨迹估计平移误差

表3加入离群值后的轨迹估计平移误差

SHEN H M, ZONG Q, LU H C,et al. A distributed approach for lidar-based relative state estimation of multi-UAV in GPS-denied environments[J]. Chinese Journal of Aeronautics,2022,35(1):59-69.

GILLIJNS S, DE MOOR B. Unbiased minimum-variance input and state estimation for linear discrete-time systems[J]. Automatica,2007,43(5):934-937.

DEMİRBAŞ K. Oba filter for online-adaptive nonlinear state estimation with interference[J]. IEEE Transactions on Aerospace and Electronic Systems,2019,55(3):1347-1356.

SARKKA S. On unscented Kalman filtering for state estimation of continuous-time nonlinear systems[J]. IEEE Transactions on Automatic Control,2007,52(9):1631-1641.

GUIHAL J M, AUGER F, BERNARD N,et al. Efficient implementation of continuous-discrete extended Kalman filters for state and parameter estimation of nonlinear dynamic systems[J]. IEEE Transactions on Industrial Informatics,2022,18(5):3077-3085.

MEI W J, USHIROBIRA R, EFIMOV D. On nonlinear robust state estimation for generalized Persidskii systems[J]. Automatica,2022,142:110411.

AUCOIN R, CHEE S A, FORBES J R. Linear-and linear-matrix-inequality-constrained state estimation for nonlinear systems[J]. IEEE Transactions on Aerospace and Electronic Systems,2019,55(6):3153-3167.

STEIN C M. Estimation of the mean of a multivariate normal distribution[J]. The Annals of Statistics,1981,9(6):1135-1151.

CHEN S C, LIU Y. Robust distributed parameter estimation of nonlinear systems with missing data over networks[J]. IEEE Transactions on Aerospace and Electronic Systems,2020,56(3):2228-2244.

NA J, XING Y S, COSTA-CASTELLÓ R. Adaptive estimation of time-varying parameters with application to roto-magnet plant[J]. IEEE Transactions on Systems, Man,and Cybernetics: Systems,2021,51(2):731-741.

BURT DAVID R, EDWARD R C, VAN DER WILK M. Convergence of sparse variational inference in Gaussian processes regression[J]. The Journal of Machine Learning Research,2020,21(1):5120-5182.

VAKILI S, SCARLETT J, SHIU D S,et al. Improved convergence rates for sparse approximation methods in kernel-based learning[J]. Proceedings of Machine Learning Research,2022,162:21960-21983.

SAFAEI A, MAHYUDDIN M N. Adaptive model-free control based on an ultra-local model with model-free parameter estimations for a generic SISO system[J]. IEEE Access,2018,6:4266-4275.

BONNEXIE R, SCHMIDT MIKKEL N. Matrix product states for inference in discrete probabilistic models[J]. Journal of Machine Learning Research,2021,22:1-48.

RANGANATHAN A, KAESS M, DELLAERT F. Loopy SAM[C]//Proceedings of the 20th International Joint Conference on Artificial Intelligence,2007:2191-2196.

TANG Y Y, HUANG P K. Boost-Phase ballistic missile trajectory estimation with ground based radar[J]. Journal of Systems Engineering and Electronics,2006,17(4):705-708.

ZHANG X L, YANG Z, WU C S,et al. Robust trajectory estimation for crowdsourcing-based mobile applications[J]. IEEE Transactions on Parallel and Distributed Systems,2014,25(7):1876-1885.

TANG K, CHEN S Y, LIU Z Y. Citywide spatial-temporal travel time estimation using big and sparse trajectories[J]. IEEE Transactions on Intelligent Transportation Systems,2018,19(12):4023-4034.

程媛, 迟荣华, 黄少滨, 等. 基于非参数密度估计的不确定轨迹预测方法[J]. 自动化学报,2019,45(4):787-798. CHENG Y, CHI R H, HUANG S B,et al. Uncertain trajectory prediction method using non-parametric density estimation[J]. Acta Automatica Sinica,2019,45(4):787-798.(in Chinese)

WANG Y, WANG X G, SHAN Y Z,et al. Quantized genetic resampling particle filtering for vision-based ground moving target tracking[J]. Aerospace Science and Technology,2020,103:105925.

WALTER M R, EUSTICE R M, LEONARD J J. Exactly sparse extended information filters for feature-based SLAM[J]. The International Journal of Robotics Research,2007,26(4):335-359.

CHO G R, LI J H, PARK D,et al. Robust trajectory tracking of autonomous underwater vehicles using back-stepping control and time delay estimation[J]. Ocean Engineering,2020,201:107131.

PERETROUKHIN V, VEGA-BROWN W, ROY N,et al. PROBE-GK:predictive robust estimation using generalized kernels[C]//Proceedings of the IEEE International Conference on Robotics and Automation(ICRA),2016:817-824.

ZHANG Z H, ZHOU G J. Maneuvering target state estimation based on separate modeling with B-splines[J]. Aerospace Science and Technology,2021,119:107172.

宋宇, 孙富春, 李庆玲. 移动机器人的改进无迹粒子滤波蒙特卡罗定位算法[J]. 自动化学报,2010,36(6):851-857. SONG Y, SUN F C, LI Q L. Mobile robot Monte Carlo localization based on improved unscented particle filter[J]. Acta Automatica Sinica,2010,36(6):851-857.(in Chinese)

DELLAERT F. Factor graphs:exploiting structure in robotics[J]. Annual Review of Control, Robotics,and Autonomous Systems,2021,4:141-166.

Home

About Journal

Guide for Authors

Editorial Board

Publication Statement

Open Access Statement

Contact

Journal Subscription

Rss

AI assistant

Chinese

1 ESGVI状态估计方法

2 ESGVI参数学习方法

3 实例分析与验证

4 结论