Skip to content

Sparsity Extended Information Filter SLAM

M. R. Walter, R. M. Eustice, and J. J. Leonard, “Exactly Sparse Extended Information Filters for Feature-based SLAM,” The International Journal of Robotics Research, vol. 26, no. 4, pp. 335–359, Apr. 2007, doi: 10.1177/0278364906075026.

EKF 通过均值和协方差矩阵估计机器人状态和地图,但协方差矩阵的计算和更新复杂度为  O(n2),限制了其在小规模环境中的应用。子图方法通过划分环境降低计算负担,但可能牺牲全局地图的实时性和收敛速度。

EIF 使用信息矩阵(逆协方差矩阵)和信息向量描述高斯分布,更新步骤复杂度为  O(m2),优于 EKF。然而,时间预测步骤仍为  O(n2),且均值恢复需要  O(n3)  的矩阵求逆,限制了其在大规模环境中的应用。

SEIF 利用信息矩阵的稀疏性,将非对角元素近似为零,显著降低了更新和时间预测的计算成本,接近常数复杂度。但这种近似往往造成滤波器的过分自信,同时它依赖稀疏性和均值估计的近似求解,使得在实际应用中仍面临挑战。

Gaussian Probability

Duality of Covariance and Information

对服从高斯分布的多元随机变量 ξtN(μt,Σt) 可以通过均值向量 μt 和协方差矩阵 Σt 参数化,同时也可以被规范形式 N1(ηt,Λt) 所表示,其中 Λt=Σt1,ηt=Σt1μt.

p(ξt)=N(μt,Σt)exp{12(ξtμt)Σt1(ξtμt)}=exp{12(ξtΣt1ξt2μtΣt1ξt+μtΣt1μt)}exp{12ξtΣt1ξt+μtΣt1ξt}=exp{12ξtΛtξt+ηtξt}N1(ηt,Λt)

在标准形式中,边缘化操作只需从均值向量和协方差矩阵中移除相应的元素。然而在规范形式中,边缘化操作需要计算舒尔补,计算复杂度较高。条件化则相反,在标准形式中操作较为复杂,在规范形式下则相对简单。

Implied Conditional Independence

p(ξ)exp{12ξTΛξ+ηTξ}=exp{i(ηiξi12jξiλijξj)}=iexp{12λiiξi2+ηiξi}ijexp{12ξiλijξj}=iΨi(ξi)ijΨij(ξi,ξj)

其中

Ψi(ξi)=exp{12λiiξi2+ηiξi}Ψij(ξi,ξj)=exp{12ξiλijξj}

“The meaning of a zero in an inverse covariance matrix (at location i,j) is conditional on all the other variables, these two variables i and j are independent. ... So positive off-diagonal terms in the covariance matrix always describe positive correlation; but the off-diagonal terms in the inverse covariance matrix can’t be interpreted that way. The sign of an element (i,j) in the inverse covariance matrix does not tell you about the correlation between those two variables.” (MacKay and Cb, 2006, p. 4)

如果信息矩阵中的非对角元素为零,即 λij=0Ψij(ξi,ξj)=1,这意味着两个节点之间没有边约束,表明 ξiξj 条件独立。相反,如果非对角元素不为零,则表明 ξiξj 之间存在一条边约束,其强度正比于 λij。这种关系很好地体现在无向图中,直观地反映了变量之间的条件独立性。使用规范形式的一个主要好处是,信息矩阵 Λ  提供了马尔可夫场的显式结构表示,清晰地揭示了变量之间的依赖关系。关于协方差矩阵和信息矩阵的更多深入理解,可以参考 David J.C. MacKay 2006 年的手稿 The Humble Gaussian Distribution.

Extended Information Filter

p(ξt|zt,ut)=N(μt,Σt)=N1(ηt,Λt)

记状态 ξt=[xtTMtT]T 为机器人位姿为 xt 和地图特征 M={m1,,mn} 的组合,z1:tu1:t 表示观测数据和输入的历史。地图基于信息矩阵的结构被划分为两个集合,M=(m+,m),其中 m+ 包含那些与机器人存在非零非对角项连接的地图元素,而 m 则表示与车辆位姿条件独立的特征。

Measurement Update Step

观测对减小机器位姿和地图的估计的不确定性有重要影响,在均值处对非线性观测模型做一阶近似

zt=h(ξt)+vth(μ¯t)+H(ξtμ¯t)+vt,vtN(0,R)

根据马尔科夫假设 p(zt|ξt,z1:t1,u1:t)=p(zt|ξt)ξt,p(zt|z1:t1u1:t)=1η,贝叶斯定理给出

p(ξt|z1:t,u1:t)=p(ξt|zt,z1:t1,u1:t)=p(zt|ξt,z1:t1,u1:t)p(ξt|z1:t1,u1:t)p(zt|z1:t1,u1:t)=ηp(zt|ξt)p(ξt|z1:t1,u1:t)=N1(ηt,Λt)

在更新时,EIF 估计规范形式的新的后验概率

Λt=Λ¯t+HTR1Hηt=η¯t+HTR1(zth(μ¯t)+Hμ¯t)

其中测量模型是一个只包含机器当前位姿以及现在观测到的地标的函数,在雅可比中表现为极度稀疏(没观测到的地标的梯度为 0

H=[h1ξt0h1mi0hmξthmmj00]

信息矩阵通过稀疏矩阵  HTR1H 进行更新,仅修改与机器人位姿和观测地标相关的项,从而加强或建立新的约束;由于  H  的稀疏性和机器人视野的限制,更新时间与观测数量  m  相关,复杂度为  O(m2),且不随地图规模增长;在非线性情况下,计算均值需要  O(n3) 的矩阵求逆,而在线性情况下,更新可以在常数时间内完成,无需均值计算。

Time Projection Step

时间预测包括状态扩展和边缘化。首先参考观测模型,对于运动学模型我们同样给出一阶近似

xt+1=f(xt,ut+1)+wtf(μxt,ut+1)+F(xtμxt)+wt,wtN(0,Q)

首先将新的机器人位姿加入状态向量,并同步扩展信息矩阵和信息向量。其中状态向量 ξ^t+1=[xtT,xt+1T,MT]T 遵循后验分布 p(ξt|z1:t,u1:t)=N1(ηt,Λt). 如图所示,根据马尔科夫性质,新的位姿只和上一步位姿相关而与地图无关。

p(ξ^t+1|z1:t,u1:t+1)=p(xt+1,ξt|z1:t,u1:t+1)=p(xt+1|xt,ut+1)p(ξt|z1:t,u1:t)

新的状态估计服从更新后的高斯分布

p(xt,xt+1,M|z1:t,u1:t+1)=N(μ^t+1,Σ^t+1)=N1(η^t+1,Λ^t+1)Σ^t+1=[ΣxtxtFΣxtxtFΣxtMΣxtxtFTFΣxtxtFT+QΣxtMΣMxtFTΣMxtΣMM]=[Σ^t+111Σ^t+112Σ^t+121Σ^t+122]μ^t+1=[μxtf(μxt,ut+1)μM]=[μ^t+11μ^t+12]

由协方差矩阵和信息矩阵的对偶性得到

Λ^t+1=[Λxtxt+FQ1FFTQ1ΛxtMQ1FQ10ΛMxt0ΛMM]=[Λ^t+111Λ^t+112Λ^t+121Λ^t+122]η^t+1=[ηxtFTQ1[f(μxt,ut+1)Fμxt]Q1[f(μxt,ut+1)Fμxt]ηM]=[η^t+11η^t+12]

第二步是边缘化 xt, 使状态向量变为 ξt+1=[xt+1T,MT]T.

p(xt+1,M|z1:t,u1:t+1)=xtp(xt,xt+1,M|z1:t,u1:t+1)dxtp(ξt+1|z1:t,u1:t+1)=N1(η¯t+1,Λ¯t+1)

η¯t+1Λ¯t+1 由前面表中给出的边缘化公式得到

Λ¯t+1=Λ^t+122Λ^t+121(Λ^t+111)1Λ^t+112η¯t+1=η^t+12Λ^t+121(Λ^t+111)1η^t+11

虽然 EIF 能高效处理新观测的增量更新,但通过边缘化旧位姿时产生的全连接问题会导致信息矩阵迅速稠密化,使得运动预测的计算复杂度达到 O(n2) 量级。边缘化过程会在新位姿与被移除旧位姿关联的所有特征 m+ 之间建立新的信息连接,导致信息矩阵稠密化。但由于这些新连接通常具有较弱的关联强度,这为通过稀疏化近似来维持计算效率提供了可能,即保留强连接舍弃弱连接。

Sparse Extended Information Filter

Active Sparsity Maintenance

记原先激活后续变为被动的特征为 m0,则地图被划分为 M={m0,m+,m} 三部分。下图表明通过主动控制激活特征断开,可以有效控制信息矩阵的稀疏性。而对与机器无关联的地标 m,我们可以任意给出估计 ϕ,但实际上如果用了非均值的估计会使 SEIF 失准。

SEIF 给出去除 m0 后的近似后验估计

p~SEIF(ξt|z1:t,u1:t)=p~SEIF(xt,m0,m+,m|z1:t,u1:t)=p(xt|m+,m=ϕ,z1:t,u1:t)p(m0,m+,m,z1:t,u1:t)

Discussion on Overconfidence

假设三元变量 [a,b,c] 服从高斯分布

p(a,b,c)=N([μaμbμc],[σa2ρabσaσbρacσaσcρabσaσbσb2ρbcσbσcρacσaσcρbcρbρcσc2])=N1([ηaηbηc],[λaaλabλacλabλbbλbcλacλbcλcc])

并且变量 a,b 在条件 c 下独立,将近似结果记为 p~(a,b,c)

p(a,b,c)=p(a,b|c)p(c)p(a|c)p(b|c)p(c)=p~(a,b,c)

对于在 c 条件下条件独立的 ab, 前面讨论过合适的做法是把信息矩阵的 λab 设为 0, 这等价于协方差矩阵变为

p~(a,b,c)=N([μaμbμc],[σa2ρacρbcσaσbρacσaσcρacρbcσaσbσb2ρbcσbσcρacσaσcρbcρbρcσc2])

为了保证近似后的一致性,一个充要条件是 Σ¯Σ 半正定

Σ¯Σ=[0(ρacρbcρab)σaσb0(ρacρbcρab)σaσb00000]0

其中,一个使 Σ¯Σ 半正定的必要条件是左上的 2×2 子矩阵非负。

det([0(ρacρbcρab)σaσb(ρacρbcρab)σaσb0])=[(ρacρbcρac)σaσb]20

只有在 ρab=ρacρbcΣ¯Σ 半正定,否则强制稀疏化会导致信息矩阵过于自信,即信息矩阵被过度强化,导致估计的协方差比实际小。