观测方程
参考前面的博客中关于贝叶斯滤波、卡尔曼滤波的讨论，在运动学模型中我们建立了机器人的运动学模型，运动学模型求解 $p(\boldsymbol{x}_t|\boldsymbol{z}_{1:t},\boldsymbol{u}_{1:t})$ ，而感知模型求解的是观测方程：$p(\boldsymbol{z}_{t}|\boldsymbol{x}_{t})$ 在有地图信息时，上述两个方程应表述为 $p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1},\boldsymbol{m})$ 和 $p(\boldsymbol{z}_{t}|\boldsymbol{x}_{t},\boldsymbol{m})$ 。为书写方便起见，下面不再用粗体来表示这些矢量，相信这不会引起理解的困难。
$x_t$ 是机器人位姿， $z_t$ 是 $t$ 时刻的测量，$m$ 是环境地图，概率机器人要将传感器测量的过程建模为一个条件概率密度 $p(z_t|x_t)$ ，这种概率密度包含了由随机噪声引起的不确定性，而不是经典机器人模型中的一个确定的函数 $z_t = f(x_t)$ ，概率机器人技术相对于经典机器人技术的主要优势在于，在实际应用中可以使用极其粗糙的模型。
许多传感器通常会产生不止一个值，例如相机会生成包含亮度、饱和度、颜色等不同的信息。用 $K$ 表示在一个测量 $z_t$ 内的测量值的数目，有：
$$ \begin{align}
z_t = \{ z_t^1,\cdots,z_t^K \}
\end{align} $$
用 $z_t^K$ 表示一个独立的测量。假设每个独立测量束噪声之间是独立的：
$$ \begin{align}
p(z_t | x_t ,m ) = \prod_{k=1}^K p(z_t^k|x_t,m)
\end{align} $$
该独立性假设仅在理想情况下是正确的，事实上传感器之间往往并不独立，尤其是相邻的同类型传感器之间。
关于地图
前面机器人运动模型中我们已经初步讨论了地图，现在正式定义：一个地图就是环境中物体列表及其属性：
$$ \begin{align}
m = \{ m_1,m_2,\cdots ,m_N \}
\end{align} $$
其中 $m$ 是环境中物体的总数，每个 $m_n$ 指定一个属性。
地图通常有基于特征（feature-based）的地图和基于位置（location-based）的地图两种。在基于特征的地图中， $n$ 是特征的索引，$m_n$ 包括特征的属性及特征的位置。在基于位置的地图中，索引号 $n$ 与特定的位置对应。在平面地图中，通常用 $m_{x,y}$ 来表示一个地图元素， $(x,y)$ 是坐标。
测距仪的波束模型
假设有一个测距传感器，能测量到附近物体的距离，（激光测距仪沿着一个波束测量距离，超声波传感器在一个测量锥内测量距离）。
四类测量噪声：
小的测量噪声、意外对象引起的误差，以及由于未检测到对象引起的误差和随机意外噪声。
small measurement noise, errors due to unexpected objects, errors due to failures to detect objects, and random unexplained noise.
因此期望模型 $p(z_t | x_t ,m )$ 是四个密度的混合。
- Correct range with local measurement noise
使用 $z_t^{k \ast}$ 表示由 $z_t^k$ 测量的对象的真实距离，这部分测量噪声(measurement noise) 通常由一个均值为 $z_t^{k \ast}$ 、标准偏差为 $\sigma _{hit}$ 的高斯模型来建模，用 $p_{hit}$ 表示，如下图a所示
四种误差分布形式
测距传感器测量的值会有上下限，在区间 $[0;z_{max}]$ 范围内， $z_{max}$ 表示传感器测量的最大距离，有：
$$ \begin{align}
p_{hit} (z_t^k | x_t,m) &= \begin{cases} \eta \Nu(z_t^k;z_t^{k \ast} ,\sigma _{hit}^2) &\text{if } 0 \leq z_t^k < z_{max} \\ 0 &\text{otherwise} \end{cases} \\
\Nu(z_t^k;z_t^{k \ast} ,\sigma _{hit}^2) &= \frac{1}{\sqrt{2 \pi \sigma _{hit}^2}} \exp \{ -\frac{1}{2} \frac{(z_t^k -z_t^{k \ast})^2} {\sigma _{hit}^2} \} \\
\eta &= (\int _0 ^{zmax} \Nu(z_t^k;z_t^{k \ast} ,\sigma _{hit}^2) d z_t^k )^{-1}
\end{align} $$
标准偏差 $p_{hit}$ 是测量模型的一个固有的噪声参数。
- Unexpected objects
假设移动机器人的环境中突然出现了意外的对象（不在地图中），处理这类对象的一种方法是将它们作为状态向量的一部分来对待并估计它们的位置，另一种更简单的方法是将它们作为传感器噪声来处理。
检测到意外对象的可能性随着距离而减少。即：意外的对象更有可能在近处被检测到。它们会导致传感器测量的距离更短
使用指数分布来描述这种概率，其中参数 $\lambda _{short}$ 是是测量模型的固有参数：
$$ \begin{align}
p_{short} (z_t^k | x_t,m) &=\begin{cases} \eta \lambda _{short} \exp(-\lambda_{short} z_t^k ) &\text{if } 0 \leq z_t^k \leq z_t^{k \ast} \\ 0 &\text{otherwise} \end{cases} \\
\int _0^{z_t^{k \ast}} \lambda _{short} \exp(-\lambda_{short} z_t^k ) d z_t^k &= - \exp(-\lambda_{short} z_t^{k \ast}) + \exp(-\lambda_{short} 0) \notag \\
& =1-\exp(-\lambda_{short} z_t^{k \ast}) \\
\eta &=\frac{1}{1-\exp(-\lambda_{short} z_t^{k \ast})}
\end{align} $$
- Failures. Sometimes, obstacles are missed altogether
传感器检测失败(sensor failure) 的典型结果是最大距离测量(max-range measurement) 问题：传感器返回它的最大允许值 $z_{max}$ 。这样的事情是经常发生的，我们采用一个以 $z_{max}$ 为中心的点群分布来建模：
$$ \begin{align}
p_{max} (z_t^k | x_t,m) = I (z=z_{max}) = \begin{cases} 1 &z=z_{max} \\ 0 & \text{otherwise} \end{cases}
\end{align} $$
上面的 $I$ 是一个指示函数，当其参数为真时取值为1，否则取值为0。理论上来说，此时 $p_{max}$ 是离散的，不是一个概率密度函数了，不过这对我们后面的推导没有影响。
4. Random measurements.
测距仪偶尔会产生完全无法解释的测量，。例如，当超声波被几面墙反弹或者它们受到不同传感器之间的串扰时，声呐常产生幻读。为了使之简单化，对于这样的测量，这里将使用一个分布在完整传感器测量范围 $[0;z_{max}]$ 的均匀分布来建立模型。
$$ \begin{align}
p_{rand} (z_t^k | x_t,m)= \begin{cases} \frac{1}{z_{max}} &0 \leq z_t^k < z_{max} \\ 0 & \text{otherwise} \end{cases}
\end{align} $$
现在用四个参数来使上面的四种分布加权平均混合，且四个参数之和 $z_{hit} + z_{short} + z_{max} + z_{rand} =1$ 有：
$$ \begin{align}
p(z_t^k | x_t,m) = \begin{pmatrix}
z_{hit}\\ z_{short}\\ z_{max}\\ z_{rand} \end{pmatrix} \cdot \begin{pmatrix}
p_{hit} (z_t^k | x_t,m) \\ p_{short} (z_t^k | x_t,m)\\ p_{max} (z_t^k | x_t,m) \\ p_{rand} (z_t^k | x_t,m) \end{pmatrix}
\end{align} $$
四种密度分布混合后效果如下图：
mixture distribution下图是一个测距仪模型观测方程的算法实现，其中第四行采用射线投射来为特定的传感器测量计算无噪声距离。
measurements调节模型的固有参数
模型的固有参数包括四个混合因子： $z_{hit}$ 、 $z_{short}$ 、 $z_{max}$ 、 $z_{rand}$ 和参数 $\sigma _{hit}$ 、 $\lambda _{short}$ ，这些都是传感器内部参数，把他们共同记作 $\Theta$ 。传感器的似然就是 $\Theta$ 的函数。
可以根据工程经验指定固有参数，也可以通过参考数据集的似然最大化函数计算。数据 $Z$ 的似然为：
$$ \begin{align}
p(Z|X,m,\Theta)
\end{align} $$
任何使数据似然最大化的估计或算法称为极大似然(Maximum Likelihood, ML) 估计或者简称ML 估计(ML estimator) 。这曾经在前面关于贝叶斯准则的博客中提过。
下面的learn_intrinsic_parameters算法是极大似然估计的一种程序实现
从数据中确定基于波束的传感器模型固有参数的算法极大似然估计的数学推导
引入辅助变量 $c_i$ ，它被称为 correspondence variable，一致性变量，它可以取hit, short, max, and random 四个值，分布对应可以产生 $z_i$ 的四个可能的途径。
首先考虑 $c$ 为已知的情况，基于 $c_i$ 可以将Z分解为四个不相交的集合：$Z_{hit}$ , $Z_{short}$ ,$Z_{max}$,$Z_{rand}$ 它们共同组成了 $Z$ ，计算固有参数 $z_{hit}$ 、 $z_{short}$ 、 $z_{max}$ 、 $z_{rand}$ 的极大似然估计就是简单的归一化系数：
$$ \begin{align}
\begin{pmatrix}
z_{hit}\\ z_{short}\\ z_{max}\\ z_{rand} \end{pmatrix} = |Z|^{-1} \begin{pmatrix}
|Z_{hit}|\\ | Z_{short}|\\ |Z_{max}| \\ |Z_{rand}| \end{pmatrix}
\end{align} $$
剩下的两个固有参数 $\sigma _{hit}$ 、 $\lambda _{short}$ 由（5）式，有：
$$ \begin{align}
p(Z_{hit} | X,m,\Theta) &= \prod_{z_i \isin Z_{hit}} p_{hit}(z_i|x_i,m,\Theta) \notag \\
&= \prod_{z_i \isin Z_{hit}} \frac{1}{\sqrt{2 \pi \sigma _{hit}^2}} \exp \{ -\frac{1}{2} \frac{(z_i -z_i^{ \ast})^2} {\sigma _{hit}^2} \}
\end{align} $$
前面的贝叶斯准则的博客中提过，在计算最大似然估计时，常常使用对数最大化，上式的对数似然为：
$$ \begin{align}
\log p(Z_{hit} | X,m,\Theta) &= \sum_{z_i \isin Z_{hit} }[-\frac{1}{2} \log 2 \pi \sigma _{hit}^2 -\frac{1}{2} \frac{(z_i -z_i^{ \ast})^2} {\sigma _{hit}^2} ] \\
&= -\frac{1}{2} \sum_{z_i \isin Z_{hit} }[ \log 2 \pi \sigma _{hit}^2 + \frac{(z_i -z_i^{ \ast})^2} {\sigma _{hit}^2} ] \notag \\
&= -\frac{1}{2} \Big( |Z_{hit}| \log 2 \pi + 2 |Z_{hit}| \log \sigma _{hit} + \sum_{z_i \isin Z_{hit} } \frac{(z_i -z_i^{ \ast})^2} {\sigma _{hit}^2} \Big) \\
\frac{\partial \log p(Z_{hit} | X,m,\Theta)}{\partial \sigma _{hit} } &= -\frac{|Z_{hit}|}{\sigma _{hit}} + \frac{1}{\sigma _{hit}^3} \sum_{z_i \isin Z_{hit} }(z_i -z_i^{ \ast})^2
\end{align} $$
令（18）式中一阶导为0，有：
$$ \begin{align}
\sigma _{hit} = \sqrt{\frac{1}{|Z_{hit}|} \sum_{z_i \isin Z_{hit} }(z_i -z_i^{ \ast})^2 }
\end{align} $$
同样的，剩下的一个参数 $\lambda _{short}$ 可以由完全相同的办法计算得到：
$$ \begin{align}
p(Z_{short} | X,m,\Theta) &= \prod_{z_i \isin Z_{short}} p_{short}(z_i|x_i,m) \notag \\
&= \prod_{z_i \isin Z_{short}} \lambda _{short} \exp (-\lambda_{short} z_i) \\
\log p(Z_{short} | X,m,\Theta) &= \sum_{z_i \isin Z_{short}} \log \lambda _{short} - \lambda _{short} z_i \notag \\
&= |Z_{short}| \log \lambda _{short} - \lambda _{short} \sum_{z_i \isin Z_{short}} z_i \\
\frac{\partial \log p(Z_{short} | X,m,\Theta)}{\partial \lambda _{short}} &= \frac{|Z_{short}|}{\lambda _{short}} -\sum_{z_i \isin Z_{short} }z_i \\
\lambda _{short} &= \frac{|Z_{short}|}{ \sum_{z_i \isin Z_{short} }z_i}
\end{align} $$
好了，现在推广到 $c_i$ 未知的情况。
首先定义数据Z的似然函数：
$$ \begin{align} \begin{split}
\log p(Z|X,m,\Theta) &= \sum_{z_i \isin Z} \log p(z_i|x_i,m,\Theta) \\
&= \sum_{z_i \isin Z_{hit} } \log p_{hit} (z_i|x_i,m,\Theta) +\sum_{z_i \isin Z_{short} } \log p_{short} (z_i|x_i,m,\Theta) \\
&+\sum_{z_i \isin Z_{max} } \log p_{max} (z_i|x_i,m,\Theta) + \sum_{z_i \isin Z_{rand} } \log p_{rand} (z_i|x_i,m,\Theta)
\end{split} \end{align} $$
使用 $c_i$ 重写为：
$$ \begin{align} \begin{split}
\log p(Z|X,m,\Theta) &= \sum_{z_i \isin Z } I(c_i = hit) \log p_{hit} (z_i|x_i,m) \\
&+ I(c_i = short) \log p_{short} (z_i|x_i,m) \\
&+ I(c_i = max) \log p_{max} (z_i|x_i,m) \\
&+ I(c_i = rand) \log p_{rand} (z_i|x_i,m)
\end{split} \end{align} $$
其中 $I$ 是指标函数，indicator function，对上式取期望，并将期望最大化：
$$ \begin{align} \begin{split}
E[\log p(Z|X,m,\Theta)] &= \sum _i p(c_i =hit) \log p_{hit} (z_i|x_i,m) \\
&+ p(c_i = short) \log p_{short} (z_i|x_i,m) \\
&+ p(c_i = max) \log p_{max} (z_i|x_i,m) \\
&+ p(c_i = rand) \log p_{rand} (z_i|x_i,m) \\
&=: \sum_i e_{i,hit} \log p_{hit} (z_i|x_i,m) + e_{i,short} \log p_{short} (z_i|x_i,m) \\
& + e_{i,max} \log p_{max} (z_i|x_i,m) + e_{i,rand} \log p_{rand} (z_i|x_i,m)
\end{split} \end{align} $$
上式第一步认为 $\sigma _{hit}$ 、 $\lambda _{short}$ 是给定的，关于变量 $c_i$ 计算期望。
$$ \begin{align}
\begin{pmatrix} e_{i,hit}\\ e_{i,short}\\ e_{i,max}\\ e_{i,rand} \end{pmatrix} := \begin{pmatrix} p(c_i =hit) \\ p(c_i =short) \\ p(c_i =max) \\ p(c_i =rand) \end{pmatrix} = \eta \begin{pmatrix} p_{hit} (z_i|x_i,m) \\ p_{short} (z_i|x_i,m) \\ p_{max} (z_i|x_i,m) \\ p_{rand} (z_i|x_i,m) \end{pmatrix} \\
\eta = [p_{hit} (z_i|x_i,m) +p_{short} (z_i|x_i,m)+ p_{max} (z_i|x_i,m) + p_{rand} (z_i|x_i,m)]^{-1}
\end{align} $$
期望将传感器不同组件之间的依赖关系进行了解耦。
极大似然混合参数是简单的归一化期望：
$$ \begin{align}
\begin{pmatrix} z_{hit}\\ z_{short}\\ z_{max}\\ z_{rand} \end{pmatrix} = |Z|^{-1} \sum_i \begin{pmatrix} e_{i,hit}\\ e_{i,short}\\ e_{i,max}\\ e_{i,rand} \end{pmatrix}
\end{align} $$
最后根据（19）、（23）式，类似的得到：
$$ \begin{align}
\sigma _{hit} &= \sqrt{\frac{1}{\sum_{z_i \isin Z } e_{i,hit}} \sum_{z_i \isin Z_{hit} } e_{i,hit} (z_i -z_i^{ \ast})^2 } \\
\lambda _{short} &= \frac{ \sum_{z_i \isin Z} e_{i,short} }{ \sum _{z_i \isin Z_{short} } e_{i,short} z_i}
\end{align} $$
测距仪的似然域模型
似然域(likelihood field)，它能得到更光滑的后验分布，计算也更高效。
用 $(x_{k,sens} \enspace y_{k,sens})^T$ 表示于机器人固连的传感器局部坐标系统的位置，用 $\theta_{k,sens}$ 表示传感器波束相对于机器人航向的角度，测量 $x_t^k$ 的终点可以通过三角变换，映射到全局坐标系中：
$$ \begin{align}
\begin{pmatrix} x_{z_t^k}\\ y_{z_t^k}\end{pmatrix} =\begin{pmatrix} x\\ y \end{pmatrix} + \begin{pmatrix} \cos \theta & -\sin\theta \\ \sin \theta & \cos \theta \end{pmatrix}\begin{pmatrix} x_{k,sens}\\ y_{k,sens}\end {pmatrix} + z_t^k \begin{pmatrix} \cos (\theta + \theta_{k,sens}) \\ \sin (\theta + \theta_{k,sens}) \end{pmatrix}
\end{align} $$
这些坐标只有当传感器检测到一个障碍物时才是有意义的，如果压根没有检测到障碍物，传感器输出的是最大值，那么似然域模型简单地将最大距离读数丢弃。
- 测量噪声 Measurement noise.
令 dist 表示测量坐标 $\begin{pmatrix} x_{z_t^k}\ y_{z_t^k}\end{pmatrix}$ 与地图 $m$ 上最近物体之间的欧氏距离，那么：
$$ \begin{align}
p_{hit} (z_t^k |x_t ,m) = \varepsilon_{\sigma _{hit}} (dist)
\end{align} $$
-
Failures.
As before, we assume that max-range readings have a distinct large likelihood. As before, this is modeled by a point-mass distribution $p_{max}$.
-
Unexplained random measurements
Finally, a uniform distribution $p_{rand}$ is used to model random noise in perception.
算法如下：
likelihood_field_range_finder_model基于特征的测量模型
大多数特征提取器是从高维的传感器测最中提取较少数量的特征。这种方法的主要优点是大大减少了计算的复杂性：高维测量空间的推断可能是昂贵的，而低维特征空间的推断却可以将计算效率提高几个数量级。
特定的特征提取算法暂不讨论，这是计算机视觉领域（Computer vision, CV）的内容。
处理地标最通用的模型认为，传感器能测最地标相对于机器人局部坐标系的距离和方位。这样的传感器叫作距离和方位传感器(range and bearing sensors) 。另外，特征提取器可以生成一个签名(signature)。用 $r$ 表示距离， $\Phi$ 表示方位， $s$ 表示签名，则特征向量为：
$$ \begin{align}
f(z_t) = \{ f_t^1 ,f_t^1 , \cdots \} = \Big( \begin{pmatrix} r_t^1 \\ \Phi_t^1 \\ s_t^1 \end{pmatrix} , \begin{pmatrix} r_t^2 \\ \Phi_t^2 \\ s_t^2 \end{pmatrix} , \cdots \Big)
\end{align} $$
假定特征之间条件独立：
$$ \begin{align}
p(f(z_t) | x_t , m) = \prod_{i} p(r_t^i,\Phi_t^i,s_t^i|x_t,m)
\end{align} $$
地标测量模型仅适用于基于特征的地图，每组特征都具有一个签名和一个位置坐标，无噪声地标传感器的测量向量可以容易地由标准的几何定律描述。用相互独立的距离高斯、方位高斯和签名高斯噪声来为地标感知中的噪声建立模型。由此而得的测最模型对时刻t的第t个特征与地图中第j个地标相关的情况进行了确切的阐述。
$$ \begin{align}
\begin{pmatrix} r_t^i \\ \Phi_t^i \\ s_t^i \end{pmatrix} = \begin{pmatrix} \sqrt{(m_{j,x} - x)^2 + (m_{j,y} - y)^2} \\ atan2(m_{j,y} - y,m_{j,x} - x) - \theta \\ s_j \end{pmatrix} + \begin{pmatrix} \varepsilon_{\sigma_r^2} \\ \varepsilon_{\sigma_{\Phi}^2} \\ \varepsilon_{\sigma_s^2} \end{pmatrix}
\end{align} $$
已知相关性的传感器模型
距离／方位传感器的一个关键问题就是数据关联问题(data ssociation problem)。当地标不能唯一确定时，这个问题就会出现，因此对于地标的身份存在一些残留的不确定性。
用 $c_t^i$ 表示特征 $f_t^i$ 与地图地标 $m_j$ 的相关变量，$c_i \isin { 1,\cdots ,N+1 }$ ，N是地图 $m$ 中的地标数量.
Algorithm landmark_model_known_correspondence