从程序中学习EKF-SLAM（四）

六、数据关联

数据关联的概念在之前的博客中已经解释了，这里就不在赘述了，这里重点说说程序中的两种实现，在本project里有两种数据关联：
（1）已知数据关联，是指路标landmark的观测顺序已知，即观测的信息含有路标id代号（这一种方法，和之前的博客“UKS-SLAM入门”中的类似）；
（2）未知数据关联，是指landmark的观测顺序未知，也即观测信息只有距离和角度（本project中使用的就是这一种）。

在project中，它是通过一个选择参数来控制执行哪种数据关联的，看代码：

if(switch_association_know == 1)
	ekf_cal->data_associate_known(ftag_visible);			//已知数据关联，路标观测顺序已知
else ekf_cal->data_associate(gate_reject, gate_augment);		//默认进程

首先介绍第一种，已知数据关联。它的核心就是一个一维数组da_table，就是数据关联表。它会记录每次观测的路标landmark的唯一id代号。那么，将观测信息中的id代号进行比对，就能区分是新landmark还是旧landmark了。程序的主要流程是：
（1）检索数据关联表，同时与当前观测信息比较，区分新landmark和旧landmark，并记录相应的id代号；
（2）根据id代号，填充相应的观测矩阵；
（3）更新数据关联表。
它的代码如下：

void extend_kf::data_associate_known(std::vector< int > ftag_visible)
{
	int marker_id=0;
	int len = ftag_visible.size();
	
	//相关变量进行初始化
	Zn.resize(2, len);
	Zn.setZero(2, len);
	int Zn_col=0;
	
	Zf.resize(2, len);
	Zf.setZero(2, len);
	int Zf_col=0;
	
	//检索数据关联表，记录新旧特征
	std::vector<int> idn;	
	idn.clear();
	idf.clear();
	for(int i=0 ; i<len ; i++)
	{
		marker_id=ftag_visible[i];	//从观测列表中取出路标id号
		if(da_table[marker_id] == 0)//new feature
		{
			Zn.col(Zn_col)=Z.col(i);
			idn.push_back(marker_id);
			Zn_col++;
		}else{
			Zf.col(Zf_col)=Z.col(i);
			idf.push_back(marker_id);
			Zf_col++;
		}
	}

	//填充观测矩阵
	Eigen::MatrixXd temp;
	temp = Zn;
	Zn.resize(2, Zn_col);
	for(int i = 0 ; i < Zn_col ; i++)
		Zn.col(i) = temp.col(i);

	temp = Zf;
	Zf.resize(2, Zf_col);
	for(int i = 0 ; i < Zf_col ; i++)
		Zf.col(i) = temp.col(i);
	
	//更新数据关联表
	int Nxv= 3;					// number of vehicle pose states
	double Nf= (x.rows() - Nxv)/2; 	//number of features already in map
	//add new feature positions to lookup table
	int new_fea_size = idn.size();
	for(int i = 0 ; i < new_fea_size; i++)
		da_table[idn[i]]= Nf +i+1;
}

接下来，是第二种，未知数据关联，这个方法就比较有意思了。它使用归一化马氏距离来评判观测路标landmark是新的还是旧的。

马氏距离(Mahalanobis distance)是由印度统计学家马哈拉诺比斯(P. C. Mahalanobis)提出的，表示数据的协方差距离。它是一种有效的计算两个未知样本集的相似度的方法。与欧氏距离不同的是，它考虑到各种特性之间的联系（例如：一条关于身高的信息会带来一条关于体重的信息，因为两者是有关联的），并且是尺度无关的(scale-invariant)，即独立于测量尺度。对于一个均值为μ，协方差矩阵为Σ的多变量向量，其马氏距离为$d=\sqrt{(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}$。
——摘自百度百科《马氏距离》

那么它是如何计算的呢？其实计算很简单，有这么几步，非常好理解：
（1）利用观测模型计算每个landmark的观测矩阵$\hat{z}$以及对应的协方差矩阵$H$；
（2）计算当前观测矩阵与每一个已知路标的观测矩阵的差值\bar{z}，然后计算样本协方差$S$，也即数据的协方差距离；
$$\begin{gathered} \bar{z}=z-\hat{z} \\ S=H\cdot \Sigma \cdot H+R \end{gathered}$$
（3）根据公式计算马氏距离；
$$d_{nis}={\bar{z}}^T\cdot S^{-1}\cdot \bar{z}$$
（4）先计算出马氏距离，然后再通过log函数计算归一化马氏距离。
$$d_{nd}=d_{nis}+\log \left(\det \left(S\right)\right)$$
可以参照代码理解：

double nis=0, nd=0;
observe_model(j, zp, H);
Eigen::Vector2d v = Z.col(i) - zp;
v(1)=tool::normalize_angle(v(1));
Eigen::Matrix2d S=H*P*( H.transpose() ) + RE;	//计算样本协方差，又称为数据的协方差距离
nis =  ( v.transpose() ) * ( S.inverse() ) * v;		//计算马氏距离
nd = nis + log( (S.determinant() ) );			//计算归一化距离(determinant是指行列式值)

（5）比较马氏距离与归一化距离，判断当前观测的路标是否是新路标。
判断方法可以参照代码进行理解：

if((nis < reject_flag) && (nd < nbest))			//if within gate, store nearest-neighbour
{
	nbest= nd;
	jbest = j;
}else if(nis < outer)					//else store best nis value
	outer= nis;

（6）再之后，就是更新观测矩阵等操作了，就不再进行细说了。
参考代码：

int id_size=Zf_id.size();
Zf.resize(2, id_size);
Zf.setZero(2, id_size);
if(id_size != 0)
{//将具有关联的路标点信息提取到观测矩阵
	for(int i=0;i<id_size;i++)
		Zf.col(i)=Z.col(Zf_id[i]);
}
id_size=Zn_id.size();
Zn.resize(2, id_size);
Zn.setZero(2, id_size);
if(id_size != 0)
{//将新的路标信息提取出来，并保存
	for(int i=0;i<id_size;i++)
		Zn.col(i)=Z.col(Zn_id[i]);
}

其中提到了观测模型计算，这里再补充一下观测模型计算的内容。它其实就是将landmark的坐标信息转换为观测信息，用于和当前观测进行对比。主要过程是：
（1）计算路标landmark与当前状态下机器人位置的差值；
$$\left(\begin{array}{c}{\delta }_x\\ {\delta }_y\end{array}\right)={\left(\begin{array}{c}x\\ y\end{array}\right)}_{landmark}-{\left(\begin{array}{c}x\\ y\end{array}\right)}_{robot}$$
（2）计算观测信息中的射线长度，构建观测矩阵；
$$\begin{gathered} q={\delta }^T\delta =(x_{landmark}-x_{robot}{)}^2+(y_{landmark}-y_{robot}{)}^2 \\ \hat{z}=\left(\begin{array}{c}\sqrt{q}\\ \text{atan}2\left({\delta }_y,{\delta }_x\right)-{\theta }_{robot}\end{array}\right) \end{gathered}$$
（3）接下来，就是计算观测模型的雅可比，也就是观测矩阵对状态变量的导数；
首先已知
$$\begin{gathered} h\left({\overline{\mu }}_t\right)={\left(\begin{array}{c}\sqrt{q}\\ \text{atan}2\left({\delta }_y,{\delta }_x\right)-{\theta }_{robot}\end{array}\right)}_t \\ {\overline{\mu }}_t=\left(x,y,\theta ,m_x^j,m_y^j\right) \end{gathered}$$
然后，求导数
$$\begin{gathered} {}^{low}{H}_t^i=\frac{\partial h\left({\overline{\mu }}_t\right)}{\partial {\overline{\mu }}_t} \\ =\left(\begin{array}{ccc}\begin{array}{c}\frac{\partial \sqrt{q}}{\partial x}\\ \frac{\partial \text{atan}2\left(...\right)}{\partial x}\end{array}& \begin{array}{c}\frac{\partial \sqrt{q}}{\partial y}\\ \frac{\partial \text{atan}2\left(...\right)}{\partial y}\end{array}& \begin{array}{c}...\\ ...\end{array}\end{array}\right) \end{gathered}$$
其中，两个部分分开计算，第一部分：
$$\begin{gathered} \frac{\partial \sqrt{q}}{\partial x}=\frac{1}{2}\cdot q^{-\frac{1}{2}}\cdot 2{\delta }_x\cdot \left(-1\right) \\ =\frac{1}{\sqrt{q}}\cdot \left(-{\delta }_x\right) \\ =\frac{1}{q}\cdot \left(-\sqrt{q}{\delta }_x\right) \end{gathered}$$
第二部分：
$$\begin{gathered} \frac{\partial \left(\text{atan}2\left(\frac{{\delta }_y}{{\delta }_x}\right)\right)}{\partial x}=\frac{1}{1+{\left(\frac{{\delta }_y}{{\delta }_x}\right)}^2}\cdot \left(-\frac{{\delta }_y}{{\delta }_x^2}\right)\cdot \left(-1\right) \\ =\frac{{\delta }_x^2}{{\delta }_x^2+{\delta }_y^2}\cdot \frac{{\delta }_y}{{\delta }_x^2} \\ =\frac{{\delta }_y}{{\delta }_x^2+{\delta }_y^2} \\ =\frac{1}{q}\cdot {\delta }_y \end{gathered}$$
则最后求得：
$${}^{low}{H}_t^i=\frac{\partial h\left({\overline{\mu }}_t\right)}{\partial {\overline{\mu }}_t}=\frac{1}{q}\left(\begin{array}{cccc}\begin{array}{c}-\sqrt{q}{\delta }_x\\ {\delta }_y\end{array}& \begin{array}{c}-\sqrt{q}{\delta }_y\\ -{\delta }_x\end{array}& \begin{array}{c}0\\ -q\end{array}& \begin{array}{cc}\begin{array}{c}\sqrt{q}{\delta }_x\\ -{\delta }_y\end{array}& \begin{array}{c}\sqrt{q}{\delta }_y\\ {\delta }_x\end{array}\end{array}\end{array}\right)$$
最后就能求得$t$时刻第$i$个观测数据的协方差矩阵$H_t^i{=}^{low}{H}_t^i{F}_{x,j}$:

现在看代码吧：

void extend_kf::observe_model(int IDf, Eigen::MatrixXd& out_Z, Eigen::MatrixXd& out_H)
{
	int Nxv= 3; 			//number of vehicle pose states
	int fpos= Nxv + IDf*2 ; // position of xf in state
	out_Z.resize(2, 1);
	out_Z.setZero(2, 1);
	out_H.resize(2, x.rows());
	out_H.setZero(2, x.rows());

	//% auxiliary values 辅助值
	double dx= x(fpos)  -x(0); 
	double dy= x(fpos+1)-x(1);
	double d2= dx*dx + dy*dy;
	double d= sqrt(d2);
	double xd= dx/d;
	double yd= dy/d;
	double xd2= dx/d2;
	double yd2= dy/d2;
	
	//predict 观测模型估计矩阵Zit
	out_Z<<d, (atan2(dy, dx) - x(2));
	//calculate H 观测模型协方差矩阵Hit
	out_H.leftCols(3)<<-xd, -yd, 0, yd2, -xd2, -1 ;
	out_H.middleCols(fpos, 2)<<xd, yd,  -yd2, xd2 ;
}