HDL-localization源码阅读
最近在回顾⾃⼰的⼈⽣（误），⾄少也是⼀个阶段性的总结，发现定位这⼀块虽然也做过⼀点但是⼀直没钻得很深。对于低速运动来说，将demo进⾏魔改后在95%的情况下也够⽤，另外的额外选择就是加⼊视觉定位，即每⼀次建图或者定位都将第⼀帧的坐标系转移到地图坐标系下，重新做⼀次视觉slam，⽤视觉⾥程计加⼊robot-po-ekf⾥⾯进⾏估计。不过，之前的技术⽅案是针对三维激光雷达做的，在更常见的⼆维激光定位中，amcl是⽤得最多的⽅法，粒⼦滤波可以最⼤程度地拟合状态量的概率分布，这也衍⽣出3d-mcl的多线雷达的定位⽅法。
对于我的16线雷达，我使⽤的是hdl-localization，它⽤的是ndt匹配加上UKF的数据融合，这不禁想到了MSCKF系列中的视觉约束与速度先验的融合，以及EKF-SLAM中⼆维码路标和轮式⾥程计的校验，其实在滤波框架下都是殊途同归的。它的新颖之处在于后端数据融合使⽤的UKF，与EKF相⽐较，UKF更好地拟合⾮线性的概率分布，⽽不是强⾏进⾏线性化，EKF由于要计算雅可⽐矩阵，更烧脑⼀些，UKF要对协⽅差矩阵做开⽅，也需要⽤到矩阵分解，两者计算量差不多。当然，如果⼀定要⽤EKF应该也能解决如今的低速定位问题。
在hdl中使⽤的ndt就像我们熟悉的pcl接⼝⼀样，只不过调⽤了多线程omp模式，它提供的是测量值。imu提供的是先验值，我们如果选择不⽤imu则先验值为原有的位姿。
参与融合的状态向量依然是p、v、q、ba、bg，⼀共16维，观测量则是7维，即p、q。和其它KF的接⼝⼀样，这⾥提供了predict和correct两个接⼝：
1.更新预测
control是传⼊的⼀帧imu数据，我们姑且看作是控制量，预测时⾸先判断协⽅差矩阵是否是⽅阵并且是否正定，因为我们在求sigma点的过程中要求协⽅差矩阵的逆，因此提取出它们的特征值和特征向量并再次相乘。接下来就是求解sigma点了：
void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
const int n = mean.size();
ws() == n && ls() == n);
//⽤llt分解求协⽅差的逆矩阵
Eigen::LLT<MatrixXt> llt;
MatrixXt l = llt.matrixL();
w(0) = mean;
for (int i = 0; i < n; i++) {
w(1 + i * 2) = mean + l.col(i);夹子
w(1 + i * 2 + 1) = mean - l.col(i);
}
}
在计算到所有的sigma点之后，⽤这些点重新拟合⼀个正态分布，在这个过程中引⼊了权重weights，它的初始赋值在构造函数中：
weights[0] = lambda / (N + lambda);flowergarden
生活大爆炸第三季for (int i = 1; i < 2 * N + 1; i++) {
weights[i] = 1 / (2 * (N + lambda));
}
void predict(const VectorXt& control)
{
// calculate sigma points
ensurePositiveFinite(cov);
computeSigmaPoints(mean, cov, sigma_points);
for (int i = 0; i < S; i++) {
//状态转移⽅程
w(i) = system.f(w(i), control);
}
const auto& R = process_noi;
// unscented transform
VectorXt mean_pred(mean.size());
MatrixXt cov_ws(), ls());
mean_pred.tZero();
cov_pred.tZero();
for (int i = 0; i < S; i++) {
mean_pred += weights[i] * w(i);
}
for (int i = 0; i < S; i++) {
VectorXt diff = w(i).transpo() - mean_pred;
cov_pred += weights[i] * diff * anspo();
}
cov_pred += R;
mean = mean_pred;
cov = cov_pred;
}
在每帧imu传⼊后都进⾏⼀次预测更新，在观测矫正之前我们得到了sigma点拟合的状态分布，⽽⾮状态转换矩阵，很像粒⼦滤波。
2.观测值估计真值
correct函数是观测矫正的过程，我们先拟合观测的sigma点，再⽤观测⽅程求解观测的均值，并加权计算扩增后的预测状态以及观测状态的协⽅差矩阵。
void correct(const VectorXt& measurement)
{
/
/ create extended state space which includes error variances
//状态扩增，即先更新预测值的协⽅差矩阵，将上⼀部分⽤sigma点拟合的协⽅差加上测量噪声
VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1);
MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K);
boatman
ext_pLeftCorner(N, 1) = VectorXt(mean);
ext_pLeftCorner(N, N) = MatrixXt(cov);
ext_cov_pred.bottomRightCorner(K, K) = measurement_noi;
//拟合状态扩增后的均值与协⽅差，也就是再算⼀遍sigma点
ensurePositiveFinite(ext_cov_pred);
computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points);
// unscented transform
/
/UT变换，即拟合测量的均值和协⽅差
expected_measurements.tZero();
for (int i = 0; i < ext_ws(); i++) {
w(i) = system.h(ext_w(i).transpo().topLeftCorner(N, 1));
w(i) += VectorXt(ext_w(i).transpo().bottomRightCorner(K, 1)); }
//虽然叫expected，但这是7维的测量值，也就是⽤sigma点拟合的测量分布！
VectorXt expected_measurement_mean = VectorXt::Zero(K);
for (int i = 0; i < ext_ws(); i++) {
expected_measurement_mean += ext_weights[i] * w(i);intelligent design
}英孚英语价格
//测量的协⽅差矩阵，即Pk+1
MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K);
for (int i = 0; i < ext_ws(); i++) {
VectorXt diff = w(i).transpo() - expected_measurement_mean;
expected_measurement_cov += ext_weights[i] * diff * anspo();
}
// calculated transformed covariance四年级上册教学计划
//那么，在这⾥的23*7的sigma矩阵就是状态扩增后的Pk|k+1
MatrixXt sigma = MatrixXt::Zero(N + K, K);
for (int i = 0; i < ext_ws(); i++) {
auto diffA = (ext_w(i).transpo() - ext_mean_pred);
auto diffB = (w(i).transpo() - expected_measurement_mean);
sigma += ext_weights[i] * (diffA * anspo());
}
//计算卡尔曼增益K=Pk|k+1/Pk
kalman_gain = sigma * expected_measurement_cov.inver();
const auto& K = kalman_gain;
//更新观测后的真值估计
VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean);
MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpo();
sunx
mean = pLeftCorner(N, 1);
cov = pLeftCorner(N, N);
loss}
最后简单总结⼀下UKF的过程：
1.产⽣2n+1个sigma点，在状态向量的附近，有点像粒⼦滤波。
2.利⽤状态转移矩阵，预测观测量的sigma点，并根据权重计算状态向量的均值和协⽅差矩阵。
岷山的意思
3.利⽤测量矩阵得到sigma点的预测测量值。
4.根据sigma点和权重得到状态向量的预测值。
5.根据KF公式，将状态向量的预测值和观测值进⾏真值估计。
因此hdl的逻辑就很清晰地显⽰出来了，在获取到估计值后，便将状态向量的前10维，也就是pvq取出得到位姿的估计值，实现了定位的迭代。
Eigen::Vector3f pos() const {
return Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]);
}
Eigen::Vector3f vel() const {
return Eigen::Vector3f(ukf->mean[3], ukf->mean[4], ukf->mean[5]);
}
Eigen::Quaternionf quat() const {
return Eigen::Quaternionf(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]).normalized();
}
Eigen::Matrix4f matrix() const {
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
m.block<3, 3>(0, 0) = quat().toRotationMatrix();
m.block<3, 1>(0, 3) = pos();
return m;
}
