Marginalization in Sliding Window Optimization滑动窗口优化的边缘化方法

目前的VIO(Visual Inertial Odometry)都会建立图优化(Graph Optimization)来求解，为了节省计算量，一般采用滑动窗口法(Sliding Window)。本文将从概率论的框架下阐述Sliding Window的原理，以及Marginalization的计算方法。

Sliding Window Optimization

简单模型

{xk=zk=f(xk−1)+wkh(xk)+nk(1) (1) { x k = f ( x k − 1 ) + w k z k = h ( x k ) + n k

其中 {xk} { x k } 为系统的状态， {zk} { z k } 为观测值。我们的最终目标即求最大条件概率

x∗0:T=argmaxx0:Tp(x0:T|z0:T)(2) (2) x 0 : T ∗ = arg ⁡ max x 0 : T p ( x 0 : T | z 0 : T )

如果直接对整个时间段 t=[0,T] t = [ 0 , T ] 计算优化求解，即使Hessian矩阵存在稀疏性，依然会设由于时间的累积导致计算量急剧增大。一般会使用sliding window的方法进行求解。

假设sliding window的窗口为 N N ，则 t=N−1 t = N − 1 时刻对应的条件概率为 p(x0:N−1|z0:N−1) p ( x 0 : N − 1 | z 0 : N − 1 ) 。在下一时刻 t=N t = N 时，需要向优化框架中增加状态 xN x N 和测量值 zN z N ，此时对应的条件概率为

p(x0:N|z0:N)=∝=p(x0,x1:N|z0:N)=p(x0|z0:N)p(x1:N|x0,z0:N)p(x0|z0:N)p(x1:N|x0,z0)p(x1:N|z1:N)p(x0|z0:N)p(x1:N|x0)p(x1:N|z1:N)(3) (3) p ( x 0 : N | z 0 : N ) = p ( x 0 , x 1 : N | z 0 : N ) = p ( x 0 | z 0 : N ) p ( x 1 : N | x 0 , z 0 : N ) ∝ p ( x 0 | z 0 : N ) p ( x 1 : N | x 0 , z 0 ) p ( x 1 : N | z 1 : N ) = p ( x 0 | z 0 : N ) p ( x 1 : N | x 0 ) p ( x 1 : N | z 1 : N )

则有

x∗1:N===argmaxx1:Np(x0:N|z0:N)argmaxx1:Np(x0|z0:N)p(x1:N|x0)p(x1:N|z1:N)argmaxx1:Np(x1:N|x0)p(x1:N|z1:N)(4) (4) x 1 : N ∗ = arg ⁡ max x 1 : N p ( x 0 : N | z 0 : N ) = arg ⁡ max x 1 : N p ( x 0 | z 0 : N ) p ( x 1 : N | x 0 ) p ( x 1 : N | z 1 : N ) = arg ⁡ max x 1 : N p ( x 1 : N | x 0 ) p ( x 1 : N | z 1 : N )

这里，将最早的状态 x0 x 0 和最早的测量值 z0 z 0 从窗口的剔除掉了，并转换成先验信息 p(x1:N|x0) p ( x 1 : N | x 0 ) ，从而维持求解方程的个数始终保持不变。这个过程叫做边缘化，即marginalization。

从 (3)和(4) (3) 和 (4) 可以看出，sliding window optimization其实是一个增量式的优化过程，通过上一个窗口 t=[0,N−1] t = [ 0 , N − 1 ] 求得的条件概率密度 p(x1:N|x0) p ( x 1 : N | x 0 ) 作为下一个窗口 t=[1,N] t = [ 1 , N ] 的变量，这个变量在很多论文中解释成先验信息（Prior Info），从而保持整个窗口计算复杂度的一致性和信息传递的一致（即信息不丢失）。

复杂模型

上面的为最简单的模型，但对于SLAM或VIO来说，可能会更复杂。 {xk} { x k } 为VIO中的状态，一般分为两类，一是每个时刻的车辆位姿、速度和IMU的两个bias，即 [R⊺k,p⊺k,v⊺k,b⊺gk,b⊺ak]⊺ [ R k ⊺ , p k ⊺ , v k ⊺ , b g k ⊺ , b a k ⊺ ] ⊺ ；另一类为三维点的坐标(对应于pose graph，如果是structureless graph，只有相应的尺度信息)。 {zk} { z k } 为观测值，同样有两类，分别是IMU的预积分和图像的角点(或KeyPoint)的位置信息。

对于复杂模型的推导，会有些不一样，不能简单的认为 zk z k 只与 xk x k 相关，假设状态依然为 x0:N x 0 : N ，测量集合为 z0:M z 0 : M (注意这里的下标是 m m ，而不是 N N )。必须将 x0:N x 0 : N 拆成三部分：要marginalized的状态集 xm x m ，与 xm x m 直接相关的状态集 xb x b ，剩下的其他状态集 xr x r 。对于 z0：M z 0 ： M 同样如此，然后其余推导与此类似。

Marginalization

待完善…

Marginalization的概率意义

待完善…