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

三、ekf预测之前

1.离线数据初始化

进入主循环的第一个部分，就是保存当前状态下的数据。这里顺带提一下系统里的数据格式。首先看数据离线化保存的方法：

STATE temp;
OffLine_data.i = 0;
OffLine_data.estimate_path.push_back(x);
OffLine_data.real_path.push_back(xtrue);
temp.x = x;
temp.P = P.diagonal();//获取对角线上的元素
//temp.P << P(0, 0), P(1, 1), P(2, 2);
OffLine_data.state.push_back(temp);

这里的Offine_data和STATE是一种自定义的数据格式，在tool.h文件里有保存：

typedef struct state_data
{//the SLAM state for robot
	Eigen::MatrixXd x;//the SLAM state vector at time k
	Eigen::MatrixXd P;//the diagonals of the SLAM covariance matrix at time k
}STATE;
	
typedef struct stored_data
{//stored data for off-line
	size_t i;
	std::vector<Eigen::Vector3d> estimate_path;//path-x the vehicle path estimate (ie, where SLAM estimates the vehicle went)
	std::vector<Eigen::Vector3d> real_path;//true-xtrue the vehicle 'true'-path (ie, where the vehicle *actually* went)
	std::vector<STATE> state;//the SLAM state vector at time k
}Sdata;

Sdata OffLine_data;//Offline database.

数据保存的中的x和P数据就是本系统需要维护的重要数据，其中状态向量的构成如下图所示：

那么，这个x也就是ekf中的均值$\mu$，以及P也就是协方差$\sum$。当系统有n个landmark，他们就是是3+2n维的高斯分布 ，表示如下：

进行简化一下就是：

2.计算真实位姿

这里通过仿真器计算观测信息，添加噪声后送入ekf滤波器，那么，这里先看看真实位姿的计算。个人觉得它的计算思想特别巧妙，可以熟悉熟悉编程方法。

首先，这里贴出代码：

void ekf_console::compute_steering()
{
	Eigen::Vector2d current_wp;
	current_wp = waypoints.col(iwp);
	double d2= pow((current_wp(0) - ekf_cal->xtrue(0)), 2) + pow((current_wp(1) - ekf_cal->xtrue(1)), 2);
		
	if(d2<(at_waypoint*at_waypoint))
	{
		iwp= iwp+1; 					// switch to next
		if(iwp>(waypoints.cols()-1))		//reached final waypoint, flag and return
		{
			iwp=-1;
			return ;
		}
		current_wp = waypoints.col(iwp);//next waypoint
	}
	
	//compute change in G to point towards current waypoint
	double deltaG=tool::normalize_angle( (atan2( current_wp(1) - ekf_cal->xtrue(1), current_wp(0) - ekf_cal->xtrue(0) ) - ekf_cal->xtrue(2) - G)  );
	
	//limit rate
	double maxDelta=rateG*dt;
	if(abs(deltaG)>maxDelta)
		deltaG= maxDelta*(sign(deltaG));
	G=G+deltaG;
	if(abs(G)>maxG)
		G=maxG*(sign(G));
}

解释一下它的过程：
（1）首先，计算car真实位姿xtrue与当前路径点current_wp（注意car未到这个路径点）的欧几里得距离，其中current_wp通过路径点代号iwp进行索引；
（2）然后，判断他们的距离是否小于（at_waypoint*at_waypoint），若是则说明达到路径点附近了，开始更换下一个路径点。此处会检查路径点代号是否超过最大值，若超过了则置-1。
（3）紧接着，计算（前面的）路径点与当前位姿的夹角deltaG。这里程序进行了最大值限制，最后累加到转角G中，完成转角的更新。

接下来，程序进行了一个简单的回环检测，下面就说说其中涉及的回环检测。

3.回环检测

在视觉SLAM问题中，位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便这样一帧一帧的传递下去，也就是我们所说的累计误差。

我们的位姿约束都是与上一帧建立的，第五帧的位姿误差中便已经积累了前面四个约束中的误差。但如果我们发现第五帧位姿不一定要由第四帧推出来，还可以由第二帧推算出来，显然这样计算误差会小很多，因为只存在两个约束的误差了。像这样与之前的某一帧建立位姿约束关系就叫做回环。回环通过减少约束数，起到了减小累计误差的作用。

那么我们怎么知道可以由第二帧推算第五帧位姿呢？也许第一帧、第三帧也可以呢。确实，我们之所以用前一帧递推下一帧位姿，因为这两帧足够近，肯定可以建立两帧的约束，但是距离较远的两帧就不一定可以建立这样的约束关系了。找出可以建立这种位姿约束的历史帧，就是回环检测。

———摘自浅谈SLAM的回环检测技术

而在这个project中，就是通过简单地判断目前到达的路径点是否回到原点，若是回到原点，则回环次数number_loops减1，若number_loops次数用完则置iwp为0，退出程序。路径点iwp的更新发生在compute_steering()函数中，回环检测部分代码如下：

if((iwp==-1)&&(number_loops>1))
{
	iwp=0;
	number_loops=number_loops-1;
}

4.更新真实位姿

更新最新路径点之后，就是需要开始更新真实位姿了，这里非常好理解，看代码：

void extend_kf::vehicle_model(double velocity, double Sangle, double dt)
{
	double angle = xtrue(2);
	xtrue(0) = xtrue(0) + velocity*dt*cos(Sangle+angle);
	xtrue(1) = xtrue(1) + velocity*dt*sin(Sangle+angle);
	xtrue(2) = tool::normalize_angle(angle + velocity*dt*sin(Sangle)/wheel_base);
}

这里可以参照上一篇博客中的car运动模型。
得到了真实位姿，就需要开始为ekf滤波器准备数据了，首先是线速度Vn和角速度Gn，看代码：

void extend_kf::add_control_noise(double V, double G, int flag)
{
	if(flag == 1)
	{
		Vn=V+  (normal(gen) )  * sqrt(Q(0, 0));
		Gn=G+  (normal(gen) )  * sqrt(Q(1, 1));
	}
}

这里是对真实线速度V和角速度G增加了正态分布噪声，由正态分布随机数发生器配合预测噪声Q产生。