非线性贝叶斯滤波的优化视角重塑与高斯近似技术解读

状态估计是具身智能控制、自动驾驶等领域的核心支撑技术，而贝叶斯滤波则构成了状态估计问题的基本理论框架。围绕这一框架，滤波算法已形成较为成熟的技术谱系：从维纳滤波（Wiener filter）、卡尔曼滤波（Kalman filter），到扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、迭代扩展卡尔曼滤波（IEKF）等高斯滤波方法，相关技术已广泛应用于线性及弱非线性系统的状态估计任务。

然而，在强非线性动力学系统中，传统高斯滤波方法仍面临明显局限。例如：EKF 依赖泰勒一阶展开对系统模型进行局部线性化，UKF 则通过 sigma 点传播来近似非线性变换后的统计矩，二者都会引入难以完全消除的近似误差，难以满足高精度状态估计的需求。这类方法的共同范式可以概括为"先把非线性模型线性化成带高斯噪声的线性模型，再套用卡尔曼滤波"，差别只在于所采用的线性化技术不同。一个长期被忽视、却始终没有被认真回答的问题是：这种"先线性化再滤波"的范式，真的能得到贝叶斯滤波的最优高斯近似吗？

一、从优化视角重新解构贝叶斯滤波

针对这一问题，清华大学智能驾驶课题组近期于 IEEE Transactions on Pattern Analysis and Machine Intelligence（TPAMI）发表的《Nonlinear Bayesian Filtering with Natural Gradient Gaussian Approximation》一文中提出的 Natural Gradient Gaussian Approximation（NANO）滤波器 旨在从机制上减少非线性滤波中的线性化近似误差。其核心思想是从优化视角重新解构贝叶斯滤波，将经典滤波中的预测（prediction）与更新（update）两个步骤分别转化为两个变分推断问题。

对于预测步，NANO滤波器将其建模为一个"最大对数似然期望"的变分优化问题，这一问题的求解会得到状态的先验分布 $\mathcal{N}({\hat{x}}_{t|t-1},P_{t|t-1})$：

$${\hat{x}}_{t|t-1},P_{t|t-1}=\arg \underset{{\hat{x}}_t,P_t}{\max }{\mathbb{E}}_{\mathcal{N}({\hat{x}}_{t-1|t-1},P_{t-1|t-1})}{\mathbb{E}}_{p(x_t|x_{t-1})}\{\log \mathcal{N}(x_t;{\hat{x}}_t,P_t)\}$$

对于更新步，NANO滤波器将其建模为一个目标函数最小化问题，该目标由"观测负对数似然项"与"相对于先验分布的 KL 散度正则项"共同构成，从而获得后验状态分布 $\mathcal{N}({\hat{x}}_{t|t},P_{t|t})$：

$${\hat{x}}_{t|t},P_{t|t}=\arg \underset{{\hat{x}}_t,P_t}{\min }\underset{\text{Measurement Fitting}}{\underbrace{{\mathbb{E}}_{\mathcal{N}({\hat{x}}_t,P_t)}\{\ell (x_t,y_t)\}}}+\underset{\text{Regularization}}{\underbrace{D_{\mathrm{KL}}(\mathcal{N}({\hat{x}}_t,P_t)\Vert \mathcal{N}({\hat{x}}_{t|t-1},P_{t|t-1}))}}$$

这个形式有一个很好理解的直觉：更新步本质上是在"贴合新观测"和"贴近先验信念"之间做权衡。其中 $\ell (x_t,y_t)=-\log p(y_t|x_t)$ 是观测相关的损失。值得一提的是，只要把这里的 $\ell$ 替换成更一般的Gibbs损失，包括huber损失，加权对数似然损失，Beta损失等等，可以进一步增强NANO对于观测异常值的鲁棒性。

二、求解两个优化问题：预测步好解，更新步是症结

对这两个优化问题分别求解，关键在于分析它们的驻点（stationary point）。

预测步的求解。 该优化问题的驻点可以直接等价为一个矩匹配（moment matching）问题：

$${\hat{x}}_{t|t-1}={\mathbb{E}}_{\mathcal{N}({\hat{x}}_{t-1|t-1},P_{t-1|t-1})}\{f(x_{t-1})\}$$

$$P_{t|t-1}={\mathbb{E}}_{\mathcal{N}({\hat{x}}_{t-1|t-1},P_{t-1|t-1})}\{f(x_{t-1})f^{\mathrm{T}}(x_{t-1})\}+Q-{\hat{x}}_{t|t-1}{\hat{x}}_{t|t-1}^{\mathrm{T}}$$

也就是说，预测步的最优解最终归结为"计算非线性函数在高斯分布下的期望"。这类期望可以借助现成的数值积分方法高效求出。常用做法是把高斯期望近似为加权采样点之和：

x∼N(μ,P),P=LLT,E{f(x)}≈∑iWi f(μ+Lξi)x\sim\mathcal{N}(\mu,P),\quad P=LL^{\mathrm{T}},\quad \mathbb{E}\{f(x)\}\approx\sum_i W_i\,f(\mu+L\xi_i)x∼N(μ,P),P=LLT,E{f(x)}≈i∑​Wi​f(μ+Lξi​)

具体的采样点 ${\xi }_i$ 与权重 $W_i$ 取决于所选的积分规则：无迹变换（unscented transform）取 $2n+1$ 个 sigma 点，球面径向容积规则（cubature rule）取 $2n$ 个点，Gauss–Hermite 积分则在每一维取若干节点。换言之，在预测步上，NANO 与现有的 UKF、CKF 等矩匹配滤波器是一致的。

更新步的求解。 麻烦出在这里。更新步的驻点不再是一个干净的矩匹配方程，而是两个互相耦合的隐式方程：

$${\hat{x}}_{t|t}={\hat{x}}_{t|t-1}-P_{t|t-1}{\mathbb{E}}_{\mathcal{N}({\hat{x}}_{t|t},P_{t|t})}\{{\ell }^{\prime }(x_t,y_t)\}$$

$$P_{t|t}^{-1}=P_{t|t-1}^{-1}+{\mathbb{E}}_{\mathcal{N}({\hat{x}}_{t|t},P_{t|t})}\{{\ell }^{\prime \prime }(x_t,y_t)\}$$

右边的期望同时依赖后验的均值和协方差，两个方程缠在一起，一般没有解析根。线性高斯系统是唯一的特例，此时两式解耦、变成线性，于是塌缩成经典卡尔曼滤波的解析更新。正是因为非线性下这个方程组解不出来，现有方法才不得不用线性化去"凑"它，这才是非线性场景下大误差的真正根源。

三、关键思想：自然梯度求解

既然驻点解不出来，那就换条路——直接最小化更新代价 $J({\hat{x}}_t,P_t)$。

但这里有一个容易被忽略的几何问题：高斯分布的参数空间是有曲率的（它是一个黎曼流形，而非平直的欧氏空间），普通梯度方向并不是真正意义上的最速下降方向。在病态（ill-conditioned）情形下，普通梯度下降会出现反复横跳的锯齿路径，收敛很慢。为此，该研究引入自然梯度下降（natural gradient descent）来求解。与普通梯度不同，自然梯度利用 Fisher 信息矩阵对参数空间的曲率进行校正，使迭代方向成为高斯流形上真正的最速下降方向——它在概率流形上走出的是一条平滑、直达极小点的路径。

具体而言，把后验高斯分布的参数堆叠成一个向量 $\theta =[{\hat{x}}_t,\mathrm{vec}(P_t^{-1}){]}^{\mathrm{T}}$，自然梯度的迭代公式为：

$${\theta }^{(i+1)}={\theta }^{(i)}-F_{\mathrm{fisher}}^{-1}\frac{\partial J}{\partial \theta }{|}_{\theta ={\theta }^{(i)}}$$

其中 $F_{\mathrm{fisher}}$ 是该高斯分布的 Fisher 信息矩阵。对于高斯分布，它的逆恰好有一个块对角形式：

$$F_{\mathrm{fisher}}^{-1}=\left[\begin{array}{cc}{P}_t& 0\\ 0& 2(P_t^{-1}\otimes P_t^{-1})\end{array}\right]$$

将各项代入，即得到 NANO 滤波器更新步的最终迭代公式：

$$(P_t^{-1}{)}^{(i+1)}=P_{t|t-1}^{-1}+{\mathbb{E}}_{\mathcal{N}({\hat{x}}_t^{(i)},P_t^{(i)})}\{{\ell }^{\prime \prime }(x_t,y_t)\}$$

$${\hat{x}}_t^{(i+1)}={\hat{x}}_t^{(i)}-P_t^{(i+1)}{\mathbb{E}}_{\mathcal{N}({\hat{x}}_t^{(i)},P_t^{(i)})}\{{\ell }^{\prime }(x_t,y_t)\}-P_t^{(i+1)}{P}_{t|t-1}^{-1}({\hat{x}}_t^{(i)}-{\hat{x}}_{t|t-1})$$

其中所有期望同样用上一节的矩匹配技术（无迹变换等）来计算。若 Gibbs 损失不够光滑、其一二阶导数难以稳定计算，还可以再次借助 Stein 引理，把对 $\ell$ 求导转化为只用 $\ell$ 本身的零阶信息的"无导数"形式，从而对非光滑损失更鲁棒。这套迭代替代了原本解不出的驻点方程，在每个时刻内部多次迭代直到收敛，从根本上绕开了线性化误差。

从信息几何的角度看，这一步有更深的含义：根据 Chentsov 定理，Fisher 信息度量是高斯流形上唯一在充分统计量变换下保持不变的黎曼度量，因此"自然梯度 = 高斯流形上的最速下降"并非一个工程技巧，而是有几何唯一性支撑的最优选择。也正是在这个意义上，可以证明 NANO 的单次迭代在线性高斯系统下严格退化为卡尔曼滤波，从而给了卡尔曼滤波一个全新的"自然梯度"解读。

四、实验结果

NANO 滤波器在仿真与真实系统上进行了广泛验证。

人形机器人位姿估计： 实验采用宇树 G1 机器人，其包含 21 维状态（位姿、速度、IMU 偏置、足端位置），转移与观测模型均为非线性。NANO 把位置平均平移误差从 EKF/UKF 的约 0.39 m 大幅降到 0.10 m。

方法	ATE（位置）[m]	ATE（速度）[m]
EKF	0.386	0.346
UKF	0.387	0.345
NANO	0.101	0.267

无人车定位: 实验在 Victoria Park 数据集上进行，这是一段越野车在公园环境中行驶、依靠车载传感器对周围树木等地标做距离-方位观测来同步定位与建图的经典基准。其状态含车辆 2D 位置与航向，观测为多路距离-方位量测，状态转移与观测模型均为非线性。NANO-SLAM 把 RMSE 从 EKF-SLAM 的 7.78 m、Ufast-SLAM (基于UKF的方法) 的 5.15 m 降到 2.54 m，且耗时相当。

方法	RMSE [m]	耗时 [s]
EKF	7.783	0.312
Ufast-SLAM	5.147	0.248
NANO-SLAM	2.538	0.270

五、总结

总的来说，NANO 不只是提出了一个新的滤波算法，而是重新审视了非线性滤波的基本范式：将滤波问题的预测和更新步等价为对状态分布的优化问题，从而规避了线性化近似；通过自然梯度在高斯流形上直接优化后验，滤波器可以快速收敛到滤波问题的最优高斯解。对于高非线性、高噪声、异常观测频发的实际系统，这一方法为状态估计提供了一条兼具理论完备性、鲁棒性和工程效率的新路径！

源代码下载网址：

• 百度网盘：https://pan.baidu.com/s/10A1pKbdNgOA2lK0LEuYhCA?pwd=3zyc
• Github：https://github.com/wenhancao/NANO-idlab

参考文献：

[1] W. Cao, T. Zhang, Z. Sun, C. Liu, S. S. Yau and S. E. Li. Nonlinear Bayesian Filtering with Natural Gradient Gaussian Approximation. IEEE Trans on Pattern Analysis and Machine Intelligence, 2026 (online in arXiv, Oct 2024)

[2] T. Zhang, W. Cao, C. Liu, F. Zhang, W. Wu and S. E. Li. NANO-SLAM: Natural Gradient Gaussian Approximation for Vehicle SLAM. IEEE 28th International Conference on Intelligent Transportation Systems (ITSC), Gold Coast, Australia, 2025

[3] W. Cao, T. Zhang and S. E. Li. Algorithm Design and Comparative Test of Natural Gradient Gaussian Approximation Filter. IFAC Modeling Estimation and Control Conference (MECC), Pittsburgh, USA, 2025

[4] T. Zhang, W. Cao and S. E. Li. Natural Gradient Gaussian Approximation Filter with Positive Definiteness Guarantee. American Control Conference (ACC), New Orleans, USA, 2026