从程序中学习UKF-SLAM（三）

下面接着说UKF的更新部分，在这个环节里需要分开两种情况进行讲述：

1. 观测到已知路标时，需要对已有的所有的状态变量和协方差进行更新；
2. 观测到未知路标时，需要将新路标的信息添加到状态变量中，同时更新协方差（换句话说，就是需要对状态变量和协方差进行扩充）；

这里说到了已知路标和未知路标的问题，在SLAM领域就叫做数据关联。

五、数据关联

数据关联技术来源于多目标跟踪环境和传感器的观测过程之间的不确定性。这是因为在目标跟踪过程中，同一个目标可以在名个传感器上产生多个观测值，这些观测值不完全相似，主要是由杂波和传感器的系统误差导致，数据关联的目的就是要判定这些不完全相似的观测值是否来源于同一目标。换句话说，数据关联就是用于确定传感器的测量信息和目标源之间的对应关系。目前，数据关联技术在多月标跟踪领域中占有非常重要的位置，一直是研究的热点问题，因为数据关联的优劣将直接影响跟踪系统的整体性能。

数据关联一般分为三种方式。
第一种是点迹和点迹的关联，最终形成航迹或对航迹进行初始化。航迹的形成，是按照准则对来自不同采样周期的点迹进行处理实现的。

第二种是点迹和航迹之间的关联，其目的是对已有的航述来进行保持或进行状态更新。例如，判断雷达站所接收到的点迹(哪些是已有航迹的点迹，哪些是新航迹的点迹，哪些是杂波产生的虚假点迹),然后根据相应的准则把属于已有航迹的点迹和该航迹匹配起来，使该航迹能得到延续，实现航迹更新。经过长时间运行之后，那些没有匹配上的点迹和虚警便成了孤立点迹，此时再采用一定的准则来剔除这些点迹。

第三种是航迹和航迹的关联，即用于确定不同传感器的航边来自同一目标。在多传感器系统中，每个传感器利用自身的信息处理系统，来实现对多目标的跟踪。股情况下，把单个传感器的航迹假设成局部航迹，然后按照一定的时间，每个传感器把自身的全部航迹发送给信息处理中心，接着在该处理中心对各局部航还进行匹配判断，形成全局航迹。最后对全局航迹的状态进行更新。

摘录自：贾渭娟. SLAM中数据关联算法研究[D]. 2013.

通俗点说，数据关联的工作就是识别当前的路标是否是之前遇见过的，若是则说明是已知路标，若不是则是新路标。

在project中，路标的识别工作是通过landmark的绝对id代号来完成的，这里的每一个landmark上都有唯一且固定的id代号，而且观测信息里也会包含看到的landmark的id代号，所以，这里通过对landmark的id代号进行查找就能实现数据关联操作了，若找到了则说明是已知路标，若没有找到则说明是未知路标（新路标）。

所以，从这里将会分开两种情况进行更新，看代码：

void unscented_kf::Correction(const std::vector< Eigen::Vector3f >& observation)
{
	// number of measurements in this step
	int m = observation.size();
	//[range, bearing, range, bearing, .....]
	//Jacobian matrix;
	for (int i = 0; i < m; i++)
	{
		auto&  reading  = observation[i];
		//landmark is not seen before, so to initialize the landmarks
		if (std::find(LandmarkINmap.begin(), LandmarkINmap.end(), (int)reading(0)) == LandmarkINmap.end())
		{
			add_landmark_to_map(reading);
		} else {
			//Compute sigma points from the predicted mean and covariance
			Eigen::MatrixXd sigma_points;
			compute_sigma_points(sigma_points);
			update(sigma_points, reading);
		}
	}
}

六、未知路标下的变量扩充

当在数据关联环节发现该路标是未知路标时，就需要对状态变量和协方差进行扩充，同时将该路标的id代号保存起来，在project中用全局变量LandmarkINmap（在unscented_kf.cpp中）保存。

然后，再进行sigma点的采样工作和变量的更新操作，这里通过调用compute_sigma_points()函数和recover_mu_sigma()函数就可以实现了。详细代码如下：

void unscented_kf::add_landmark_to_map(const Eigen::Vector3f& z)
{
	int n = Mu_x.rows();
	LandmarkINmap.push_back((int)z(0));

	//increae size of mu and sigma
	Eigen::VectorXd tempmu = Eigen::VectorXd::Zero(n + 2);
	tempmu.head(n) 	=  Mu_x; 
	tempmu(n)      	= (double)z(1);
	tempmu(n + 1)  	= (double)z(2);
	Mu_x = tempmu;

	//this operation initializes the uncertainty in the position of the landmark
	Eigen::MatrixXd tempsigma = Eigen::MatrixXd::Zero(n + 2, n + 2);
	tempsigma.topLeftCorner(n, n) = Sigma_y;
	tempsigma.bottomRightCorner(2, 2) = Q_;
	Sigma_y = tempsigma;

	//sample sigma points
	Eigen::MatrixXd sig_pts;
	compute_sigma_points(sig_pts);
	//normalize
	Eigen::VectorXd Z = (sig_pts.row(2)).transpose();
	tool::normalize_bearing(Z);
	sig_pts.row(2) = Z.transpose();

	//compute newX and newY
	Eigen::RowVectorXd angle_c = (sig_pts.row(2) + sig_pts.row(n + 1)).array().cos();
	Eigen::RowVectorXd angle_s = (sig_pts.row(2) + sig_pts.row(n + 1)).array().sin();
	Eigen::RowVectorXd delta_X = sig_pts.row(n).array() * angle_c.array();
	Eigen::RowVectorXd delta_Y = sig_pts.row(n).array() * angle_s.array();

	//transform from [range, bearing] to the x/y location of the landmark
	sig_pts.row(n)   = sig_pts.row(0) + delta_X ; 
	sig_pts.row(n+1) = sig_pts.row(1) + delta_Y ; 
	
	//update mu and sigma
	recover_mu_sigma(sig_pts);
}

其中需要注意的有：
（1）扩充状态向量Mu_x时，先不进行路标坐标的计算，先把观测信息（距离，角度）放入状态向量中，然后在经过compute_sigma_points()函数的采样后，再进行转换，将观测信息转换成路标坐标。具体流程参考代码。
那么这里可以怎么理解呢？我的理解是观测数据是具有噪声，所以在经过sigma点采样后再计算观测路标的坐标，这样可以降低噪声。当然，先转换成坐标值，再进行采样计算来去噪也是可以的。但是后一种的转换已经是带着误差的，这样做会使得误差累积越来越大。所以，先去噪再计算才能更进一步地减小噪声的影响。
（2）而对应的协方差Sigma_y直接用观测噪声Q_进行初始化。
（3）可以注意到这里的compute_sigma_points()函数是对所有的状态向量进行了采样。

七、已知路标下的状态更新

那么当数据关联发现现在的路标是已知的时候，就需要进行状态更新了。同样的，首先用compute_sigma_points()函数对所有的状态向量进行采样，然后用update函数进行状态更新。

1.估计已知路标的新位置

用路标的id代号在LandmarkINmap中的位置关系，从采样好的sigma点中提取对应的采样点，换算成观测信息格式（距离，角度），再进行权重计算：
$$\begin{gathered} {\bar{Z}}_t=h\left({\xi }_t\right) \\ {\bar{z}}_t=\sum _{i=0}^{2n}{W}_m^i{\bar{Z}}_t^i \end{gathered}$$
其中，${\bar{z}}_t$对应下面代码中的z_mean变量，t = 在LandmarkINmap中的位置（程序中的index变量）。

Eigen::VectorXd DX = sigma_points.row(2*index + 3) - sigma_points.row(0);
Eigen::VectorXd DY = sigma_points.row(2*index + 4) - sigma_points.row(1);

Eigen::MatrixXd Z_points = Eigen::MatrixXd::Zero(2, 2 * n + 1);
for (int i = 0; i < num_sig; i++) 
{
	double q = pow(DX(i), 2) + pow(DY(i), 2);
	Z_points(0, i) = sqrt(q);
	Z_points(1, i) = tool::normalize_angle(atan2(DY(i), DX(i)) - sigma_points(2, i));
}

//weight vector
Eigen::VectorXd weights = Eigen::VectorXd(num_sig);
weights(0) = lambda / Scale;
for (int i = 1; i < num_sig; i++) 
	weights(i) = 0.5 / Scale;

//zm is the recovered expected measurement mean from z_points.
//It will be a 2x1 vector [expected_range; expected_bearing].
Eigen::VectorXd z_mean = Eigen::VectorXd::Zero(2);
z_mean(0) = Z_points.row(0)*weights;

Eigen::RowVectorXd angle_c = Z_points.row(1).array().cos();
Eigen::RowVectorXd angle_s = Z_points.row(1).array().sin();
double x_bar = angle_c * weights;
double y_bar = angle_s * weights;
z_mean(1) = tool::normalize_angle(atan2(y_bar, x_bar));

2.计算卡尔曼参数k

然后，再利用该路标的估计值与sigma采样点进行差值计算，计算估计的不确定性，最后计算卡尔曼参数：
$$\begin{gathered} S_t=\sum _{i=0}^{2n}{W}_c^i\left({\bar{Z}}_t^i-{\bar{z}}_t\right){\left({\bar{Z}}_t^i-{\bar{z}}_t\right)}^T+Q \\ {\Sigma }_t^{x,z}=\sum _{i=0}^{2n}{W}_c^i\left({\xi }_k^i-X_k\right){\left({\bar{Z}}_t^i-{\bar{z}}_t\right)}^T \\ {K}_k={\Sigma }_t^{x,z}\times S_t^{-1} \end{gathered}$$
其中，$S_t$和${\Sigma }_t^{x,z}$在程序中分别用变量S和变量sigma_x_z表示，因为两个变量的计算具有重复性，所以放在了同一个循环中进行。k是指k时刻的状态。

Eigen::MatrixXd S = Eigen::MatrixXd::Zero(2,2);
Eigen::MatrixXd sigma_x_z = Eigen::MatrixXd::Zero(n,2);
for (int i = 0; i < num_sig; i++)
{
	Eigen::VectorXd tmpz = Z_points.col(i) - z_mean;
	tmpz(1) = tool::normalize_angle(tmpz(1));
	S = S + tmpz*tmpz.transpose()*weights(i);

	Eigen::VectorXd tmpx = sigma_points.col(i) - Mu_x;
	tmpx(2) = tool::normalize_angle(tmpx(2));
	sigma_x_z = sigma_x_z + weights(i)*tmpx*tmpz.transpose();
}
S = S + Q_;

// Compute the Kalman gain
Eigen::MatrixXd Kt = sigma_x_z*(S.inverse());

这里需要注意的有：
（1）S矩阵的维数是2x2，变量Z_points 的维数是2x（2n+1）;

3.计算更新

最后一步，就是计算更新了。这里的公式参照程序编写，因为在预测步骤中由时刻k-1到了k时刻，所有这里都是k时刻的状态变化：
$$\begin{gathered} X_k=X_k+K_k\times \left(z_t-{\bar{z}}_t\right) \\ {P}_k=P_k-K_k{S}_t{K}_k^T \end{gathered}$$

Eigen::VectorXd z_actual = Eigen::VectorXd(2);
z_actual << Z(1), Z(2);
Eigen::VectorXd zdiff = z_actual-z_mean;
zdiff(1) = tool::normalize_angle(zdiff(1));

Mu_x = Mu_x + Kt*(zdiff);
Sigma_y = Sigma_y - Kt*S*Kt.transpose();

// TODO: Normalize the robot heading mu(3)
Mu_x(2) = tool::normalize_angle(Mu_x(2));

对应程序的伪代码是（参考《概率机器人》）：

到这里，所有的内容就结束了，相关代码参考我的GitHub。