CJA ENGLISH

导航

联系我们
学术质量   |
广告服务   |
期刊征订   |
English Version CJA

航空学报 >

2026 , Vol. 47 >Issue 7: 332837 - 332837

DOI: https://doi.org/10.7527/S1000-6893.2025.32837

电子电气工程与控制

一种基于线性约束的鲁棒几何滤波方法

  • 金宇强 ,
  • 杨旭升 ,
  • 张文安
展开
  • 浙江工业大学 信息工程学院，杭州 310014
E-mail：

收稿日期: 2025-09-28

  修回日期: 2025-10-24

  录用日期: 2025-11-18

  网络出版日期: 2025-11-28

基金资助

国家自然科学基金重点（联合）项目(U25A20456)

杭州市重大科技创新项目(2022AIZD0080)

收起

Robust geometric filtering via linear constraint

  • Yuqiang JIN ,
  • Xusheng YANG ,
  • Wenan ZHANG
Expand
  • College of Information Engineering，Zhejiang University of Technology，Hangzhou 310014，China
E-mail：

Received date: 2025-09-28

  Revised date: 2025-10-24

  Accepted date: 2025-11-18

  Online published: 2025-11-28

Supported by

National Natural Science Foundation of China(U25A20456)

Major Science and Technology Innovation Project of Hangzhou(2022AIZD0080)

Fold

摘要

针对几何滤波结果对模型失配敏感的问题，提出了一种线性约束方法。首先，通过分析群仿射系统中模型失配对滤波过程的影响机制，揭示了标准几何滤波（如不变扩展卡尔曼滤波（InEKF））在模型失配时误差累积的本质原因。其次，设计了一种线性约束滤波方法，通过约束滤波增益矩阵在模型偏差方向上的行为来抑制误差传播，同时保持李群流形上的几何特性。该方法在保持实时性的情况下，有效提升了系统对模型失配的鲁棒性。最后，通过惯性导航与全球卫星导航系统（INS/GNSS）组合导航的仿真与实验表明：所提方法相较标准不变扩展卡尔曼滤波的估计精度上均取得显著提升，为复杂环境下的鲁棒滤波提供了有效解决方案。

关键词： 几何滤波; 模型失配; 鲁棒估计; 卡尔曼滤波; 李群与李代数

本文引用格式

金宇强 , 杨旭升 , 张文安 . 一种基于线性约束的鲁棒几何滤波方法[J]. 航空学报, 2026 , 47(7) : 332837 -332837 . DOI: 10.7527/S1000-6893.2025.32837

Abstract

To address the sensitivity of geometric filtering results to model mismatch， this paper introduces a novel filtering framework grounded in linear constraint enforcement. We begin by analyzing how model mismatch propagates through the filtering process in group-affine systems， revealing the fundamental mechanism behind model mismatch accumulation in conventional geometric filters such as the Invariant Extended Kalman Filter （InEKF）. Second， we design a constrained filtering strategy that actively regulates the gain matrix along the direction of model deviation， thereby suppressing model mismatch growth while preserving the intrinsic geometric structure of the underlying Lie group manifold. Notably， the proposed method achieves this enhanced robustness without sacrificing computational efficiency， making it suitable for real-time deployment. Finally， simulations and real-world experiments on INS/GNSS （Inertial Navigation System/Global Navigation Satellite System） integrated navigation demonstrate that the proposed method consistently achieves substantially improved estimation accuracy compared to the standard InEKF， offering an effective solution for robust filtering in complex environments.

Key words： geometric filtering; model mismatch; robust estimation; Kalman filter; Lie groups and Lie algebras

几何滤波方法［1］通过利用李群系统固有的对称性和底层几何结构，显著降低了系统建模难度并提高了滤波精度，已在机器人位姿估计［2］、无人机定位以及航天器、航空器导航［3-5］等应用中验证其优越性。不变卡尔曼滤波（Invariant Extended Kalman Filter， InEKF）是几何滤波的代表性方法［6］，其可以看作是欧式空间中的卡尔曼滤波（Kalman Filter， KF）在群仿射系统中的推广，通过适当的定义，欧式空间中的典型非线性系统在李群空间上具有良好的线性性质［7-9］，从而在理论上规避了传统EKF因线性化误差导致的发散风险。这一思想被拓展至几何视角下的无迹卡尔曼滤波（Unscented Kalman Filter， UKF）与粒子滤波（Particle Filter， PF）框架，即通过在李群流形上重新定义采样策略与权重更新机制，UKF避免了显式雅可比计算，PF则通过粒子采样逼近后验分布，二者在特定场景下展现出更强的非线性适应能力［10-12］。
几何滤波方法的优越性很大程度上来源于其对物理系统的精确建模。然而，这种对模型精确性的高度依赖使其对失配问题尤为敏感。在实际工程中，模型失配现象普遍存在：既源于未建模动力学，如运动平台受到风扰或地面滑动影响，也来自传感器参数标定误差，这导致传统滤波器的估计性能显著退化［13-14］。现有鲁棒滤波研究主要集中于噪声统计特性异常的处理，包括协方差自适应估计［15-16］、基于鲁棒统计的异常值抑制［17-18］、重尾分布建模［19-20］等。尽管这些方法在应对观测异常方面成效显著，但对系统动态模型本身的失配问题关注较少。值得注意的是，近期线性约束卡尔曼滤波（Linearly Constrained KF， KF-LC）［21］及其非线性扩展EKF-LC［22］通过引入时变等式约束，在欧式空间中实现了对模型参数偏差的主动补偿，有效抑制了特定方向上的误差增益，且未显著增加计算负担。这类方法虽未直接针对李群结构设计，却为模型失配补偿提供了启发性思路，即通过约束滤波器内部自由度，在模型偏差主导的方向上主动抑制误差增益。然而，上述方法未考虑状态空间的非欧几何结构，直接应用于李群系统将破坏流形一致性，导致估计结果偏离物理可行域。更重要的是，现有工作尚未探讨关键问题：如何在保持李群几何结构的前提下，系统性地嵌入对模型结构性失配的鲁棒机制。
针对上述挑战，本文聚焦群仿射系统下的几何滤波问题，深入分析模型失配通过李代数空间特定方向影响误差演化的机理，并在此基础上提出一种融合线性约束机制的改进型不变滤波器，记为InEKF-LC。该方法在不改变状态传播与观测映射几何结构的前提下，通过约束滤波增益在模型偏差方向上的投影行为，主动阻断误差的结构性传播路径。理论推导表明，所提约束可等价为对后验修正量在偏差激励方向上的正交性要求，从而在保持实时性的同时显著提升对模型不确定性的鲁棒性。本文的主要贡献可概括为以下3点：第一，揭示了模型失配在群仿射系统中诱导误差增长的几何机理，指出其作用方向由偏差对数映射所决定；第二，首次将线性约束机制系统性地引入不变滤波框架，提出InEKF-LC，在严格保持李群流形结构的同时实现对模型失配的主动抑制；第三，通过INS/GNSS组合导航的仿真与真实无人机飞行实验，验证了所提方法在估计精度与计算效率上的综合优势，为促进可复现研究，完整实现代码已开源发布于https：∥github.com/yuqJin/InEKF-LC。

1 问题描述

1.1 不变卡尔曼滤波

为了公式上的简洁并突出本文的核心思想，本节主要针对群仿射系统进行讨论。考虑一个状态 X演化在 d维矩阵李群 𝒢上的离散时间状态空间模型，其状态演化过程满足以下方程
X k = f k - 1 u X k - 1 e x p ( w k - 1 )
X 0 ~ 𝒩 𝒢 X ¯ 0 , P 0
式中： f k - 1 u : 𝒢 → 𝒢是群仿射动态系统； w k是零均值高斯噪声，协方差为 Q k - 1，初始状态 X 0服从集中高斯分布。对于群仿射系统，可以用一个群自同构 Ψ : 𝒢 → 𝒢和一个依赖于输入 u k - 1的位移项 A k - 1 u描述，即式（1）可以表示为
X k = Ψ ( X k - 1 ) A k - 1 u e x p ( w k - 1 )
同时，有观测模型为
y k = h X k + n k
式中： h : 𝒢 → 𝒴将李群上的状态映射到观测向量空间 𝒴； n k是协方差为 R k的零均值高斯噪声。如文献［6］中所述，基于李群状态空间的InEKF算法通过误差动力学线性化实现高效状态估计，给定 k - 1时刻的后验估计 𝒩 𝒢 X ^ k - 1 + , P ^ k - 1 +，InEKF的核心步骤可表述为以下传播-更新流程［6-7］
X ^ k - = Ψ ( X ^ k - 1 + ) A k - 1 u
P ^ k - = F k - 1 P ^ k - 1 + F k - 1 T + Q k - 1
S k = H k P ^ k - H k T + R k
K k = P ^ k - H k T S k - 1
X ^ k + = X ^ k - e x p K k y k - h X ^ k -
P ^ k + = I - K k H k P ^ k -
式中：雅克比矩阵 F k - 1和 H k仅依赖于系统固有结构，与状态估计值 X ^无关，避免了一般扩展卡尔曼滤波EKF的线性化误差问题。

1.2 模型不匹配问题

本节将重点分析不匹配来源及其对滤波系统的影响。考虑一个存在模型不匹配的群仿射动态系统，其状态演化过程可以表示为
X k = Ψ ( X k - 1 ) A k - 1 u δ k e x p ( w k - 1 )
式中： δ k ∈ 𝒢表示动态模型中的偏差。定义不变误差 η = X ^ - 1 X = e x p ( ξ ) ∈ 𝒢，根据贝克-坎贝尔-豪斯多夫（Baker-Campbell-Hausdorff，BCH）公式，对数误差 ξ的传播过程可以通过以下公式描述［6］：
ξ k - ≈ F k - 1 ξ k - 1 + + b k δ + w k - 1
式中： F k - 1满足 Ψ e x p ( ξ ) = e x p F k - 1 ξ。群自同构 Ψ可以完全编码在一个简单矩阵 F k - 1 ∈ R p × p中， b k δ = l o g ( δ k )是由模型偏差引起的额外误差项，其中 l o g ( ⋅ )是从李群到李代数的对数映射。此处采用一阶近似，其有效性要求 b k δ不显著大于过程噪声的典型幅值，即 b k δ与 t r ( Q k - 1 ) / p 同阶，其中 p为李代数维数，在此条件下BCH展开的高阶项可忽略［1-3，6-12］。这一假设在工程实践中具有合理性，例如由传感器标定误差或执行器缩放引起的系统性偏差通常表现为李代数空间中的小幅扰动。此类模型不匹配虽小，却会在滤波过程中持续累积，导致估计性能退化，尤其是在长时间运行中，其影响会变得更加显著。为了更直观地理解模型不匹配问题，以二维空间刚体变换，即 S E ( 2 )为例进行说明。对于 X ∈ S E ( 2 )，有
X 1 X 2 = R 1 p 1 0 1 × 2 1 R 2 p 2 0 1 × 2 1 = R 1 R 2 R 1 p 2 + p 1 0 1 × 2 1 X 1 - 1 = R 1 T - R 1 T p 1 0 1 × 2 1
式中： R 1 ,   R 2 ∈ S O ( 2 )， p 1 ,   p 2 ∈ R 2。考虑一个基于里程计的传播模型，给定输入增量 u k - 1 = ( Δ R k - 1 , Δ p k - 1 )，对于状态 X k - 1，其状态传播可以表示为
X k = R k - 1 p k - 1 0 1 × 2 1 Δ R k - 1 Δ p k - 1 0 1 × 2 1 = R k - 1 Δ R k - 1 R k - 1 p k - 1 + Δ p k - 1 0 1 × 2 1
回顾式（3），式中： Ψ = I d， A k - 1 u = Δ R k - 1 Δ p k - 1 0 1 × 2 1。在实际应用中，由于地面滑动或轮胎气压不平稳等因素，线性速度可能存在缩放偏差 s k。此时，式（11）中的 δ k可以表示为
δ k = I 2 s k Δ R k - 1 T Δ p k - 1 0 1 × 2 1
对应地， b k δ = l o g ( δ k ) = 0 , s k Δ R k - 1 T Δ p k - 1 T T。一个简易的数值模拟如图1所示，其中参数设置与第3节仿真验证一致。可以看到在一般情况下，InEKF的定位精度始终高于欧式空间建模的滤波方法，如EKF。但若出现模型不匹配时，两者的最终误差水平趋于接近，表明几何滤波方法对这些问题更敏感。具体而言，在该数值模拟中，模型不匹配时的InEKF误差增幅为859%，远高于EKF的427%。
图 1 InEKF的模型不匹配问题

Fig.1 Model mismatch issue in InEKF

2 主要结果

为了解决上述问题，本节提出了一种基于线性约束的几何贝叶斯滤波器，该方法通过引入线性约束来缓解模型不匹配对状态估计的影响。对于式（1）~式（4）描述的动态系统，模型不匹配的影响建模为式（5）~式（6），在状态预测与更新过程中，任意滤波增益 L下的后验误差可表示为
ξ k + ( L ) = ξ k - - L H k ξ k - = I - L H k F k - 1 ξ k - 1 + + w k - 1 - L n k + I - L H k b k δ
式中： ξ k - 1 + ,   ξ k + ,   ξ k -分别是 k - 1时刻和 k时刻的后验估计误差，及 k时刻的先验估计误差。为显式分离模型失配的影响，将式（16）重写为
ξ k + ( L ) = ξ k - - L H k ξ k - = I - L H k F k - 1 ξ k - 1 + + w k - 1 - L n k + ϵ k ( L )
式中： ϵ k ( L ) = I - L H k b k δ是由模型偏差 b k δ通过增益 L诱导的额外误差项。为了缓解模型不匹配带来的影响，通过引入线性约束的方式，将模型偏差嵌入到滤波框架中。具体而言，为了消除上述误差项 ϵ k ( L )，满足以下约束条件：
L H k b k δ = b k δ
这一约束条件确保了由模型偏差 δ k引起的额外误差能够被最小化。从几何意义上看，上述条件暗示增益矩阵 L必须在 b k δ的方向上保持一致性。换句话说，当 b k δ表示由模型偏差引起的额外误差时， L必须确保这一误差不会被放大或扭曲，而是被正确地处理。从代数角度上看，式（18）作为一个线性约束条件，限制了滤波增益矩阵 L的自由度。通过施加这一约束确保了 L在 b k δ方向上具有特定的行为，即保持 b k δ不变。
为了实现上述约束条件，将目标定义为寻找满足约束条件的滤波增益矩阵 L k，即
L k = a r g m i n L J M S E = a r g m i n L P k ( L ) s . t .   L k G k = b k δ
式中： P k ( L )是估计误差的协方差矩阵，定义为 P k ( L ) = E ξ k ( L ) ξ k ( L ) T，此外，约束条件表示为紧凑形式，即 G k = H k b k δ。基于式（19），并结合InEKF的步骤，新的鲁棒线性约束几何EKF的具体实现步骤如下：给定 k - 1时刻的估计 X ^ k - 1 +和估计误差协方差矩阵 P ^ k - 1 +，首先根据系统模型式（1）和式（2）计算先验状态估计
X ^ k - = Ψ X ^ k - 1 + A k - 1 u
P ^ k - = F k - 1 P ^ k - 1 + F k - 1 T + Q k - 1
对于状态更新过程，假设状态修正量为 ξ k = L k z k，其中 z k是观测新息。目标是最小化代价函数式（19），其中约束条件可重构为 G k T ξ k = 0，这表明修正量 ξ k将不在模型预测方向 G k上引入额外偏差。此时，引入拉格朗日算子 λ构造增广代价函数如下：
ℒ = J M S E ( ξ k ) + λ T G k T ξ k
式中： J M S E ( ξ k )可由马氏距离定义为
J M S E ( ξ k ) = 1 2 ξ k T P k - - 1 ξ k + 1 2 y k - H k ξ k T R k - 1 y k - H k ξ k
对 ℒ分别关于 ξ k和 λ求导，有
∂ ℒ ∂ ξ k = P k - - 1 ξ k - H k T R k - 1 ( y k - H k ξ k ) + G k λ = 0
∂ ℒ ∂ λ = G k T ξ k = 0
有 P k - - 1 + H k T R k - 1 H k ξ k + G k λ = H k T R k - 1 y k，因此有关于 ξ k的解析解
ξ k = P k - - 1 + H k T R k - 1 H k - 1 × H k T R k - 1 y k - G k λ
定义标准卡尔曼增益 K k = P k - H k T S k - 1其中 S k = H k P k - H k T + R k，根据矩阵求逆公式，有
P k - - 1 + H k T R k - 1 H k - 1 = P k - - P k - H k T S k - 1 H k P k -
将式（26）代入式（25），可解得
λ = - G k T T k 0 G k - 1 G k T T k 0 H k T R k - 1 y k
式中： T k 0 = P k - - P k - H k T S k - 1 H k P k -。可以定义中间变量
Γ k = b k δ - K k G k
Σ k = G k T ( S k ) - 1 G k
式中： Γ k表示标准卡尔曼增益无法直接满足约束条件的部分； Σ k是一个正定矩阵。整理后可得
ξ k = K k + Γ k Σ k - 1 G k T ( S k ) - 1 z k
因此，最终受约束滤波增益矩阵 L k可以表示为
L k = K k + Γ k Σ k - 1 G k T ( S k ) - 1
使用受约束增益 L k更新状态得到后验估计
X k + = X k - e x p ξ k = X k - e x p L k y k - h X ^ k -
P ^ k + = I - K k H k P ^ k - + Γ k Σ k - 1 Γ k T
式中： Γ k Σ k - 1 Γ k T是由于施加线性约束而引入的额外协方差修正项，其反映了由模型不匹配导致的额外不确定性。为保证协方差矩阵的对称性与数值稳定性，实际计算中对 P ^ k +采用对称化处理，即 P ^ k + ← 1 2 P ^ k + + ( P ^ k + ) T。算法流程如算法1所示，该方法在保留几何滤波器结构特性的同时，提升了对模型不匹配的适应能力，并通过协方差修正保持一致性，记为InEKF-LC。

算法1 InEKF-LC算法流程

输入： X ̂ 0 + ∈ G， P ̂ 0 + ∈ R p × p

1. for k = 1 to T， do

2.    根据式（3）和式（4）定义 F k和 H k

3.    定义 Q k为 w k的协方差， R k为 r k的协方差

4.     X ̂ k - = Ψ X ̂ k - 1 | k - 1 A k - 1 u

5.     P ̂ k - = F k - 1 P ̂ k - 1 + F k - 1 T + Q k - 1

6.     S k = H k P ̂ k - H k T + R k， K k = P ̂ k - H k T S k - 1

7.     Γ k = b k δ - K k G k， Σ k = G k T ( S k ) - 1 G k

8.     L k = K k + Γ k Σ k - 1 G k T ( S k ) - 1

9.     X ̂ k + = X ̂ k - e x p L k y k - h X ̂ k -

10.   P ̂ k + = I - K k H k P ̂ k - + Γ k Σ k - 1 Γ k T

11.   P ̂ k + ← 1 2 P ̂ k + + ( P ̂ k + ) T

输出： X ̂ k +， P ̂ k +

3 实 验

3.1 数值仿真

为了验证所提算法在模型失配环境下的性能，选取了基于惯性导航系统和全球定位系统的INS/GNSS融合定位与导航模型作为仿真实验场景。该问题被认为是状态估计领域的基准问题，对应的仿真参数经过适当选择以充分展示所提鲁棒滤波器的性能表现，本实验平台同样为配备AMD Ryzen 5@3.00 GHz处理器、16 GB内存的计算机。所考虑的离散动态系统如式（3）和式（4）所示。在本实验中，待估计的车辆状态包括位置 p和姿态信息 R，其运动信息可由IMU采集的线速度和角速度计算，即
R k = R k - 1 e x p ω k - 1 + w k - 1 ω Δ t
p k = p k - 1 + v k - 1 + w k - 1 v Δ t
具体而言，IMU提供的体固定速度测量值采样频率为100 Hz，而GNSS提供的位置测量值采样频率为1 Hz， Δ t = 0.01   s，仿真时间 t ∈ [ 0 , T ]，其中 T = 60   s，线速度设置为 v = 2 π r / T ,   0 T m / s，其中 r = 5   m，角速度为 ω = 2 π / T r a d / s，这些输入用于描述式（1）中 f k - 1 u或式（3）中 A k - 1 u的动态特性。对于噪声特性，线速度量测标准差设置 σ v = 0.5   m / s，角速度量测的标准差为 σ ω = 5 π / 180   r a d / s。位置观测数据由GNSS接收器提供，系统观测模型为：
y k = p k + n k
式中： y k ∈ R 2为观测值，观测标准差 σ p = 0.05   m。为了量化滤波器的估计性能，在100次蒙特卡洛仿真后，使用均方根误差RMSE指标进行评估。通过引入线速度输入的缩放偏差来模拟模型不匹配问题，参见式（15）：在时间区间 t ∈ [ 12,18 ]   s 内，线速度输入被缩小至 s k = 0.7倍；在时间区间 t ∈ [ 36,48 ]   s内，线速度输入被缩小至 s k = 1.3倍。需要注意的是，这种偏差可以长期存在，例如由于传感器外参标定不准确导致的系统性误差，且缩放因子也不需要以固定常数的形式存在。为模型了突出结果的可视化效果，我们在部分时间段内人为加入模型偏差，其影响如图2所示。
图 2 模型不匹配的场景设置

Fig.2 Scenario setup with model mismatch

在上述仿真设置下，重点考察了模型不匹配对状态估计性能的影响，并对比了4种不同的滤波方法：所提出的线性约束不变扩展卡尔曼滤波器InEKF-LC、标准的不变扩展卡尔曼滤波器InEKF［5］、李群上的无迹卡尔曼滤波器UKF-LG［11］以及基于李群的扩展卡尔曼滤波器EKF-LG［23］。为确保各方法性能对比的公平性，本文在仿真与实验中对所有滤波器采用完全一致的噪声统计参数与初始条件。过程噪声协方差矩阵设为 Q = d i a g ( 0 . 1 2 , 0 . 1 2 , 0 . 1 2 , 0 . 01 2 , 0 . 01 2 , 0 . 01 2 )其中前3个分量对应位置随机游走强度（ m / s），后3个对应姿态随机游走强度（ r a d 2 / s），观测噪声协方差设为 R = d i a g ( 0 . 05 2 , 0 . 05 2 )，对应GNSS位置观测噪声（m）。初始状态估计 X ^ 0 +与初始误差协方差 P 0 = d i a g ( 1 2 , 1 2 , 1 2 , 0 . 1 2 , 0 . 1 2 , 0 . 1 2 )，上述参数在仿真与实物实验中保持一致。对于UKF-LG，其sigma点采样采用李群无迹变换的标准配置［9，11］：缩放参数 α = 10 - 3，分布参数 β = 2，偏移参数 κ = 0。EKF-LG与InEKF的区别仅在于误差定义方式，其余实现均遵循原始文献［5，23］。
4种方法对应的量化结果可见表1，4种算法在姿态RMSE和位置RMSE方面的表现存在显著差异。其中，InEKF-LC的姿态RMSE仅为0.58°，远低于InEKF的3.63°、UKF-LG的3.39°和EKF-LG的9.78°。这表明当系统存在模型不匹配时，InEKF-LC能够有效缓解由模型偏差引起的估计误差累积，保持较高的姿态估计精度。在位置估计方面，InEKF-LC的表现同样突出，其位置RMSE为0.21 m，明显优于InEKF的0.79 m、UKF-LG的0.76 m和EKF-LG的2.34 m。这一结果验证了线性约束策略在抑制模型不匹配影响方面的有效性。尽管InEKFLC引入了线性约束以提升鲁棒性，但其单步计算耗时仅略微增加至1.98 ms，与InEKF和EKF-LG相当，且远低于UKF-LG。这表明，InEKF-LC在保证较高精度的同时，依然具有较低的计算复杂度。
表1 不同方法的性能与计算开销对比

Table 1 Performance and computational cost comparison of different methods

算法 姿态RMSE/（°）

位置

RMSE/m

 单步计算耗时/ms
InEKF 3.63 0.79 313.6
UKF-LG 3.39 0.76 293.2
EKF-LG 9.78 2.34 1.88
InEKF-LC 0.58 0.21 1.98
此外，由图3所示，当模型不匹配发生时，3种传统几何滤波方法的误差开始逐渐增大。这种现象的根本原因在于这些方法对模型不匹配较为敏感，尤其是在非线性系统的动态特性发生变化时，容易导致估计误差累积。相比之下，InEKF-LC在整个过程中始终保持了较高的精度，误差波动较小且稳定。这得益于其在线性约束框架下对模型偏差的有效抑制能力，从而避免了估计误差的快速扩散。尽管InEKF-LC引入了线性约束以提升鲁棒性，其计算复杂度与标准 InEKF仍处于同一量级。具体而言，InEKF的主要计算负担在于协方差传播 𝒪 ( p 2 )与观测更新中的矩阵求逆 𝒪 ( m 3 )，其中 p为状态维数， m为观测维数。InEKF-LC新增的计算包括：约束向量 G k = H k b k δ的构建 𝒪 ( m p )、修正项 Γ k与标量或低维矩阵 Σ k = G k T ( S k ) - 1 G k的计算 𝒪 ( m 2 )，以及协方差修正项 Γ k Σ k - 1 Γ k T 𝒪 ( p 2 )。由于模型偏差 b k δ通常仅作用于状态子空间，如仿真中仅影响位置分量， Σ k的维度极低，其求逆可忽略不计。因此，InEKF-LC的总时间复杂度仍为 𝒪 ( p 2 m + m 3 )，与InEKF同阶。综上所述所提方法InEKFLG，在保持实时性（1.98 ms/step）的同时，将位置和姿态误差分别降低至0.21 m和0.58°，较标准InEKF提升73.4%和84.0%。
图 3 不同方法的估计误差结果

Fig.3 Estimation error results of different methods

需要指出的是，仿真中采用的分段常值线速度缩放偏差虽为确定性信号，但其在李代数空间中所激发的附加误差项 b k δ，与真实飞行中由持续侧风或控制输入偏置引起的模型失配具有相同的几何结构，即在平移子空间中引入低频、有偏的激励。通过对3.2节实测数据的后验残差分析可知，在高楼峡谷等强扰动场景下，InEKF的位置估计误差呈现出与仿真中相似的单调漂移趋势，且主导方向一致。因此，尽管仿真简化了偏差的时间动态特性，但有效保留了模型失配影响滤波性能的核心机制。

3.2 真实实验

为在真实系统中评估所提方法的工程适用性，我们在无人机飞行平台上开展了实地实验，采集真实IMU/GNSS融合数据，以验证InEKF-LC在面对实际模型失配与环境扰动时的鲁棒性表现。实验平台搭载北云UG012_A1组合导航板卡，内置多频GNSS接收机与IMU模块，支持100 Hz高频输出，量程分别为陀螺仪 ± 500   ( ° ) / s和加速度计 ± 8 g，能够在动态环境中提供高精度的姿态估计。为获取厘米级定位真值，平台还配备了Vision-RTK 2高精度定位模组，该模组集成了双频RTK GNSS接收器、嵌入式摄像头和初始运动单元IMU，即使在GNSS信号退化或中断的情况下，仍能通过智能算法引擎实现连续高精度定位，其RTK固定解水平定位精度优于2 cm，姿态精度优于0.1°，可作为可靠的评估基准。计算单元搭载Intel Core i7-8700 CPU，内置Ubuntu 18.04操作系统和ROS Melodic开发环境，能够高效处理多源传感器数据，并支持复杂路径规划与实时跟踪控制算法的运行。四旋翼无人机的飞行轨迹覆盖校园园区内多种典型场景，包括开阔广场、树荫遮蔽区及楼宇峡谷，伴随着风扰以及失重，总飞行距离约3 000 m，飞行时长约15 min，实验轨迹如图4所示，其中轨迹1长度为1 274 m，轨迹2长度为1 351 m，轨迹3长度为489 m。
图 4 园区中运行的参考轨迹示例

Fig.4 Reference trajectory examples for campus environment operations

同样对比了4种几何滤波器的性能，所有算法均在相同初始条件下运行，过程噪声协方差统一设为 Q = d i a g ( 0 . 1 2 , 0 . 1 2 , 0 . 1 2 , 0 . 01 2 , 0 . 01 2 , 0 . 01 2 )，对应位置与姿态的随机游走强度，噪声参数经离线标定。表2和表3汇总了各算法在整段轨迹上的位置RMSE，以及单步平均计算耗时。从结果可见，在真实飞行环境中，所提出的InEKF-LC方法在全部测试轨迹中均表现出优于现有方法的定位精度。在环境干扰较显著的轨迹3（高楼峡谷区域，存在持续侧风与控制输入偏置）中，InEKF-LC的平均定位误差为0.131 m，相较InEKF的0.281 m降低约53%，相较EKF-LG的0.415 m降低约68%。UKF-LG在轨迹2中表现略优，误差为0.227 m，较InEKF的0.243 m改善约7%，但其单步计算耗时达45.3 ms，难以满足无人机系统对高频率状态更新的实时性要求。EKF-LG在复杂动态环境下误差最大，反映出其未利用系统几何结构所带来的性能局限。
表2 不同方法的性能对比

Table 2 Performance comparison of different methods

算法 位置RMSE/m
轨迹1 轨迹2 轨迹3
InEKF 0.256 0.243 0.286
UKF-LG 0.233 0.227 0.265
EKF-LG 0.376 0.352 0.415
InEKF-LC 0.112 0.104 0.131
表3 不同方法的计算耗时对比

Table 3 Comparison of computational time of different methods

算法 单步计算耗时/ms
InEKF 2.2
UKF-LG 45.3
EKF-LG 2.1
InEKF-LC 2.4
从上述结果中可以看出，InEKF-LC的性能提升源于其对模型失配传播路径的主动约束机制：在实际飞行中，风扰与控制偏置导致IMU运动模型与真实动力学之间出现结构性偏差，该偏差在李代数空间中沿特定方向持续激励误差增长，而所提方法通过在滤波增益设计中引入线性约束，有效抑制了该方向上的误差增益，从而在不显著增加计算负担的前提下，提升了系统在扰动环境下的估计稳定性。三段轨迹的平均定位误差由InEKF的0.260 m降至InEKF-LC的0.116 m，相对改善幅度约为53%，同时可见表3所示，计算耗时维持在2.4 ms/step的高效水平。
为更直观地说明模型失配对估计性能的影响机制，以及验证仿真模型与真实失配场景的一致性，对3.2节中轨迹3的实测数据进行了后验残差分析。图5展示了在高楼峡谷区域（共1 min）内，标准InEKF的位置估计误差随时间的演化情况。可见，误差并非零均值波动，而是呈现出持续的单向漂移趋势，并在该时段内累积至约0.3 m。这一行为与仿真中由线速度缩放偏差引起的结构性误差在主导方向与累积特性上高度一致，表明真实环境中的风扰与控制偏置在系统层面可等效为李代数空间中沿特定方向的慢时变偏差。因此，本文采用的仿真失配模型虽为简化形式，但有效反映了实际扰动对几何滤波器的核心影响机制。
图5 结构性模型失配下的位置误差趋势示意图

Fig.5 Illustration of position error trends under structural model mismatch

4 结 论

本文提出并验证了一种面向模型失配场景的几何状态估计方法，其核心在于将线性约束机制系统性地嵌入李群滤波框架，在不破坏流形结构的前提下主动抑制误差传播路径。主要结论如下：
1） 在群仿射系统中，模型失配会沿李代数特定子空间诱导结构性误差增长，而所提方法通过约束滤波增益在该方向上的投影行为，从机制上阻断了误差的持续累积，为几何滤波的鲁棒性设计提供了新的理论视角。
2） 所构建的InEKF-LC框架在保持标准 InEKF几何一致性的基础上，仅引入轻量级线性约束，计算开销增幅控制在9%以内，具备良好的工程可部署性。
3） 在INS/GNSS仿真与真实无人机飞行实验中，该方法相较标准 InEKF、UKF-LG与EKF-LG均表现出显著性能优势：仿真实验中姿态与位置误差分别降低84.0%与73.4%；实物实验中三段轨迹平均定位误差降低53%，在风扰与控制偏置条件下仍维较高精度。
4） 本文方法的有效性建立在模型失配具有可建模结构且偏差幅度有限的前提之上。当系统动态模型严重失准、先验信息高度不可信时，递推滤波器可能因引入有偏先验而劣于仅依赖观测的直接估计［24］。未来工作可结合模型置信度评估或自适应约束机制，进一步避免在强失配场景下引入有害先验。

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
[1]
BARRAU A， BONNABEL S. The geometry of navigation problems［J］. IEEE Transactions on Automatic Control， 2023， 68（2）： 689-704.

[2]
HARTLEY R， GHAFFARI M， EUSTICE R M， et al. Contact-aided invariant extended Kalman filtering for robot state estimation［J］. The International Journal of Robotics Research， 2020， 39（4）： 402-430.

[3]
FU M L， LU X Y， JIN Y Q， et al. Semantic map-based visual localization with consistency guarantee［J］. IEEE Sensors Journal， 2024， 24（1）： 1065-1078.

[4]
LEE T. Bayesian attitude estimation with the matrix fisher distribution on SO（3）［J］. IEEE Transactions on Automatic Control， 2018， 63（10）： 3377-3392.

[5]
WANG W X， LEE T. INS-GNSS navigation for large attitude uncertainties with the matrix fisher-Gaussian distribution［C］∥2023 American Control Conference （ACC）. Piscataway： IEEE Press， 2023： 4858-4863.

[6]
BARRAU A， BONNABEL S. The invariant extended Kalman filter as a stable observer［J］. IEEE Transactions on Automatic Control， 2017， 62（4）： 1797-1812.

[7]
BARRAU A， BONNABEL S. Invariant Kalman filtering［J］. Annual Review of Control， Robotics， and Autonomous Systems， 2018， 1： 237-257.

[8]
VAN GOOR P， HAMEL T， MAHONY R. Equivariant filter （EqF）［J］. IEEE Transactions on Automatic Control， 2023， 68（6）： 3501-3512.

[9]
JIN Y Q， ZHANG W A， TANG J W， et al. A nonlinear filter for pose estimation based on fast unscented transform on lie groups［J］. IEEE Robotics and Automation Letters， 2024， 9（11）： 10431-10438.

[10]
杨旭升， 夏晓翠， 金宇强， 等. 基于广义最大相关熵准则的几何滤波方法［J］. 自动化学报， 2025， 51（7）： 1612-1625.

YANG X S， XIA X C， JIN Y Q， et al. Geometric filtering method based on generalized maximum correntropy criterion［J］. Acta Automatica Sinica， 2025， 51（7）： 1612-1625 （in Chinese）.

[11]
BROSSARD M， BARRAU A， BONNABEL S. A code for unscented Kalman filtering on manifolds （UKF-M）［C］∥2020 IEEE International Conference on Robotics and Automation （ICRA）. Piscataway： IEEE Press， 2020： 5701-5708.

[12]
JIN Y Q， ZHANG W A， LU X Y， et al. Computationally efficient lie algebraic particle filters for state estimation［J］. IEEE Transactions on Automatic Control， 2025， 70（5）： 3370-3377.

[13]
崔加瑞， 张礼廉， 王茂松， 等. 基于改进李群卡尔曼滤波的惯性/里程计组合导航方法［J］. 控制理论与应用， 2023， 40（12）： 2179-2186.

CUI J R， ZHANG L L， WANG M S， et al. An improved LG-EKF for SINS/ODO integrated navigation［J］. Control Theory & Applications， 2023， 40（12）： 2179-2186 （in Chinese）.

[14]
何友， 刘瑜， 李耀文， 等. 多源信息融合发展及展望［J］. 航空学报， 2025， 46（6）： 531672.

HE Y， LIU Y， LI Y W， et al. Development and prospects of multisource information fusion［J］. Acta Aeronautica et Astronautica Sinica， 2025， 46（6）： 531672 （in Chinese）.

[15]
DUNÍK J， STRAKA O， KOST O， et al. Noise covariance matrices in state-space models： A survey and comparison of estimation methods： Part Ⅰ［J］. International Journal of Adaptive Control and Signal Processing， 2017， 31（11）： 1505-1543.

[16]
南子寒， 刘大禹， 董明， 等. GNSS拒止下多源自主导航鲁棒滤波方法［J］. 航空学报， 2024， 45（S1）： 730782.

NAN Z H， LIU D Y， DONG M， et al. Robust filtering method for GNSS denied multi-source autonomous navigation［J］. Acta Aeronautica et Astronautica Sinica， 2024， 45（S1）： 730782 （in Chinese）.

[17]
CHEN B D， LIU X， ZHAO H Q， et al. Maximum correntropy Kalman filter［J］. Automatica， 2017， 76： 70-77.

[18]
CHANG L， HU B， CHANG G， et al. Huber-based novel robust unscented Kalman filter［J］. IET Science， Measurement & Technology， 2012， 6（6）： 502-509.

[19]
ZHANG J， YANG X S， ZHANG W A. A progressive Bayesian filtering framework for nonlinear systems with heavy-tailed noises［J］. IEEE Transactions on Automatic Control， 2023， 68（3）： 1918-1925.

[20]
胡玉梅， 潘泉， 邓豹. 基于Fisher信息的传感器航迹自适应滤波算法［J］. 航空学报， 2024， 45（20）： 629825.

HU Y M， PAN Q， DENG B. A Fisher information based adaptive filtering algorithm for sensor trajectory planning［J］. Acta Aeronautica et Astronautica Sinica， 2024， 45（20）： 629825 （in Chinese）.

[21]
TEIXEIRA B O S， CHANDRASEKAR J， PALANTHANDALAM-MADAPUSI H J， et al. Gain-constrained Kalman filtering for linear and nonlinear systems［J］. IEEE Transactions on Signal Processing， 2008， 56（9）： 4113-4123.

[22]
HRUSTIC E， ABDALLAH R BEN， VILÀ-VALLS J， et al. Robust linearly constrained extended Kalman filter for mismatched nonlinear systems［J］. International Journal of Robust and Nonlinear Control， 2021， 31（3）： 787-805.

[23]
BOURMAUD G， MÉGRET R， ARNAUDON M， et al. Continuous-discrete extended Kalman filter on matrix lie groups using concentrated Gaussian distributions［J］. Journal of Mathematical Imaging and Vision， 2015， 51（1）： 209-228.

[24]
LI T C， CORCHADO J M， BAJO J， et al. Effectiveness of Bayesian filters： An information fusion perspective［J］. Information Sciences， 2016， 329： 670-689.

Options
文章导航

/

〈 〉

地址：北京市海淀区北四环中路辅路238号柏彦大厦

邮政编码：100083

E-mail：hkxb@buaa.edu.cn

关于我们

期刊社服务

专业学科

封面文章

友情链接

版权所有 © 航空学报编辑部

版权所有 © 2011航空学报杂志社

主管单位：中国科学技术协会 主办单位：中国航空学会 北京航空航天大学