七、状态更新
在完成了数据关联后,就确定了路标是新路标还是旧路标,最后就能进行状态更新了。这里同样有新landmark下的状态更新和旧landmark下的状态更新。
这里先介绍旧landmark下的状态更新,大致有下面几个步骤:
(1)利用观测模型计算每个landmark的观测矩阵
z
^
\hat{z}
z^以及对应的协方差矩阵
H
H
H(这一步骤和上一篇博客“从程序中学习EKF-SLAM(四)”中的一样,这里就不细说了),同时计算实际观测值与理想观测值的差值
z
ˉ
\bar{z}
zˉ:
z
ˉ
=
z
−
z
^
\bar{z}=z-\hat{z}
zˉ=z−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
=
Σ
‾
t
H
t
i
T
(
H
t
i
Σ
‾
t
H
t
i
T
+
Q
t
)
−
1
K_{t}^{i}={\overline\varSigma_t}{{H_{t}^{i}}^T}\left({ H_{t}^{i}\overline{\varSigma }_t}{{H_{t}^{i}}^T}+Q_t \right) ^{-1}
Kti=ΣtHtiT(HtiΣtHtiT+Qt)−1
(3)更新状态向量矩阵和协方差矩阵。
μ
‾
t
=
μ
‾
t
+
K
t
i
(
z
t
i
−
z
^
t
i
)
Σ
‾
t
=
(
I
−
K
t
i
H
t
i
)
Σ
‾
t
\overline{\mu }_t=\overline{\mu }_t+K_{t}^{i}\left( z_{t}^{i}-\widehat{z}_{t}^{i} \right) \\ \overline{\varSigma }_t=\left( I-K_{t}^{i}H_{t}^{i} \right) \overline{\varSigma }_t
μt=μt+Kti(zti−z
ti)Σt=(I−KtiHti)Σt
可以参照代码理解:
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变量就是计算
Σ
‾
t
H
t
i
T
{\overline\varSigma_t}{{H_{t}^{i}}^T}
ΣtHtiT,S变量就是计算
(
H
t
i
Σ
‾
t
H
t
i
T
+
Q
t
)
\left({ H_{t}^{i}\overline{\varSigma }_t}{{H_{t}^{i}}^T}+Q_t \right)
(HtiΣtHtiT+Qt);
(2)在求解S的逆矩阵之前,进行了单位化生成对角阵,这里为什么这样做呢?问过大佬后得知:虽然这样做改变了矩阵S的大小,但是能确保处理之后的矩阵S保持正定的性质,因为协方差只是表征状态向量的变化程度或者说区别,所有只要矩阵S的每个元素之间“关系”保持不变,S矩阵还能能够体现这个系统的运行状态的;
(3)需要计算变量S的逆矩阵,程序中采用Cholesky分解法计算三角阵来求矩阵的逆(注意S的逆矩阵应是变量乘以变量的转置),这样能加快求解速度;
另外一种情况,就是新路标情况下的状态更新了,这部分的重点就是变量的扩充。那么,他大致有如下步骤:
(1)将landmark的观测信息转换为地图坐标信息,这里的转换关系就不再啰嗦了;
(2)计算观测模型的雅可比矩阵
G
v
G_v
Gv,也就是观测模型对变量
(
x
,
y
,
θ
)
(x, y, \theta)
(x,y,θ)的导数;
∂
(
x
+
r
cos
(
θ
+
φ
)
y
+
r
sin
(
θ
+
φ
)
)
∂
(
x
,
y
,
θ
)
=
(
1
0
0
1
−
r
sin
(
θ
+
φ
)
r
cos
(
θ
+
φ
)
)
\frac{\partial \left( \begin{array}{c} x+r\cos \left( \theta +\varphi \right)\\ y+r\sin \left( \theta +\varphi \right)\\ \end{array} \right)}{\partial \left( x,y,\theta \right)}=\left( \begin{matrix} \begin{array}{c} 1\\ 0\\ \end{array}& \begin{array}{c} 0\\ 1\\ \end{array}& \begin{array}{c} -r\sin \left( \theta +\varphi \right)\\ r\cos \left( \theta +\varphi \right)\\ \end{array}\\ \end{matrix} \right)
∂(x,y,θ)∂(x+rcos(θ+φ)y+rsin(θ+φ))=(1001−rsin(θ+φ)rcos(θ+φ))
(3)计算观测模型的雅可比矩阵
G
z
G_z
Gz,也就是观测模型对观测变量
(
r
,
θ
)
(r, \theta)
(r,θ)的导数,用于计算观测的噪声;
∂
(
x
+
r
cos
(
θ
+
φ
)
y
+
r
sin
(
θ
+
φ
)
)
∂
(
r
,
φ
)
=
(
cos
(
θ
+
φ
)
sin
(
θ
+
φ
)
−
r
sin
(
θ
+
φ
)
r
cos
(
θ
+
φ
)
)
\frac{\partial \left( \begin{array}{c} x+r\cos \left( \theta +\varphi \right)\\ y+r\sin \left( \theta +\varphi \right)\\ \end{array} \right)}{\partial \left( r,\varphi \right)}=\left( \begin{matrix} \begin{array}{c} \cos \left( \theta +\varphi \right)\\ \sin \left( \theta +\varphi \right)\\ \end{array}& \begin{array}{c} -r\sin \left( \theta +\varphi \right)\\ r\cos \left( \theta +\varphi \right)\\ \end{array}\\ \end{matrix} \right)
∂(r,φ)∂(x+rcos(θ+φ)y+rsin(θ+φ))=(cos(θ+φ)sin(θ+φ)−rsin(θ+φ)rcos(θ+φ))
(4)更新状态变量和协方差。
l
a
n
d
m
a
r
k
:
P
t
i
=
G
v
∗
P
r
o
b
o
t
∗
G
v
T
+
G
z
∗
E
∗
G
z
T
landmark:P_{t}^{i}=G_v*P_{robot}*G_{v}^{T}+G_z*E*G_{z}^{T}
landmark:Pti=Gv∗Probot∗GvT+Gz∗E∗GzT
可以参考代码理解:
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。