我怎么会写得那么长……如果您有兴趣可以和我一块把公式过一遍。
要讲清这个问题,得从状态估计理论来说。先摆上一句名言:
状态估计乃传感器之本质。(To understand the need for state estimation is to understand the nature of sensors.)任何传感器,激光也好,视觉也好,整个SLAM系统也好,要解决的问题只有一个: 如何通过数据来估计自身状态。 每种传感器的测量模型不一样,它们的精度也不一样。换句话说,状态估计问题,也就是「 如何最好地使用传感器数据 」。可以说,SLAM是状态估计的一个特例。
===================== 离散时间系统的状态估计 ======================
记机器人在各时刻的状态为x_1,\ldots,x_k ,其中k 是离散时间下标。在SLAM中,我们通常要估计机器人的位置,那么系统的状态就指的是机器人的位姿。用两个方程来描述状态估计问题:
\[\left\{ \begin{array}{l} {x_k} = f\left( {{x_{k - 1}},{u_k},{w_k}} \right)\\ {y_k} = g\left( {{x_k},{n_k}} \right) \end{array} \right.\]
解释一下变量:
f -运动方程
u - 输入
w - 输入噪声
g - 观测方程
y - 观测数据
n - 观测噪声
运动方程描述了状态x_{k-1} 是怎么变到x_k 的,而观测方程描述的是从x_k 是怎么得到观察数据y_k 的。
请注意这是一种抽象的写法。当你有实际的机器人,实际的传感器时,方程的形式就会变得具体,也就是所谓的 参数化 。例如,当我们关心机器人空间位置时,可以取x_k = [x,y,z]_k 。进而,机器人携带了里程计,能够得到两个时间间隔中的相对运动,像这样\Delta x_k=[\Delta x, \Delta y, \Delta z]_k ,那么运动方程就变为:
x_{k+1}=x_k+\Delta x_k+w_k
同理,观测方程也随传感器的具体信息而变。例如激光传感器可以得到空间点离机器人的距离和角度,记为y_k=[r,\theta]_k ,那么观测方程为:
\[{\left[ \begin{array}{l} r\\ \theta \end{array} \right]_k} = \left[ \begin{array}{l} \sqrt {{{\left\| {{x_k} - {L_k}} \right\|}_2}} \\ {\tan ^{ - 1}}\frac{{{L_{k,y}} - {x_{k,y}}}}{{{L_{k,x}} - {x_{k,x}}}} \end{array} \right] + {n_k}\] ,其中L_k=[L_{k,x},L_{k,y}] 是一个2D路标点。
举这几个例子是为了说明, 运动方程和观测方程具体形式是会变化的 。但是,我们想讨论更一般的问题:当我不限制传感器的具体形式时,能否设计一种方式,从已知的u,y (输入和观测数据)从,估计出x 呢?
这就是最一般的状态估计问题。我们会根据f,g 是否线性,把它们分为 线性/非线性系统 。同时,对于噪声w,n ,根据它们是否为高斯分布,分为 高斯/非高斯噪声 系统。最一般的,也是最困难的问题,是非线性-非高斯(NLNG, Nonlinear-Non Gaussian)的状态估计。下面先说最简单的情况:线性高斯系统。
===================== 线性高斯系统 ============================
线性高斯系统(LG,Linear Gaussian)
在线性高斯系统中,运动方程、观测方程是线性的,且两个噪声项服从零均值的高斯分布。这是最简单的情况。简单在哪里呢?主要是因为 高斯分布经过线性变换之后仍为高斯分布 。而对于一个高斯分布,只要计算出它的一阶和二阶矩,就可以描述它(高斯分布只有两个参数\mu, \Sigma )。
线性系统形式如下:
\left\{ \begin{array}{l} {x_k} = {A_{k - 1}}{x_{k - 1}} + {u_k} + {w_k}\\ {y_k} = {C_k}{x_k} + {n_k}\\ {w_k}\sim N\left( {0,{Q_k}} \right)\\ {n_k}\sim N(0,{R_k}) \end{array} \right. 其中Q_k,R_k 是两个噪声项的协方差矩阵。A,C 为转移矩阵和观测矩阵。
对LG系统,可以用贝叶斯法则,计算x 的后验概率分布——这条路直接通向 卡尔曼滤波器 。卡尔曼是线性系统的递推形式(recursive,也就是从x_{k-1} 估计x_k )的无偏最优估计。由于解释EKF和UKF都得用它,所以我们来推一推。如果读者不感兴趣,可以跳过公式推导环节。
符号:用\hat{x} 表示x 的后验概率,用\[\tilde x\] 表示它的先验概率。因为系统是线性的,噪声是高斯的,所以状态也服从高斯分布,需要计算它的均值和协方差矩阵。记第k 时刻的状态服从:x_k\sim N({{\bar x}_k},{P_k})
我们希望得到状态变量x 的最大后验估计(MAP,Maximize a Posterior),于是计算:
\[\begin{array}{ccl} \hat x &=& \arg \mathop {\max }\limits_x p\left( {x|y,u} \right)\\ &=& \arg \max \frac{{p\left( {y|x,u} \right)p\left( {x|u} \right)}}{{p\left( {y|v} \right)}} \\ &=& \arg \max p(y|x)p\left( {x|u} \right) \end{array}\]
第二行是贝叶斯法则,第三行分母和x 无关所以去掉。
第一项即观测方程,有:\[p\left( {y|x} \right) = \prod\limits_{k = 0}^K {p\left( {{y_k}|{x_k}} \right)} \] ,很简单。
第二项即运动方程,有:\[p\left( {x|v} \right) = \prod\limits_{k = 0}^K {p\left( {{x_k}|{x_{k - 1}},v_k} \right)} \] ,也很简单。
现在的问题是如何求解这个最大化问题。对于高斯分布,最大化问题可以变成最小化它的负对数。当我对一个高斯分布取负对数时,它的指数项变成了一个二次项,而前面的因子则变为一个无关的常数项,可以略掉(这部分我不敲了,有疑问的同学可以问)。于是,定义以下形式的最小化函数:
\[\begin{array}{l} {J_{y,k}}\left( x \right) = \frac{1}{2}{\left( {{y_k} - {C_k}{x_k}} \right)^T}R_k^{ - 1}\left( {{y_k} - {C_k}{x_k}} \right)\\ {J_{v,k}}\left( x \right) = \frac{1}{2}{\left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} \right)^T}Q_k^{ - 1}\left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} \right) \end{array}\]
那么最大后验估计就等价于:
\[\hat x = \arg \min \sum\limits_{k = 0}^K {{J_{y,k}} + {J_{v,k}}} \]
这个问题现在是二次项和的形式,写成矩阵形式会更加清晰。定义:
\[\begin{array}{l} z = \left[ \begin{array}{l} {x_0}\\ {v_1}\\ \vdots \\ {v_K}\\ {y_0}\\ \vdots \\ {y_K} \end{array} \right],x = \left[ \begin{array}{l} {x_0}\\ \vdots \\ {x_K} \end{array} \right]\\ H = \left[ {\begin{array}{*{20}{c}} 1&{}&{}&{}\\ { - {A_0}}&1&{}&{}\\ {}& \ddots & \ddots &{}\\ {}&{}&{ - {A_{K - 1}}}&1\\ {{C_0}}&{}&{}&{}\\ {}& \ddots &{}&{}\\ {}&{}& \ddots &{}\\ {}&{}&{}&{{C_K}} \end{array}} \right],W = \left[ {\begin{array}{*{20}{c}} {{P_0}}&{}&{}&{}&{}&{}&{}\\ {}&{{Q_1}}&{}&{}&{}&{}&{}\\ {}&{}& \ddots &{}&{}&{}&{}\\ {}&{}&{}&{{Q_K}}&{}&{}&{}\\ {}&{}&{}&{}&{{R_1}}&{}&{}\\ {}&{}&{}&{}&{}& \ddots &{}\\ {}&{}&{}&{}&{}&{}&{{R_K}} \end{array}} \right] \end{array}\]
就得到矩阵形式的,类似最小二乘的问题:
\[J\left( x \right) = \frac{1}{2}{\left( {z - Hx} \right)^T}{W^{ - 1}}\left( {z - Hx} \right)\]
于是令它的导数为零,得到:
\[\frac{{\partial J}}{{\partial {x^T}}} = - {H^T}{W^{ - 1}}\left( {z - Hx} \right) = 0 \Rightarrow \left( {{H^T}{W^{ - 1}}H} \right)x = {H^T}{W^{ - 1}}z\] (*)
读者会问,这个问题和卡尔曼滤波有什么问题呢?事实上, 卡尔曼滤波就是递推地求解(*)式的过程 。所谓递推,就是只用x_{k-1} 来计算x_k 。对(*)进行Cholesky分解,就可以推出卡尔曼滤波器。详细过程限于篇幅就不推了,把卡尔曼的结论写一下:
\[\begin{array}{l} {{\tilde P}_k} = {A_{k - 1}}{{\hat P}_{k - 1}}A_{k - 1}^T + {Q_k}\\ {{\tilde x}_k} = {A_{k - 1}}{{\hat x}_{k - 1}} + {v_k}\\ {K_k} = {{\tilde P}_k}C_k^T{\left( {{C_k}{{\tilde P}_k}C_k^T + {R_k}} \right)^{ - 1}}\\ {{\hat P}_k} = \left( {I - {K_k}{C_k}} \right){{\tilde P}_k}\\ {{\hat x}_k} = {{\tilde x}_k} + {K_k}\left( {{y_k} - {C_k}{{\tilde x}_k}} \right) \end{array}\]
前两个是预测,第三个是卡尔曼增益,四五是校正。
另一方面,能否直接求解(*)式,得到\hat{x} 呢?答案是可以的,而且这就是优化方法(batch optimization)的思路:将所有的状态放在一个向量里,进行求解。与卡尔曼滤波不同的是,在估计前面时刻的状态(如x_1 )时,会用到后面时刻的信息(y_2,y_3 等)。从这点来说,优化方法和卡尔曼处理信息的方式是相当不同的。
================== 扩展卡尔曼滤波器 ===================
线性高斯系统当然性质很好啦,但许多现实世界中的系统都不是线性的,状态和噪声也不是高斯分布的。例如上面举的激光观测方程就不是线性的。当系统为非线性的时候,会发生什么呢?
一件悲剧的事情是: 高斯分布经过非线性变换后,不再是高斯分布。而且,是个什么分布,基本说不上来 。(摊手)
如果没有高斯分布,上面说的那些都不再成立了。 于是EKF说,嘛,我们睁一只眼闭一只眼,用高斯分布去近似它,并且,在工作点附近对系统进行线性化 。当然这个近似是很成问题的,有什么问题我们之后再说。
EKF的做法主要有两点。其一,在工作点附近\[{{\hat x}_{k - 1}},{{\tilde x}_k}\] ,对系统进行线性近似化:
\[\begin{array}{l} f\left( {{x_{k - 1}},{v_k},{w_k}} \right) \approx f\left( {{{\hat x}_{k - 1}},{v_k},0} \right) + \frac{{\partial f}}{{\partial {x_{k - 1}}}}\left( {{x_{k - 1}} - {{\hat x}_{k - 1}}} \right) + \frac{{\partial f}}{{\partial {w_k}}}{w_k}\\ g\left( {{x_k},{n_k}} \right) \approx g\left( {{{\tilde x}_k},0} \right) + \frac{{\partial g}}{{\partial {x_k}}}{n_k} \end{array}\]
这里的几个偏导数,都在工作点处取值。于是呢,它就被 活生生地当成了一个线性系统 。
第二,在线性系统近似下,把噪声项和状态都 当成了高斯分布 。这样,只要估计它们的均值和协方差矩阵,就可以描述状态了。经过这样的近似之后呢,后续工作都和卡尔曼滤波是一样的了。所以EKF是卡尔曼滤波在NLNG系统下的直接扩展(所以叫扩展卡尔曼嘛)。EKF给出的公式和卡尔曼是一致的,用线性化之后的矩阵去代替卡尔曼滤波器里的转移矩阵和观测矩阵即可。
\[\begin{array}{l} {{\tilde P}_k} = {F_{k - 1}}{{\hat P}_{k - 1}}F_{k - 1}^T + Q_k'\\ {{\tilde x}_k} = f\left( {{{\hat x}_{k - 1}},{v_k},0} \right)\\ {K_k} = {{\tilde P}_k}G_k^T{\left( {{G_k}{{\tilde P}_k}G_k^T + R_k'} \right)^{ - 1}}\\ {{\hat P}_k} = \left( {I - {K_k}{G_k}} \right){{\tilde P}_k}\\ {{\hat x}_k} = {{\tilde x}_k} + {K_k}\left( {{y_k} - g({{\tilde x}_k},0)} \right) \end{array}\] 其中\[F_{k-1} = {\left. {\frac{{\partial f}}{{\partial {x_{k - 1}}}}} \right|_{{{\hat x}_{k - 1}}}},{G_k} = {\left. {\frac{{\partial g}}{{\partial {x_k}}}} \right|_{{{\tilde x}_k}}}\]
这样做听起来还是挺有道理的,实际上也是能用的,但是问题还是很多的。
考虑一个服从高斯分布的变量x \sim N(0,1) ,现在y=x^2 ,问y 服从什么分布?
我概率比较差,不过这个似乎是叫做卡尔方布。y 应该是下图中k=1那条线。