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{array}{c}{f}_1(x_{pi},u_i)\\ f_2(x_{pi},u_i)\end{array}]$$
$f(x_p)$表示永强两次行走距离，是一个４＊１的向量，而
$$x_p=[\begin{array}{c}{x}_2\\ x_3\end{array}]$$
如果x_p是永强真实位置，那么$x_p-f(x_p)$应该是一个０均值,　Q方差的正太分布．正太分布的概率密度函数可以很好的刻画不同位置时，估计位置与永强估计的符合程度．
P ( x p ) = η x p e x p { − 1 / 2 ( x p − f ( x p ) ) T Q − 1 ( x p − f ( x p ) ) } P(x_p) = \eta_{x_p} exp\{-1/2(x_p - f(x_p))^TQ^{-1}(x_p - f(x_p))\} P(xp​)=ηxp​​exp{−1/2(xp​−f(xp​))TQ−1(xp​−f(xp​))}
上式中，将永强估计的位置这一观测，与其位置估计，建模为了一个多元正态分布（４＊１）．

三，传感器模型

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

四，先验信息

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

五，点估计

综合以上三类信息，可以估计出永强自己和房屋的最优位置，根据事件同时发生的原则，可以得出新的概率分布为：
P = P ( x p ) ∗ P ( x m ) P ( x p ^ ) = η x p η x m η x π e x p { − 1 / 2 ( x p − f ( x p ) ) T Q − 1 ( x p − f ( x p ) ) } ∗ e x p { − 1 / 2 ( z i − h ( x m , x p ) ) T R − 1 ( z i − h ( x m , x p ) ) } e x p { − 1 / 2 ( x p ^ − x p 1 ) T Π − 1 ( x p ^ − x p 1 ) } 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})\} P=P(xp​)∗P(xm​)P(xp^​​)=ηxp​​ηxm​​ηxπ​​exp{−1/2(xp​−f(xp​))TQ−1(xp​−f(xp​))}∗exp{−1/2(zi​−h(xm​,xp​))TR−1(zi​−h(xm​,xp​))}exp{−1/2(xp^​​−xp1​)TΠ−1(xp^​​−xp1​)}
根据指数的运算法则:$exp(A)\ast 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^T{Q}^{-1}{f}_p+f_m^T{R}^{-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^T{Q}^{-1}{f}_p+f_m^T{R}^{-1}{f}_m+f_{\hat{p}}^T{\Pi }^{-1}{f}_{\hat{p}})$$
可以令：$f=[f_p;f_m;f_{\hat{p}}]$,
$$C=[\begin{array}{ccc}Q& 0& 0\\ 0& R& 0\\ 0& 0& \Pi \end{array}]$$
其中：Q-4* 4, R-24* 24, $\Pi$-2* 2; 那么
$$L=1/2f^T{C}^{-1}f$$,
利用克莱斯基分解，可以将$C^{-1}=S^{T}S$, $r=Sf$, 则有：
$$L=1/2r^{T}r$$
上式为标准的最小二乘形式．可以使用迭代解法来求解．
理解一下ｒ，ｒ其实就是残差，它是永强和房屋位置的函数．

1. 一阶和二阶优化

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

一阶

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

二阶

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

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

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

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

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

4.如何舍弃历史轨迹．

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

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

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