上接测量噪声$\quad$包含噪声的测量仪测量模型
$\qquad$建立含噪声的测量模型后，需要选择传感器模型参数，参数包含$z_{hit}、z_{short}、z_{max}、z_{rand}、\sigma_{hit}$zhit​、zshort​、zmax​、zrand​、σhit​和$\lambda_{short}$λshort​，所有的内部参数标记为$\boldsymbol{\Theta}$Θ，传感器测量的似然就是$\boldsymbol{\Theta}$Θ的函数。
$\qquad$从实际数据中获得这些参数，通过考虑参考数据集$Z=\{z_{i}\}$Z={zi​}(联合位置$X=\{x_{i}\}$X={xi​}和地图$m$m)的似然最大化。似然由下式给出
$p(Z|X,m,\boldsymbol{\Theta})$p(Z∣X,m,Θ)$\qquad$目标是确定使这个似然最大的固有参数$\boldsymbol{\Theta}$Θ。使用数据似然最大化的估计或算法称为极大似然估计或$ML$ML估计。为了推导极大似然估计，引入辅助变量$c_{i}$ci​，即一致性变量。$c_{i}$ci​可能取四个值，对应着产生测量$z_{i}$zi​的四个可能途径。
$\qquad$基于$c_{i}$ci​的值，可以将$Z$Z分解为四个不相交的子集$Z_{hit}、Z_{short}、Z_{max}$Zhit​、Zshort​、Zmax​和$Z_{rand}$Zrand​。对于参数$z_{hit}、z_{short}、z_{max}、z_{rand}$zhit​、zshort​、zmax​、zrand​的极大似然估计是简单的归一化系数。
$\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}$⎝⎜⎜⎛​zhit​zshort​zmax​zrand​​⎠⎟⎟⎞​=∣Z∣−1⎝⎜⎜⎛​∣Zhit​∣∣Zshort​∣∣Zmax​∣∣Zrand​∣​⎠⎟⎟⎞​$\qquad$$|Z_{*}|$∣Z∗​∣表示所有测量点$z_{i}$zi​在每次测量中的权值(对应四种途径)和。获取固有参数$\sigma_{hit}$σhit​
$p(Z_{hit}|X,m,\boldsymbol{\Theta})= \prod_{z_{i}\in Z_{hit}}p_{hit}(z_{i}|x_{i},m,\boldsymbol{\Theta})\\ = \prod_{z_{i}\in Z_{hit}}\frac{1}{\sqrt{2\pi\sigma_{hit}^{2}}}e^{-\frac{1}{2}\frac{(z_{i}-z_{i}^{*})^{2}}{\sigma_{hit}^{2}}}$p(Zhit​∣X,m,Θ)=zi​∈Zhit​∏​phit​(zi​∣xi​,m,Θ)=zi​∈Zhit​∏​2πσhit2​​1​e−21​σhit2​(zi​−zi∗​)2​$\qquad$取对数(连乘变为连加)，求导，由极值的必要条件，对数函数单调递增性质得极大似然估计解：
$\sigma_{hit}=\sqrt{\frac{1}{|Z_{hit}|}\sum_{z_{i}\in Z_{hit}}(z_{i}-z_{i}^{*})^{2}}$σhit​=∣Zhit​∣1​zi​∈Zhit​∑​(zi​−zi∗​)2​$\qquad$求解参数$\lambda_{short}$λshort​使用相同的方法，求解结果为：
$\lambda_{short}=\frac{|Z_{short}|}{\sum_{z_{i}\in Z_{short}}z_{i}}$λshort​=∑zi​∈Zshort​​zi​∣Zshort​∣​$\qquad$上述推导为假设参数$c_{i}$ci​已知，现在延伸到$c_{i}$ci​未知，使用$EM$EM算法求解，第一步计算$c_{i}$ci​的期望值，第二步计算该期望值下的固有模型参数。将前文极大似然估计中：
$log\ p(Z|X,m,\boldsymbol{\Theta})=\sum_{z_{i}\in Z}log\ p(z_{i}|x_{i},m)\\ = \sum_{z_{i}\in Z_{hit}}log\ p_{hit}(z_{i}|x_{i},m) + \sum_{z_{i}\in Z_{short}}log\ p_{short}(z_{i}|x_{i},m) + \sum_{z_{i}\in Z_{max}}log\ p_{max}(z_{i}|x_{i},m) + \sum_{z_{i}\in Z_{rand}}log\ p_{rand}(z_{i}|x_{i},m)$log p(Z∣X,m,Θ)=zi​∈Z∑​log p(zi​∣xi​,m)=zi​∈Zhit​∑​log phit​(zi​∣xi​,m)+zi​∈Zshort​∑​log pshort​(zi​∣xi​,m)+zi​∈Zmax​∑​log pmax​(zi​∣xi​,m)+zi​∈Zrand​∑​log prand​(zi​∣xi​,m)$\qquad$改写为
$log\ p(Z|X,m,\boldsymbol{\Theta}) = \sum_{z_{i}\in 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)$log p(Z∣X,m,Θ)=zi​∈Z∑​I(ci​=hit)log phit​(zi​∣xi​,m)+I(ci​=short)log pshort​(zi​∣xi​,m)+I(ci​=max)log pmax​(zi​∣xi​,m)+I(ci​=rand)log prand​(zi​∣xi​,m)$\qquad$$\color{#F00}{z_{i}的取值范围变化！！！}$zi​的取值范围变化！！！求解期望值最大得：
$E(log\ p(Z|X,m,\boldsymbol{\Theta}))=\sum_{z_{i}\in Z}e_{i,hit}log\ p_{hit}(z_{i}|x_{i},m)+e_{i,short}log\ p_{short}(z_{i}|x_{i},m)+e_{i,short}log\ p_{max}(z_{i}|x_{i},m)+e_{i,rand}log\ p_{rand}(z_{i}|x_{i},m)$E(log p(Z∣X,m,Θ))=zi​∈Z∑​ei,hit​log phit​(zi​∣xi​,m)+ei,short​log pshort​(zi​∣xi​,m)+ei,short​log pmax​(zi​∣xi​,m)+ei,rand​log prand​(zi​∣xi​,m)$\qquad$使用$EM$EM算法实现最大化，固定变量$\sigma_{hit}$σhit​和$\lambda_{short}$λshort​，计算变量$c_{i}$ci​的期望。
$\begin{pmatrix} e_{i,hit}\\ e_{i,short}\\ e_{i,max}\\ e_{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}$⎝⎜⎜⎛​ei,hit​ei,short​ei,max​ei,rand​​⎠⎟⎟⎞​=η⎝⎜⎜⎛​phit​(zi​∣xi​,m)pshort​(zi​∣xi​,m)pmax​(zi​∣xi​,m)prand​(zi​∣xi​,m)​⎠⎟⎟⎞​其中$\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}$η=[phit​(zi​∣xi​,m)+pshort​(zi​∣xi​,m)+pmax​(zi​∣xi​,m)+prand​(zi​∣xi​,m)]−1
$\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}$⎝⎜⎜⎛​zhit​zshort​zmax​zrand​​⎠⎟⎟⎞​=∣Z∣−1i∑​⎝⎜⎜⎛​ei,hit​ei,short​ei,max​ei,rand​​⎠⎟⎟⎞​
$\sigma_{hit}=\sqrt{\frac{1}{\sum_{z_{i}\in Z}e_{i,hit}}\sum_{z_{i}\in Z}e_{i,hit}(z_{i}-z_{i}^{*})^{2}}\\ \lambda_{short} = \frac{\sum_{z_{i}\in Z}e_{i,short}}{\sum_{z_{i}\in Z}e_{i,short}z_{i}}$σhit​=∑zi​∈Z​ei,hit​1​zi​∈Z∑​ei,hit​(zi​−zi∗​)2​λshort​=∑zi​∈Z​ei,short​zi​∑zi​∈Z​ei,short​​$\color{#F00}{给每个测量值z_{i}按照概率值在\eta(归一化系数)中的比例加权}$给每个测量值zi​按照概率值在η(归一化系数)中的比例加权。