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

七、状态更新

在完成了数据关联后，就确定了路标是新路标还是旧路标，最后就能进行状态更新了。这里同样有新landmark下的状态更新和旧landmark下的状态更新。
这里先介绍旧landmark下的状态更新，大致有下面几个步骤：
（1）利用观测模型计算每个landmark的观测矩阵$\hat{z}$以及对应的协方差矩阵$H$（这一步骤和上一篇博客“从程序中学习EKF-SLAM（四）”中的一样，这里就不细说了），同时计算实际观测值与理想观测值的差值$\bar{z}$：
$$\bar{z}=z-\hat{z}$$
直接看代码：

H.resize(2*Zf_len, x_len); 
H.setZero(2*Zf_len, x_len);
v.resize(2*Zf_len, 1); 
v.setZero(2*Zf_len, 1);
RR.resize(2*Zf_len, 2*Zf_len); 
RR.setZero(2*Zf_len, 2*Zf_len);

for(int i=0; i < Zf_len ; i++)
{
	int ii= 2*i ;
	observe_model(idf[i], zp, H_temp);
	H.middleRows(ii, 2) = H_temp;
	v(ii) = Zf(0, i) - zp(0);
	v(ii+1) = tool::normalize_angle( Zf(1,i) - zp(1) );
	RR.block(ii, ii, 2, 2)  = RE;
}
KF_cholesky_update(v, RR, H);

（2）计算卡尔曼常数K；
$$K_t^i={\overline{\Sigma }}_t{H_t^i}^T{\left(H_t^i{\overline{\Sigma }}_t{H_t^i}^T+Q_t\right)}^{-1}$$
（3）更新状态向量矩阵和协方差矩阵。
$$\begin{gathered} {\overline{\mu }}_t={\overline{\mu }}_t+K_t^i\left(z_t^i-{\hat{z}}_t^i\right) \\ {\overline{\Sigma }}_t=\left(I-K_t^i{H}_t^i\right){\overline{\Sigma }}_t \end{gathered}$$
可以参照代码理解：

void extend_kf::KF_cholesky_update(Eigen::MatrixXd v, Eigen::MatrixXd R, Eigen::MatrixXd H)
{
	Eigen::MatrixXd PHt=P*H.transpose();
	Eigen::MatrixXd S=H*PHt+R;
	
	//make symmetric生成对角阵
	S= (S + ( S.transpose() ) )*0.5; 
	//SChol= chol(S);
	Eigen::MatrixXd SChol = ( S.llt().matrixL() ).transpose() ;//Cholesky分解法 A=LL^T ,matlab用的是上三角矩阵
	
	//triangular matrix
	Eigen::MatrixXd SCholInv= (SChol).inverse() ;  
	Eigen::MatrixXd W1= PHt * SCholInv;
	Eigen::MatrixXd K= W1 * ( SCholInv.transpose() );	//这里的W就是卡尔曼滤波的K增益参数
	
	//update
	x = x + K*v; 			//这里的v矩阵就是观测值与预测值的差
	P = P - W1* ( W1.transpose() );	
}

其中需要注意的有：
（1）程序中PHt变量就是计算${\overline{\Sigma }}_t{H_t^i}^T$，S变量就是计算$\left(H_t^i{\overline{\Sigma }}_t{H_t^i}^T+Q_t\right)$；
（2）在求解S的逆矩阵之前，进行了单位化生成对角阵，这里为什么这样做呢？问过大佬后得知：虽然这样做改变了矩阵S的大小，但是能确保处理之后的矩阵S保持正定的性质，因为协方差只是表征状态向量的变化程度或者说区别，所有只要矩阵S的每个元素之间“关系”保持不变，S矩阵还能能够体现这个系统的运行状态的；
（3）需要计算变量S的逆矩阵，程序中采用Cholesky分解法计算三角阵来求矩阵的逆（注意S的逆矩阵应是变量乘以变量的转置），这样能加快求解速度；

另外一种情况，就是新路标情况下的状态更新了，这部分的重点就是变量的扩充。那么，他大致有如下步骤：
（1）将landmark的观测信息转换为地图坐标信息，这里的转换关系就不再啰嗦了；
（2）计算观测模型的雅可比矩阵$G_v$，也就是观测模型对变量$(x,y,\theta )$的导数；
$$\frac{\partial \left(\begin{array}{c}x+r\cos \left(\theta +\phi \right)\\ y+r\sin \left(\theta +\phi \right)\end{array}\right)}{\partial \left(x,y,\theta \right)}=\left(\begin{array}{ccc}\begin{array}{c}1\\ 0\end{array}& \begin{array}{c}0\\ 1\end{array}& \begin{array}{c}-r\sin \left(\theta +\phi \right)\\ r\cos \left(\theta +\phi \right)\end{array}\end{array}\right)$$
（3）计算观测模型的雅可比矩阵$G_z$，也就是观测模型对观测变量$(r,\theta )$的导数，用于计算观测的噪声；
$$\frac{\partial \left(\begin{array}{c}x+r\cos \left(\theta +\phi \right)\\ y+r\sin \left(\theta +\phi \right)\end{array}\right)}{\partial \left(r,\phi \right)}=\left(\begin{array}{cc}\begin{array}{c}\cos \left(\theta +\phi \right)\\ \sin \left(\theta +\phi \right)\end{array}& \begin{array}{c}-r\sin \left(\theta +\phi \right)\\ r\cos \left(\theta +\phi \right)\end{array}\end{array}\right)$$
（4）更新状态变量和协方差。
$landmark:P_t^i=G_v\ast P_{robot}\ast G_v^T+G_z\ast E\ast G_z^T$
可以参考代码理解：

void extend_kf::augment(void)
{
	Eigen::MatrixXd P_expansion;//大矩阵P的扩充矩阵
	Eigen::VectorXd x_expansion;//大矩阵x的扩充矩阵
	int Zn_len=Zn.cols();
	
	for(int i=0 ; i < Zn_len ; i++)
	{
		int x_len=x.rows();
		double r=Zn(0, i);
		double b=Zn(1, i);
		double s= sin(x(2)+b); 
		double c= cos(x(2)+b);
		
		//augment x,将新的landmark路标的x-y坐标加入状态矩阵
		x_expansion.resize(x_len+2, 1);
		x_expansion.setZero(x_len+2, 1);
		x_expansion.topRows(x_len) = x;
		x_expansion(x_len) = x(0) + r*c;
		x_expansion(x_len+1) = x(1) + r*s ;
		x.resize(x_len+2, 1);
		x = x_expansion;
		
		//jacobians
		Eigen::Matrix<double, 2, 3> Gv;
		Eigen::Matrix2d Gz;
		Gv<<1, 0, -r*s, 0, 1, r*c;
		Gz<<c, -r*s, s, r*c;
		
		//augment P
		P_expansion.resize(x_len+2, x_len+2);
		P_expansion.setZero(x_len+2, x_len+2);
		P_expansion.topLeftCorner(x_len, x_len) = P;
		P_expansion.block(x_len, x_len, 2, 2) = Gv* ( P.block(0, 0, 3, 3) ) * ( Gv.transpose() ) + Gz*RE* ( Gz.transpose() ); // feature cov
		P_expansion.block(x_len, 0, 2, 3) = Gv* ( P.block(0, 0, 3, 3) ); 									//vehicle to feature xcorr
		P_expansion.block(0, x_len, 3, 2) = ((P_expansion.block(x_len, 0, 2, 3) ).transpose());
		P.resize(x_len+2, x_len+2);
		P = P_expansion;
		if(x_len > 3)
		{//map to feature xcorr
			int rnm=x_len-3;
			P.block(x_len, 3, 2, rnm) = Gv*(P.block(0, 3, 3, rnm)); 
			P.block(3, x_len, rnm, 2) = ( (P.block(x_len, 3, 2, rnm)).transpose() );
		}
	}
}

八、RVIZ显示

以上结束了整个EKF-SLAM系统的一次循环，那么最后就是在RVIZ进行相关显示了。本project中，对机器人的预估位姿和实际位姿进行了显示，还显示了观测数据的实时状态，里程计和路径，还有整个系统的不确定性状态。这里就说说里程计的发布以及路径Path的发布，其他在之前的博客都有过介绍。

Path发布需要注意它是绘制了整个路径的，所以先对每个时刻的机器人状态进行了离线保存，看代码：

void extend_kf::store_data(void)
{
	STATE temp;
	int CHUNK= 5000;
	Eigen::Vector3d x_est;
	x_est.setZero(3, 1);
	
	OffLine_data.i = OffLine_data.i + 1;
	x_est = x.topRows(3);
	OffLine_data.estimate_path.push_back(x_est);
	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);
}

里程计发布需要注意tf坐标系转换关系，注意父子坐标系不能反了，还有一个非常重要的：发布的频率有一定的限制，必须够快，也就是说太慢的话在RVIZ里是看不到的，而且tf转换也会出问题，发生报错，这是一个经验。其他的看代码：

void ekf_console::odometry_publisher()
{// robot estimate  pose 
	double xest_x=ekf_cal->x(0);
	double xest_y=ekf_cal->x(1);
	double xest_th=ekf_cal->x(2);
	double height=1.0;
	
	// odom to map tree broadcaster
	tf::TransformBroadcaster odom_broadcaster;
	ros::Time current_time= ros::Time::now();
	//since all odometry is 6DOF we'll need a quaternion created from yaw
	geometry_msgs::Quaternion xest_quat = tf::createQuaternionMsgFromYaw(xest_th);
	//first, we'll publish the transform over tf
	geometry_msgs::TransformStamped odom_trans;
	odom_trans.header.stamp = current_time;
	odom_trans.header.frame_id = "odom";
	odom_trans.child_frame_id = "base_link";
	odom_trans.transform.translation.x = xest_x;
	odom_trans.transform.translation.y = xest_y;
	odom_trans.transform.translation.z = height;
	odom_trans.transform.rotation = xest_quat;
	//send the transform
	odom_broadcaster.sendTransform(odom_trans);

	//next, we'll publish the odometry message over ROS
	nav_msgs::Odometry odom;
	odom.header.stamp = current_time;
	odom.header.frame_id = "odom";
	//set the position
	odom.pose.pose.position.x = xest_x;
	odom.pose.pose.position.y = xest_y;
	odom.pose.pose.position.z = height;
	odom.pose.pose.orientation = xest_quat;
	//set the velocity
	odom.child_frame_id = "base_link";
	odom.twist.twist.linear.x = velocity*cos(xest_th);
	odom.twist.twist.linear.y = velocity*sin(xest_th);
	odom.twist.twist.angular.z = xest_th;
	//publish the message
	odom_pub.publish(odom);
	
	nav_msgs::Path xest_path;
	xest_path.header.stamp=current_time;
	xest_path.header.frame_id="odom";
	geometry_msgs::PoseStamped xest_stamped;
	xest_stamped.pose.position.z = height;
	
	size_t xest_paths=ekf_cal->OffLine_data.estimate_path.size();
	Eigen::Vector3d x_est;
	for(size_t i=0;i< xest_paths; i++)
	{
		x_est=ekf_cal->OffLine_data.estimate_path[i];
		xest_stamped.pose.position.x = x_est(0);
		xest_stamped.pose.position.y = x_est(1);
		
		xest_quat = tf::createQuaternionMsgFromYaw((x_est(2)));
		xest_stamped.pose.orientation.x = xest_quat.x;
		xest_stamped.pose.orientation.y = xest_quat.y;
		xest_stamped.pose.orientation.z = xest_quat.z;
		xest_stamped.pose.orientation.w = xest_quat.w;
		xest_stamped.header.stamp=current_time;
		xest_stamped.header.frame_id="odom";
		xest_path.poses.push_back(xest_stamped);
	}
	path_pub.publish(xest_path);
}

那么，现在整个EKF-SLAM系统就介绍完了，代码已经上传我的GitHub。