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

接下来具体说说UKF中的状态预测部分，将会从原理和代码两个方面来进行说明。

四、状态预测

1.UKF概述

概括起来说，就是指解决一个高斯分布经过非线性变换后，怎么用另一个高斯分布近似它。假设$x=N\left({\mu }_x,{\Sigma }_{xx}\right),y=g\left(x\right)$，我们希望用$N({\mu }_y,{\Sigma }_{yy})$近似$y$。按照EKF，需要对$g$做线性化。但在UKF里，不必做这个线性化。（摘自高翔博士的博客）

在UKF中，做的是sigma点采样，然后利用状态模型投影回去，形成了一个高斯分布。

2.UKF计算的前提

在UKF-SLAM中，机器人的运动模型和观测模型（具体模型在上一篇博客中说过）依旧为：
$$\begin{gathered} X_k^v=f_v\left(X_{k-1}^v,u_k\right)+w_k \\ {Z}_k=h\left(X_k\right)+v_k \end{gathered}$$
其中，$w_k$和$v_k$为服从$N(0,Q_K)$和$N(0,R_K)$的高斯分布；$X_k^v=(x,y,\theta )$为机器人状态向量，$Z_k=(R,\phi )$为机器人观测向量(project中是指激光测量信息)。另外，project中会将机器人位姿（也即是状态向量）和路标信息放在一起，形成增广状态向量:
$$X_k=x,y,\theta ,x_1,y_1,x_2,y_2,......,x_k,y_k$$
他就是project中的全局变量——Mu_x。
同时，还有该状态向量的协方差矩阵：
$$P_k=\left[\begin{array}{ccc}{P}_k^v& 0& 0\\ 0& P_k^1& ...\\ 0& ...& ...\end{array}\right]$$
他就是project中的全局变量——Sigma_y。

那么，在得到了观测信息之后，就会进行相应的预测和更新操作，看代码：

void unscented_kf::ProcessMeasurement(const Record& record)
{
	Prediction(record.odom); 
	Correction(record.radar);
}

3.sigma点计算

首先，在得到了$k-1$时刻的状态向量$X_{k-1}$和协方差$P_{k-1}$，那么我们需要取2n+1个sigma点$\xi$。如果这里选了三个点，那就是$X_{k-1},X_{k-1}+{\sigma }_0,X_{k-1}-{\sigma }_0$。

取点的要求是这些点要位于均值处及对称分布于主轴的协方差处（每维两个），如果状态向量是n维的话，那么需要根据如下规则进行选择：
$$\{\begin{array}{c}{\xi }_0=X_{k-1}\\ {\xi }_i=X_{k-1}+\sqrt{\left(n+\lambda \right)P_i}i=1,2,...,n\\ {\xi }_i=X_{k-1}-\sqrt{\left(n+\lambda \right)P_i}i=n+1,n+2,...,2n\end{array}$$
其中，$\lambda ={\alpha }^2\left(n+\kappa \right)-n$，$\alpha$和$\kappa$是确定sigma点分布在均值多远的范围内的比例参数。这里取$\lambda =3-n$。
在project中单独有一个函数用于计算sigma点，代码如下：

void unscented_kf::compute_sigma_points(Eigen::MatrixXd& sigma_points, bool pred)
{
	int n  = Mu_x.rows();
	if(pred) n = 3;
	int num_sig = 2 * n + 1;
	float lambda = Scale - n;

	//get the square root of matrix sigma
	Eigen::MatrixXd D = Sigma_y.topLeftCorner(n, n).llt().matrixL();

	sigma_points = Eigen::MatrixXd::Zero(n, 2 * n + 1);
	sigma_points.col(0) = Mu_x.head(n);

	for (int i = 0; i < n; i++) 
	{
		sigma_points.col(i + 1)    	=   sigma_points.col(0) + sqrt(n + lambda) *  D.col(i);
		sigma_points.col(i + n + 1) 	=   sigma_points.col(0) - sqrt(n + lambda) *  D.col(i);
	}
}

这里需要注意的有：
（1）因为在UKF的预测和更新环节都需要计算sigma点，所在这里将两个计算合并了。用函数形参pred区分哪个阶段，若为true，则是进行预测阶段的sigma点计算，默认为false，进行更新环节的sigma点计算；
（2）计算协方差矩阵的均方根时，采用了eigen矩阵库中的llt分解方法计算下三角阵。如果有不懂的，可以看如下征明：

4.sigma点的权值计算

这里有两个权值，一个权值$W_m^i$用于计算状态向量的均值，$W_c^i$用于计算状态向量的协方差。它们的计算公式如下：
$$\{\begin{array}{c}{W}_m^0=\frac{\lambda }{n+\lambda }\\ W_c^0=\frac{\lambda }{n+\lambda }+\left(1-{\alpha }^2+\beta \right)\\ W_m^i=W_c^i=\frac{1}{2\left(n+\lambda \right)}i=1,2,...,2n\end{array}$$
其中，选择参数$\beta$是对高斯表示的附加的（较高阶）分布信息进行编码。如果分布是精确的高斯分布，则$\beta =2$是最佳选择。在project中，取$\alpha =1,\beta =0$。

4.状态预测计算

在取得sigma点和计算好了权重之后，就可以进行状态预测了。计算公式如下：
$$\{\begin{array}{c}{X}_k={\sum }_{i=0}^{2n}{W}_m^i{\xi }_i\\ P_k={\sum }_{i=0}^{2n}{W}_c^i\left({\xi }_i-X_k^i\right){\left({\xi }_i-X_k^i\right)}^T\end{array}$$
project相应的子函数如下：

void unscented_kf::recover_mu_sigma(const Eigen::MatrixXd& sig_pts)
{
	int  n = sig_pts.rows();
	float lambda = Scale - n;

	//weight vector
	Eigen::VectorXd weights = Eigen::VectorXd::Zero(2*n + 1);
	weights(0) = lambda / Scale;
	for (int i = 1; i < 2 * n + 1; i++) 
		weights(i) = 0.5 / Scale;
	
	Eigen::RowVectorXd angle_c = sig_pts.row(2).array().cos();
	Eigen::RowVectorXd angle_s = sig_pts.row(2).array().sin();

	double x_bar = angle_c * weights;
	double y_bar = angle_s * weights;

	//remove mu
	Mu_x.head(n) = (sig_pts * weights);
	double angle = atan2(y_bar, x_bar);
	Mu_x(2) = tool::normalize_angle(angle);

	//remove sigma
	Eigen::MatrixXd dsigma  = Eigen::MatrixXd::Zero(n, n);
	for (int i = 0; i < 2 * n + 1; i++) 
	{
		Eigen::VectorXd diff = sig_pts.col(i) - Mu_x.head(n);
		diff(2) = tool::normalize_angle(diff(2));
		dsigma = dsigma + weights(i) * diff * diff.transpose();
	}
	Sigma_y.topLeftCorner(n, n) = dsigma;
}

这里需要注意的有：
（1）第一次的角度计算，并不是采用公式里的加权计算，因为角度加权计算没有意义，所以采用求反正切的方法来求解；
（2）第二次的角度归一化，为了保存机器人航向角在$[-\pi ,+\pi ]$范围内，所以进行了角度归一化操作。

5.小结

这里再次回顾一下，UKF的状态预测其实可以按照三个部分来理解：

a. 计算sigma点
b.对sigma点进行模型计算
c.状态更新

对应到UKF的状态预测函数如下：

void unscented_kf::Prediction(const Eigen::Vector3f& motion)
{
	Eigen::MatrixXd Xsig;

	compute_sigma_points(Xsig, true);     // Xsig    3  X    (2*3 +1) 
	//dimensionality
	double r1    	= motion(0);
	double t    	= motion(1);
	double r2    	= motion(2);

	for (int i = 0; i < Xsig.cols(); i++)
	{
		double angle = Xsig(2,i);
		Xsig(0, i) = Xsig(0, i) + t * cos(angle + r1);
		Xsig(1, i) = Xsig(1, i) + t * sin(angle + r1);
		Xsig(2, i) = Xsig(2, i) + r1 + r2;
	}

	recover_mu_sigma(Xsig);
	Sigma_y.topLeftCorner(3,3)  = Sigma_y.topLeftCorner(3,3) + R_;
}

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