A Sliding Window Filter

A Sliding Window Filter阅读

关于作者：Gabe Sibley, 07年博士毕业于南加州大学，剑桥三年博后，科罗拉多大学副教授，17年开始创业，成立了两家公司：Zippy Inc和Verdant Robotics, Inc．

一，系统参数化

为了形象一点，下面以永强作为机器人，永强观测到的房子作为路标点．
假设永强沿着象牙山庄一直朝东走，在三个点上停留并进行观察，能看到周围的四个房子，那么该系统的变量有永强停留的的三个位置 $(x_{p1}, x_{p2},x_{p3})$,　四所房子的位置 $(x_{m1}, x_{m2},x_{m3})$.其中 $x_{pi}, x_{mi}$均为2D的位置

二，过程模型

在永强行走的过程中，根据平日步行的经验，他知道自己走了大约多少米（即里程计信息），该信息并不十分准确，但是依然比较有用. $x_{pi+1} = f_i(x_{pi}, u_i) + w_i$ 为一般的过程模型，假设
$f(x_p) =[\begin{matrix} f_1(x_{pi}, u_i) \\f_2(x_{pi}, u_i) \end{matrix}]$
$f(x_p)$表示永强两次行走距离，是一个４＊１的向量，而
$x_p =[\begin{matrix} x_2 \\x_3 \end{matrix}]$
如果x_p是永强真实位置，那么 $x_p - f(x_p)$应该是一个０均值,　Q方差的正太分布．正太分布的概率密度函数可以很好的刻画不同位置时，估计位置与永强估计的符合程度．
$P(x_p) = \eta_{x_p} exp\{-1/2(x_p - f(x_p))^TQ^{-1}(x_p - f(x_p))\}$
上式中，将永强估计的位置这一观测，与其位置估计，建模为了一个多元正态分布（４＊１）．

三，传感器模型

这里的传感器，就是指永强的观测了．在每一个点上，永强都观测到４个房子，得到相对于当前位置的相对距离 $\delta x$.　对于一般的观测模型， $z_i = h(x_{mi}, x_{pi}) + v_i$.　当永强的位置和房子的位置都估计准时，那么由估计量计算的相对距离和永强观测的相对距离，应该是相等的，那么其差值应该是符合０为均值，Ｒ方差的正太分布，和上述过程模型一样，正太分布的概率密度函数可以很好的刻画不同位置时，估计位置与永强的观测值的符合程度：
$P(x_m) = \eta_{x_m}exp\{-1/2(z_i - h(x_{m},x_{p}))^TR^{-1}(z_i - h(x_{m},x_{p}))\}$
上式中，永强在每一个位置都会对所有房子进行观察，而观察一次是2＊1的向量，那么上式应该是24＊1维度的正太分布．

四，先验信息

先验信息是指对于永强的位置或者房子的相关信息的已有知识，在这里，永强从象牙山庄的最西头出发，其位置定义为(0, 0)，为了和上述概率表示一致，可以将该位置作为一个观测．当永强的位置估计准确时，第一个状态 $x_{p1}$应该是和该位置相等，那么其差值应该也是符合０为均值， $\Pi$为方差的正太分布，和上述模型一样，正太分布的概率密度函数可以很好的刻画不同位置时，估计位置与先验知识的符合程度：
$P(x_{\hat{p}}) = \eta_{x_\pi}exp\{-1/2(x_{\hat{p}} - x_{p1})^T\Pi^{-1}(x_{\hat{p}} - x_{p1})\}$
上式中，因为只对永强的起始位置有先验知识，上式表示的是2＊１维度的正态分布．

五，点估计

综合以上三类信息，可以估计出永强自己和房屋的最优位置，根据事件同时发生的原则，可以得出新的概率分布为：
$P = P(x_p)*P(x_m) P(x_{\hat{p}}) = \eta_{x_p} \eta_{x_m}\eta_{x_\pi} exp\{-1/2(x_p - f(x_p))^TQ^{-1}(x_p - f(x_p))\}\\*exp\{-1/2(z_i - h(x_{m},x_{p}))^TR^{-1}(z_i - h(x_{m},x_{p}))\}exp\{-1/2(x_{\hat{p}} - x_{p1})^T\Pi^{-1}(x_{\hat{p}} - x_{p1})\}$
根据指数的运算法则: $exp(A)*exp(B) = exp(A+B)$．令： $f_p =x_p - f(x_p)$, $f_m = z_i - h(x_{m},x_{p})$, $x_{\hat{p}} - x_{p1}$, 上述可以表示为:
$P =\eta_{x_p} \eta_{x_m}\eta_{x_\pi} exp(-1/2(f_p^TQ^{-1}f_p +f_m^TR^{-1}f_m+f_{\hat{p}}^T\Pi^{-1}f_{\hat{p}}))$
因为 $\eta_{x_p} \eta_{x_m}\eta_{x_\pi}$均大于０，且exp为单调递增函数，因此最大化上式等价于最小化下式：
$L = 1/2(f_p^TQ^{-1}f_p +f_m^TR^{-1}f_m+f_{\hat{p}}^T\Pi^{-1}f_{\hat{p}})$
可以令： $f = [f_p;f_m;f_{\hat{p}}]$,
$C = [\begin{matrix}Q & 0 & 0\\ 0 & R&0\\ 0 &0&\Pi \end{matrix}]$
其中：Q-4* 4, R-24* 24, $\Pi$-2* 2; 那么
$L = 1/2f^TC^{-1}f$,
利用克莱斯基分解，可以将 $C^{-1} = S^TS$, $r = Sf$, 则有：
$L = 1/2r^Tr$
上式为标准的最小二乘形式．可以使用迭代解法来求解．
理解一下ｒ，ｒ其实就是残差，它是永强和房屋位置的函数．

1. 一阶和二阶优化

为了简单说明一阶二阶优化的区别，这里采用 $f(x) = 1/2x^2$来理解．假设初始化在 $x_0 = 1$处．

一阶

此时梯度方向为f’,以梯度与 $x_0$处的点为直线，与 $y = 0$的交点处的x为：
$\delta x = (0 - f(x_0))/f' = -1/2$
那么 $x_1 = 1/2$

二阶

在 $x_0$处展开得： $f(x) \approx f(x_0) + f'/1*\delta x$，因为要求下降方向，那么 $f(x)$就要尽可能小,　即求f(x)最小，那么其一定出现在导数为０的地方．即
$f' + f''\delta x = 0$
即： $\delta x = -f'/f'' = -1$
那么 $x_1 = 0$
可以看出，二阶优化的步长相对于一阶优化，更准确，更快．

2. 根据初始值，求永强的位置

根据二阶优化的原则，永强的位置增量可以表示为:
$\delta x = - (L'')^{-1}L'^T = - (r'^Tr')^{-1}r'^Tr$
其中，L是1* 1, r 是30* 1, L’是1* 14, L’'是14* 14, $\delta x$是14* 1,
将 $r = Sf$, $C^{-1} = S^TS$, $J = f'$带入，得到
$\delta x = - (f'^TS^TSf')^{-1}f'^TS^TSf = -(J^TC^{-1}J)^{-1}J^TC^{-1}f$
法方程如下：
$(J^TC^{-1}J)\delta x=-J^TC^{-1}f$

3. 稀疏性，永强如何通过手算，解出自己和房屋的位置．

在上一节的法方程中，随着永强走的越远，观测到的房屋越多，状态增量 $\delta x$将会越来越大．从现在的14增加到正无穷，据说，象牙山庄一共有100座房子，而永强需要走120步，那么最终该向量的维度就有440维，这将是一个非常大的向量了．在学术上，对状态量进行删减的操作叫做边缘化marginization，名称也比较形象，指的就是远处的状态需要被边缘化，即不纳入计算．
由于观测的特殊性，上式中的 $J^TJ$一般具有稀疏性，如下图：（ＴＯＤＯ）
利用该稀疏性，可以将法方程写为：
$[\begin{matrix}\lambda _p && \lambda _{pm} \\\lambda _{pm}^T&& \lambda _m \end{matrix}][\begin{matrix}\delta x_{p} \\ \delta x_{m}\end{matrix}] = [\begin{matrix}g_p \\ g_m\end{matrix}]$
利用线性方程组的可消元性，可以得到：
$[\begin{matrix}\lambda _p- \lambda _{pm}/\lambda _m \lambda _{pm}^T&& 0 \\\lambda _{pm}^T&& \lambda _m \end{matrix}][\begin{matrix}\delta x_{p} \\ \delta x_{m}\end{matrix}] = [\begin{matrix}g_p - \lambda _{pm}/\lambda _m g_m \\ g_m\end{matrix}]$
进而可以解得 $\delta x_{p}$，带入第二个式子中，解得 $\delta x_{m}$．通常， $\lambda _{pm}/\lambda _m$叫做舍尔补．
原本是解一个14*14的方程组，现在变成了两个７＊7，完全可以通过手推的方式计算出来．

4.如何舍弃历史轨迹．

边缘化除了增加快速的状态求解，还能确定舍弃哪些变量，保留哪些变量．假设我们现在要舍弃第一个房屋的状态，那么如果直接删除，原文中说有信息损失，需要使用边缘化操作，才能保证信息不损失．上面的例子不太直观，以下使用一个简单例子来说明必须使用边缘化来舍弃
假设永强的真实位置是 $x_{p1} = 0, x_{p2} = 2$ , 房子的真实位置是 $x_{m} = 1$，假设永强的根据步子的估计不准，本来是走了２米，结果永强估计的是1.５米，观测很准，即在两个位置上观察都是一米，那么可以得到如下残差项
$f_1 = 1.5 - (x_{p2} - x_{p1}) \\ f_2 = 1 - (x_m - x_{p1})\\ f_3 = -1 - (x_m - x_{p2})\\ f_4 = x_{p1}$
以下构造三种解法(1)全局解，（２）直接删除房子，（３）采用边缘化的方式消除房子状态．初始值假设为0,0,0;

1. 全局解，构造完成的雅克比矩阵与残差矩阵
   $f =\left [ \begin{matrix}1.5\\ 1 \\ -1\\0\end{matrix} \right ]$
   $J = \left [ \begin{matrix}1&-1&0\\ 1&0&-1\\0&1&-1\\ 1&0&0 \end{matrix} \right ]$
   根据法方程，计算出增量为：
   $\delta x= [0; 1.6; 0.8]$
2. 直接删除房子的结构
   $f =\left [ \begin{matrix}1.5\\0\end{matrix} \right ]$
   $J = \left [ \begin{matrix}1&-1\\ 1&0\end{matrix} \right ]$
   根据法方程，计算出增量为：
   $\delta x= [0; 1.5]$
3. 使用边缘化，保留房子信息，但不解房子的位置
   $\left [ \begin{matrix}3 & -1 &-1\\ -1 & 2& -1\\ -1 & -1 & 2\end{matrix} \right ] \left [\begin{matrix}\delta x_p \\\delta x_m\end{matrix}\right ] = \left [\begin{matrix} -2.5\\2.5 \\0\end{matrix}\right ]$
   其中舒尔补为 $\lambda_{pm}/\lambda_m$，线性变换之后可得：
   $0.5\left [ \begin{matrix}5 & -3 &0\\ -3 & 3& 0\\ -1 & -1 & 2\end{matrix} \right ] \left [\begin{matrix}\delta x_p \\\delta x_m\end{matrix}\right ] = \left [\begin{matrix} -2.5\\2.5 \\0\end{matrix}\right ]$
   即：
   $0.5\left [ \begin{matrix}5 & -3 \\ -3 & 3\\ \end{matrix} \right ] \left [\begin{matrix}\delta x_p \end{matrix}\right ] = \left [\begin{matrix} -2.5\\2.5\end{matrix}\right ]$
   解得：
   $\delta x = [0; 1.6]$
   比较这三种方式解的结果，可以得到如下结论：

• 全部解是最优的，而采用边缘化的方式与全部解是一致的．
• 直接删除变量会导致信息损失．即相当于永强没有观测到这个房子．
  slding window的精髓在于不随意舍弃变量
  下一篇将介绍EKF与最小二乘的区别．