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

四、ekf预测

ekf预测过程主要有两个重要过程：
（1）机器人位姿的预测；
（2）预测位姿的协方差更新。
对应到伪代码里，就是开头两句：
$$\begin{gathered} {\overline{\mu }}_t=g\left(u_t,{\mu }_{t-1}\right) \\ {\overline{\Sigma }}_t=G_t^v{\Sigma }_{t-1}{G_t^v}^T+G_t^{u}Q{G_t^u}^T \end{gathered}$$

那么，目前机器人已经向前走了一步（真实位姿已经发生了更新），所以可以用运动模型估计机器人的位姿：
$$\{\begin{array}{c}\acute{x}=x+V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ \acute{y}=y+V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ \acute{\theta }=\theta +\frac{V_n\cdot dt\cdot \sin \left(G_n\right)}{l}\end{array}$$
此时，机器人位姿的不确定性也会发生变化，也即对应的协方差会发生改变。那么对于目前的状态下，由于只是更新了机器人的位姿，所以只需要更新位姿的协方差。看到公式里，会发现有两个两需要我们求解，一个是机器人运动模型雅可比矩阵——$G_t^v$，机器人控制模型雅可比矩阵——$G_t^u$。
首先说这个机器人运动模型雅可比矩阵，他是运动模型对三个状态变量之间的导数：
$$G_t^v=\frac{\partial }{\partial {\left(x,y,\theta \right)}^T}\left[\begin{array}{c}x+V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ y+V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ \theta +\frac{V_n\cdot dt\cdot \sin \left(G_n\right)}{l}\end{array}\right]$$
$$=I+\frac{\partial }{\partial {\left(x,y,\theta \right)}^T}\left(\begin{array}{c}{V}_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ \frac{V_n\cdot dt\cdot \sin \left(G_n\right)}{l}\end{array}\right)$$
$$=I+\left(\begin{array}{ccc}0& 0& -V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ 0& 0& V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ 0& 0& 0\end{array}\right)$$
$$=\left(\begin{array}{ccc}1& 0& -V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ 0& 1& V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ 0& 0& 1\end{array}\right)$$
然后是机器人控制模型雅可比矩阵，他其实是机器人运动模型对控制变量（$V_n,G_n$）的导数。可以理解为，运动噪声的一部分是由控制变量引起，所以控制变量的变化也就决定了噪声的大小：
$$G_t^v=\frac{\partial }{\partial {\left(V_n,G_n\right)}^T}\left[\begin{array}{c}x+V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ y+V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ \theta +\frac{V_n\cdot dt\cdot \sin \left(G_n\right)}{l}\end{array}\right]$$
$$=\left(\begin{array}{cc}\frac{\partial }{\partial \left(V_n\right)}\left[\begin{array}{c}x+V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ y+V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ \theta +\frac{V_n\cdot dt\cdot \sin \left(G_n\right)}{l}\end{array}\right]& \frac{\partial }{\partial \left(G_n\right)}\left[\begin{array}{c}x+V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ y+V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ \theta +\frac{V_n\cdot dt\cdot \sin \left(G_n\right)}{l}\end{array}\right]\end{array}\right)$$
$$=\left(\begin{array}{cc}\begin{array}{c}dt\cdot \cos \left(G_n+\theta \right)\\ dt\cdot \sin \left(G_n+\theta \right)\\ \frac{dt\cdot \sin \left(G_n\right)}{l}\end{array}& \begin{array}{c}-V_n\cdot dt\cdot \sin \left(G_n+\theta \right)\\ V_n\cdot dt\cdot \cos \left(G_n+\theta \right)\\ \frac{dt\cdot \cos \left(G_n\right)}{l}\end{array}\end{array}\right)$$
最后，得到了这两个矩阵之后，就可以进行协方差更新了。由上一篇博客我们知道，协方差更新的部分只有两个：${\Sigma }^{xx}$和${\Sigma }^{xm}$(${\Sigma }^{mx}={{\Sigma }^{xm}}^T$)。其中，${\Sigma }_t^{xm}=G_t^v{\Sigma }_{t-1}^{xm}$。
他的整体代码如下：

void extend_kf::predict(double dt)
{
	double angle_s=sin(Gn+x(2));
	double angle_c=cos(Gn+x(2));
	double vts=Vn*dt*angle_s;
	double vtc=Vn*dt*angle_c;
	
	//predict state
	x(0)=x(0)+vtc;
	x(1)=x(1)+vts; 
	x(2)=tool::normalize_angle(x(2)+Vn*dt*sin(Gn)/wheel_base);
	
	//% jacobians  
	Eigen::Matrix3d Gv;
	Eigen::Matrix<double, 3, 2> Gu;
	Gv.setIdentity(3, 3); 
	Gv(0,2)=-vts;
	Gv(1,2)=vtc;
	Gu<<dt*angle_c, -vts,
		 dt*angle_s, vtc,
		 dt*sin(Gn)/wheel_base, Vn*dt*cos(Gn)/wheel_base;
		 
	//% predict covariance
		P.block(0, 0, 3, 3) = Gv*( P.block(0, 0, 3, 3) )*(Gv.transpose()) + Gu*QE*(Gu.transpose());
	if( (P.rows()) >3)
	{
		int p_cols=P.cols();
		P.block(0, 3 , 3, (p_cols-3))=Gv*(P.block(0, 3 , 3, (p_cols-3)));
		P.block(3, 0, (p_cols-3), 3)=(P.block(0, 3 , 3, (p_cols-3)).transpose());
	}
}

五、状态观测

在本系统中，状态观测是有一定的观测周期的，由dt_observe参数进行控制，当满足一定的观测条件之后，仿真器会首先更新状态观测。
那么，对于仿真器而言，系统中的观测更新机理也是十分有趣的，其实也很简单，用一句话说就是：寻找在真实位姿下以一定的半径容纳的探测圆里的路标landmark。
程序寻找的思路是：
（1）计算所有landmark与真实位姿之间的差值；
（2）筛选并保存差值在扫描半径范围内的所有landmark的id代号；
（3）依据id代号提取出landmark的坐标信息；
（4）根据真实位姿和landmark坐标，将landmark信息转换成观测信息格式（观测距离，观测角度），并保存在ekf滤波器的观测矩阵中。
这里提到的转换也是非常简单的，project用的是激光传感器（和博客“UKF-SLAM”中使用的一样），对于第i个数据则有：
$$\begin{gathered} z_t^i=\left(\begin{array}{c}{r}_t^i\\ {\phi }_t^i\end{array}\right) \\ {\left(\begin{array}{c}{x}_t^i\\ y_t^i\end{array}\right)}_{landmark}={\left(\begin{array}{c}{x}_t^i\\ y_t^i\end{array}\right)}_{robot}+\left(\begin{array}{c}{r}_t^i\cos \left({\phi }_t^i+{\theta }_t^i\right)\\ r_t^i\sin \left({\phi }_t^i+{\theta }_t^i\right)\end{array}\right) \end{gathered}$$

接下来，可以对比代码进行理解：

void ekf_console::get_observations()
{
	Eigen::RowVectorXd dx=landmarks.row(0).array() - ekf_cal->xtrue(0);
	Eigen::RowVectorXd dy=landmarks.row(1).array() - ekf_cal->xtrue(1);
	double phi=ekf_cal->xtrue(2);
	
	//仿真器计算符合测距范围内的路标点并保存id
	ftag_visible.clear();
	for(int i=0 ; i < landmarks_size; i++)			//incremental tests for bounding semi-circle
	{
		if( (abs(dx(i)) < max_range) && (abs(dy(i)) < max_range)		
			&&  ( (dx(i)*cos(phi) + dy(i)*sin(phi)) > 0)			
			&&  ( (dx(i)*dx(i) + dy(i)*dy(i)) < (max_range*max_range) )  )  // bounding box bounding line bounding circle
		{
			ftag_visible.push_back(i);
		}
	}
	
	//提取路标点的x，y坐标信息
	int visible_num=ftag_visible.size();
	Eigen::MatrixXd  Z_coord = Eigen::MatrixXd::Zero(2, visible_num); 
	for(int i=0 ; i< visible_num ; i++)
	{
		int id=ftag_visible[i];
		Z_coord(0, i)=landmarks(0, id);
		Z_coord(1, i)=landmarks(1, id);
	}
	
	//Compute exact observation计算路标点与当前真实位置的距离
	Z_coord.row(0) = Z_coord.row(0).array()-ekf_cal->xtrue(0);
	Z_coord.row(1) = Z_coord.row(1).array()-ekf_cal->xtrue(1);

	ekf_cal->Z.resize(2, visible_num);
	ekf_cal->Z.setZero(2, visible_num);
	//计算每个路标点的观测距离并保存 range
	ekf_cal->Z.row(0) =  (Z_coord.row(0).array().square()  + Z_coord.row(1).array().square()).sqrt();
	//计算每个路标点的观测角度并保存 bearing
	for(int i=0 ; i < visible_num ; i++)//atan2(dy,dx) - phi
		ekf_cal->Z(1,i)=atan2(Z_coord(1, i), Z_coord(0, i)) - phi;	
}

以上的计算可以说是非常准确的，所以这样的观测信息不能直接给ekf滤波器，必须加上一点噪声。这里，添加的是分布为$N(0,1)$的高斯噪声，其中R矩阵是系统定义的观测噪声矩阵，代码如下：

void extend_kf::add_observation_noise(int flag)
{
	Eigen::MatrixXd z_rand;
	if(flag == 1)
	{
		int len=Z.cols();
		if(len > 0)
		{
			Z.row(0)=Z.row(0) + (z_rand.setRandom(1, len)) * sqrt(R(0, 0));
			Z.row(1)=Z.row(1) + (z_rand.setRandom(1, len)) * sqrt(R(1, 1));
		}
	}
}