SLAM数据关联

马氏距离与数据关联

在SLAM的数据关联中，马氏距离是一种常见的方法。本文使用简单的数据进行说明，同时对必要的公式进行推导。本文使用的数据来源于[1]. 。
在这里我们重点使用two dimension的数据进行示例。以x1为例，我们并没有假设x1是否符合正态分布，我们可以求得x1的均值和方差u1,σ2.

u1σ2=1N∑i=1Nxi1=1N−1∑i=1N(xi1−u1)2=1N−1∑i=1N(centeredxi1)2

求得u1=6,σ21=4.9211
相似得，我们可以求得u2,σ2,另外，我们可以求得x1,x2的协方差，定义如下。

Pσ12=[σ21σ21σ12σ22]=σ21=1N−1∑i=1N(cxi1)(cxi2)

这一部分的例子请运行example1.m;\
将这些点画出来，同时将不确定性显示出来。这里，每个点的不确定性都是一样的。可以看出，数据在x,y两个维度上具有高度的相关性。

下面探讨一些dataassociation中的一些问题，以Cyrill Stachniss的robotics2 data association note为线索。data association的定义： \textbf{数据关联是将noisy measurement和已知的数据进行关联的过程} ，英文原话是： Data association is the process of associating uncertain measurements to known tracks。 这里当前的measurement是有uncertainty的，已有的track（或者说数据）也应该是有噪声（uncertainty的）。主要处理方法有两类，1: Bayesian方法，根据prior，posterior，observation，计算出整个的分布。2： 非Bayesian方法，直接相信数据集，计算一个最大似然估计（我们使用的最小二乘解图优化问题都是非贝叶斯方法。\
一般使用Mahalanobis distance来衡量两个measurement的距离，在我们的数据中，假设所有的数据不确定性都是一样的，那么两个measurement的距离可以表示为

d=(xi−xj)TP−1(xi−xj)−−−−−−−−−−−−−−−−−−√

运行example2，在这个例子中，x1距离x2,x3的欧式距离都是一样的，但是由于uncertainty的原因，d1=1.2,d2=2.9，所以我们相信x1,x2是同一个measurement（或者说更近）。

在SLAM中，假设当前观测是z(k),估计是z^(k),with covariance S^,一般认为观测符合高斯分布

p(z(k))=N(z(k),z^(k),S^)

该式被称为观测似然模型measurement likelihood model.
根据Mahalanobis，我们可以定义

V(k,r)={z:(z−z^)TS^−1(z−z^)}={z:d2<r}

其中r是gate threshold， 定义的这个area被称为validation gate，\textbf{The threshold is obtained from the inverse χ2 cumulative distribution at a significance level α. }，如上图所示，这个validation gate一般是一个hyper-ellipse （ellipse in 2d）。

why χ2

有效确认域为什么要使用从χ2分布中获得阈值，关于这个contor，论文中的表述是“The gate is defined by an iso-probability contour obtained when intersecting a Gaussian with a hyperplane”， iso-probability的意思是等概率。 一个最简单的k=2的χ2如下图所示

用公式表达如下Xi是x的第i维变量，符合高斯分布Xi∈N(0,1),那么对应的χ2为

Q=∑i=1kX2i

这里X2i应该表示的是列向量逐元素相乘。Q的分布可以用k=2的χ2来描述。run example3.m. Q的分布如下图所示。

因为标准正太分布有一个阈值的特征（比如95%）,传播到Q上，同样有一个阈值的特征，而且因为都是正太分布，所以这个Q的阈值只与正太分布的个数有关，即自由度k有关，所以制作成表之后就可以直接查出具体值了。\
在1D情况下, 令u=z^,σ2=S^,

d2=(z−u)Tσ2(z−u)=y2

其中y=z−uσ,因为y N(0,1),所以在1D情况下，d2就是一个χ2 分布。\
在2D情况下时，我们依然可以令u=z^,Σ=S^,

d2=(z−u)T(Σ)−1(z−u)

再令Σ=CCT（Cholesky decomposition）,y=C−1(z−u)，那么

d2=yTCT(CCT)−1Cy=yTy=∑i=1ky2i

这里y的均值等于C−1(u−u)=0,方差等于(C−1)TC−1Σ=(C−1)TC−1CCT=I. 所以这里y的每个元素都是符合正态分布的。ND的情况和2D是一样的。

How to choose r

这里怎么选择χ2的阈值。如Q distribution图中所示，和正太分布类似，这里直接可以得出,我们有95%把握，Q落在0-5.9,如果超出了这个范围，我们可以说z,not∈{z^,S^},也就是说该值不是matching（in data association）。 当然，如图所示，我们并没有100%的把握说该值就不属于该分布，因为如图中还是有很多值在14以外的，所以The validation gate is a region of acceptance. such that 100(1−α)% of true measurements are rejected. 在这里，就是5%的true measurement are rejected。
如果您需要本文的代码，请私信我。
[1]The mahalanobis distance.De Maesschalck, Roy and Jouan-Rimbaud, Delphine and Massart