基于矩阵李群的分布式不变卡尔曼滤波：多机器人协同定位DInEKF: Distributed Invariant Kalman Filter for Cooperative Localization

01 动机Motivation

在 GPS 拒止的室内、水下等环境里，多机器人系统常靠协同定位（CL）来确定各自的位姿：每台机器人既用自身的 ego-motion 与传感器测量，也用队友共享的信息。集中式做法精度最优，但依赖一个 fusion center，既怕单点失效，通信与计算开销也大；全分布式做法可扩展、更鲁棒，代价是——机器人之间的互相关变得「unknown and intractable」，天真地当作无关去融合就会得到不一致的估计器。In GPS-denied environments such as indoor and underwater scenes, multi-robot systems commonly rely on Cooperative Localization (CL) to determine their poses: each robot uses both its own ego-motion and sensor measurements and the information shared by its teammates. The centralized approach is optimal in accuracy but depends on a fusion center, so it is vulnerable to a single-node failure and heavy in communication and computation; the fully distributed approach is scalable and more robust, at the price that the cross-correlation between robots becomes "unknown and intractable" — naively fusing it as if it were absent yields an inconsistent estimator.

既有工作用 covariance intersection（CI）、split CI、inverse CI 等融合规则来处理未知相关性，但这些方法只在「每个局部估计本身是一致的」前提下才给出一致的融合结果——而标准 EKF 恰恰不满足这个前提。问题的根源在可观测性：EKF 在当前状态估计处线性化，Jacobian 因此是估计值的函数，使 CL 系统的不可观测子空间维度被人为压低，本不该被观测到的状态「变得可观测」，估计器沿这些方向获得 spurious information，协方差越估越小。Existing work handles the unknown correlation with fusion rules such as covariance intersection (CI), split CI and inverse CI, but these methods only give a consistent fused result under the premise that each local estimate is itself consistent — and the standard EKF is exactly what fails that premise. The root cause lies in observability: the EKF linearizes at the current state estimate, so its Jacobians are functions of that estimate, which artificially lowers the dimension of the CL system's unobservable subspace; states that should not be observable "become observable", and the estimator gains spurious information along those directions, shrinking its covariance more and more.

In EKF, the Jacobian of the linearized dynamics and the measurement are functions of the estimated states, which causes the standard EKF-based CL algorithm to have an unobservable subspace of lower dimension than that of the underlying CL system.

02 方法Method

DInEKF 不去手动修 Jacobian，也不加人工的可观测性约束，而是换一个状态表示：把机器人的状态写成矩阵李群 SE₂(3) 的元素，用右不变误差定义估计误差。因为机器人的运动模型在无噪声时是 group affine 的，误差满足 log-linear 性质，线性误差动力学矩阵 A 变成时不变、且完全不含状态估计——标准 EKF 的不一致根源就此消失。算法本身是「传播 → 本地绝对观测更新 → 相对观测更新」三步，最后一步用 CI 把邻居的修正量融进来。DInEKF neither patches the Jacobians by hand nor adds artificial observability constraints; it changes the state representation instead: the robot state is written as an element of the matrix Lie group SE₂(3) and the estimation error is defined through the right-invariant error. Because the robot motion model is group affine in the noise-free case, the error obeys the log-linear property and the linear error dynamics matrix A becomes time-invariant and entirely free of the state estimate — the root cause of the standard EKF's inconsistency disappears. The algorithm itself has three steps, propagation, local measurement update and relative measurement update, with the last one folding in the neighbors' corrections via CI.

状态放到 SE₂(3) 上，误差是右不变的State on SE₂(3), with a right-invariant error

每台机器人的状态是朝向 R ∈ SO(3)、速度 v 与位置 p（论文写作 3×5 的紧凑形式），它们天然地构成 SE₂(3) 上的一个 5×5 矩阵。右不变误差定义为 η = X (X̂)⁻¹，在群作用下不变；它的对数 ξ ∈ se₂(3) 是 9 维误差向量（朝向、速度、位置各 3 维），协方差也在这 9 维上传播。关键在于误差动力学矩阵 A 只含重力的叉乘项与一个单位块：它是时不变的，与状态估计无关，因此 Jacobian 是 state estimate-independent 的。Each robot's state is its orientation R ∈ SO(3), velocity v and position p (the paper writes them in the compact 3×5 form), which together form naturally a 5×5 matrix on SE₂(3). The right-invariant error is defined as η = X (X̂)⁻¹ and is invariant under the group action; its logarithm ξ ∈ se₂(3) is a 9-dimensional error vector (3 dimensions each for orientation, velocity and position), and the covariance is propagated on those 9 dimensions as well. The key point is that the error dynamics matrix A contains only the skew term of gravity and one identity block: it is time-invariant and independent of the state estimate, so the Jacobians are state estimate-independent.

三步走：Algorithm 1（DIKF-CL）Three steps: Algorithm 1 (DIKF-CL)

用 covariance intersection 应付未知的互相关Covariance intersection for the unknown cross-correlation

若按标准 EKF 直接把邻居的修正量全部叠加，等于假设机器人 i 与 j 的估计互不相关。可实际上，机器人 i 一旦用过 j 的估计，两者从下一时刻起就相关，且这份互相关会一直传播下去，在分布式设定下既未知又难以追踪。DInEKF 因此改用 CI-EKF：给自己的中间估计配权重 α_i、给每条相对修正配权重 α_ij，满足 α_i + Σ α_ij = 1，融合出的协方差保守但一致。论文的一致性论证也正是分两层：局部估计器的一致性由不变误差（时不变的 A）保证，融合环节的一致性由 CI 保证。Summing up all the neighbors' corrections in the standard EKF way amounts to assuming that the estimates of robots i and j are mutually uncorrelated. In reality, once robot i has used j's estimate the two become correlated from the next timestep on, and that cross-correlation keeps propagating — unknown and hard to track in a distributed setting. DInEKF therefore switches to CI-EKF: a weight α_i is given to its own intermediate estimate and a weight α_ij to each relative correction, subject to α_i + Σ α_ij = 1, so that the fused covariance is conservative yet consistent. The paper's consistency argument runs on the same two levels: the consistency of the local estimator is guaranteed by the invariant error (the time-invariant A), and the consistency of the fusion step by CI.

Naively fusing the correlated information makes the estimate overconfident and causes inconsistency in the estimator.

03 实验Experiments

仿真：四台机器人在三种三维轨迹场景中各跑 50 次蒙特卡洛。每台机器人配 IMU（100 Hz）测 ego-motion，配 UWB（10 Hz）测到其它机器人的相对距离与到 UWB 基站的绝对距离；UWB 感知半径 10 m、测距噪声 0.05 m。基线是用解耦误差状态的四元数分布式 EKF（QDEKF）。指标是 RMSE 与 NEES——对一致的估计器，NEES 应接近估计的自由度，本文的位置 NEES（PNEES）与朝向 NEES（ONEES）理想值都是 3。实机：四台 Crazyflie 纳米四旋翼在 10 m×6 m×5 m 的室内空间飞行，十六台 OptiTrack 相机给真值；机载 BMI088 以 100 Hz 记 IMU，UWB tag 测到四个固定基站的绝对距离，相对观测则以 10 Hz 由动捕真值算距离再加噪生成。Simulation: four robots run 50 Monte-Carlo trials in each of three 3-D trajectory scenarios. Every robot carries an IMU (100 Hz) for ego-motion and a UWB (10 Hz) measuring relative distances to the other robots and absolute distances to UWB stations; the UWB range is 10 m and its ranging noise 0.05 m. The baseline is the quaternion-based distributed EKF (QDEKF) with decoupled error states. The metrics are RMSE and NEES — for a consistent estimator the NEES ought to be close to the estimation's degrees of freedom, which here is 3 for both position NEES (PNEES) and orientation NEES (ONEES). Hardware: four Crazyflie nano quadrotors fly in a 10 m×6 m×5 m indoor space with sixteen OptiTrack cameras providing ground truth; the onboard BMI088 logs IMU at 100 Hz, a UWB tag measures absolute distances to four fixed stations, and the relative measurements are generated at 10 Hz by computing distances from the motion-capture ground truth and adding noise.

仿真结果（50 次蒙特卡洛平均，每台机器人）Simulation results (averaged over 50 Monte-Carlo runs, per robot)

位置精度两者接近（0.057 对 0.059 等），差距主要在朝向：DInEKF 的 ORMSE 大约只有 QDEKF 的一半到二分之一强（0.669 对 1.097、0.794 对 1.169、0.662 对 1.269）。一致性上，DInEKF 的 ONEES 在三个场景都贴着 3，而 QDEKF 最大到 3.815——NEES 偏大意味着估计器过于自信。PNEES 方面 QDEKF 的 3.063 在 Traj. 3 上比 DInEKF 的 3.035 更远离 3，但两者差别不大。Position accuracy is close between the two (0.057 versus 0.059, and so on); the gap sits mainly in orientation: the ORMSE of DInEKF is roughly a half to somewhat more than a half of QDEKF's (0.669 versus 1.097, 0.794 versus 1.169, 0.662 versus 1.269). On consistency, the ONEES of DInEKF hugs 3 in all three scenarios while QDEKF reaches up to 3.815 — a larger NEES means an overconfident estimator. For PNEES, QDEKF's 3.063 on Traj. 3 is farther from 3 than DInEKF's 3.035, though the difference is small.

实机结果（两组实验数据集）Real-world results (two experimental datasets)

实机上结论基本一致，但要如实指出：第一组数据集里 QDEKF 的位置 RMSE 反而更好（0.20 m 对 DInEKF 的 0.22 m），DInEKF 的优势体现在朝向（2.01 对 2.44 度）与一致性（PNEES 3.27 对 3.49、ONEES 3.29 对 3.41）。第二组数据集上 DInEKF 则四项全胜。The hardware results point the same way, but it should be stated plainly: on the first dataset the position RMSE of QDEKF is actually better (0.20 m against DInEKF's 0.22 m), and DInEKF's advantage shows up in orientation (2.01 against 2.44 degrees) and in consistency (PNEES 3.27 against 3.49, ONEES 3.29 against 3.41). On the second dataset DInEKF wins on all four metrics.

消融实验：论文没做Ablations: none reported in the paper

全文只有 DInEKF 与 QDEKF 的整体对比，没有把 SE₂(3) 状态表示与 CI 融合两部分拆开单独消融，因此「哪一部分贡献更大」在这篇论文里没有答案。作者对差距的解释是明确的：QDEKF「uses the current estimate to compute the linearized Jacobians, which gains 'spurious information' along the unobservable space and leads to inconsistency」，而这一问题被不变误差的性质彻底消除。The whole paper only compares DInEKF against QDEKF as a whole; it never separates the SE₂(3) state representation from the CI fusion into individual ablations, so "which part contributes more" has no answer here. The authors' explanation for the gap is explicit: QDEKF "uses the current estimate to compute the linearized Jacobians, which gains 'spurious information' along the unobservable space and leads to inconsistency", and this issue is completely eliminated by the properties of the invariant error.

04 局限性Limitations