Algorithm 1 Observation likelihood.

Input: $\mathit{x}$, decim, ${\left\{{\mathbf{p}}_l^i\right\}}_{i=1}^{i=N}$, $\mathbf{map}$, $\sigma$, $d_{max}$

Output: loglik 1:begin 2:loglik ← 0 3:$d^2$← 0 4:foreach i in 1:decim:N 5:${\mathbf{p}}_g^i\leftarrow \mathit{x}\oplus {\mathbf{p}}_l^i$ 6:${\mathbf{p}}_{mapclosest}^i$← map.kdtree.query(${\mathbf{p}}_g^i$) 7:$d^2$←$d^2+\min \{d_{max}^2,||{\mathbf{p}}_g^i-{\mathbf{p}}_{mapclosest}^i{||}^2\}$ 8:end 9:loglik ←$-d^2/{\sigma }^2$ 10:return loglik 11:end