[人工智能]hdl_localization代码解析

hdl_localization代码解析

简介

hdl_localization是基于UKF滤波框架，融合了ndt点云配准结果，在已经构建的点云地图上实习激光重定位的一种方法。在使用16线激光雷达进行机器人定位时，不用IMU也可以取得不错的效果。

安装依赖

依赖库

1.Ros-Melodic
2.Pcl-1.8
3.Open-MP
4.Eigen3.3(及以上)

依赖包

1.ndt_omp
2.fast-gicp
3.hdl_global_localization

运行

rosparam set use_sim_time true
roslaunch hdl_localization hdl_localization.launch

roscd hdl_localization/rviz
rviz -d hdl_localization.rviz

注意

1.如果进行纯定位时的初始位姿在地图坐标系附近，在launch文件中可以将 “specify_init_pose" 设为 ”true“，这样，其默认的三维位置（0，0，0）和默认的表示旋转的四元数（0，0，0，1）就可以很好的给予点云一个初始状态，有利于其后续匹配和重定位。
2.如果想在地图中任意位置进行重定位，需要在开启rviz -d hdl_localization.rviz后，选择rviz上方的2D pose estimator，并在地图中左键点击和鼠标拖动，选择一个与真实位置相近的位置与航向。

效果

UKF知识补充（无迹卡尔曼滤波）

网页链接
原始高斯分布经过非线性变换之后其实是一个不规则的非线性分布，在EKF中我们在高斯均值点附近用泰勒级数近似非线性分布，从而获得近似高斯分布。但是问题在于，这个近似高斯分布有时不是足够精确，单单一个均值点往往无法预测上图中这种很不规则的分布的。这个时候我们就需要无迹卡尔曼滤波UKF了，通过无迹转换将非线性函数变换的结果近似成高斯分布。
以下是无迹变换执行的步骤：

1.计算Sigma点集

2.为每个Sigma点分配权重

3.把所有单个Sigma点代入非线性函数f

4.对经过上述加权和转换的点近似新的高斯分布

5.计算新高斯分布的均值和方差。

代码阅读

总览

该项目是使用nodelet统一管理的，apps为定义的两个类，也就是程序实现。include内为状态估计器和无迹卡尔曼的实现。launch是启动文件。rviz内为rviz的配置文件。data为实例的定位用点云地图。

launch

定义了几个参数，使用nodelet运行了velodyne_nodelet_manager、globalmap_server_nodelet、hdl_localization_nodelet三个节点。如果只用于仿真，可以在 arguments 前面加上。

 <param name="use_sim_time" value="true"/>

apps(程序实现)

本文件夹是只有两个cpp文件，直接继承了nodelet的类。

globalmap_server_nodelet

类GlobalmapServerNodelet 继承了 nodelet::Nodelet。
关于ros，声明了三个句柄，1个发布，1个计时器，1个globalmap的变量。

ros::NodeHandle nh;
ros::NodeHandle mt_nh;
ros::NodeHandle private_nh;

ros::Publisher globalmap_pub;

ros::WallTimer globalmap_pub_timer;
pcl::PointCloud<PointT>::Ptr globalmap;

globalmap_server_nodelet::onInit()

这里是在重写了初始化函数。同时利用计时器出发回调函数。

  void onInit() override {
  //定义三个节点，
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    initialize_params();

    // publish globalmap with "latched" publisher
    globalmap_pub = nh.advertise<sensor_msgs::PointCloud2>("/globalmap", 5, true);
    globalmap_pub_timer = nh.createWallTimer(ros::WallDuration(0.05), &GlobalmapServerNodelet::pub_once_cb, this, true, true); //20Hz
  }

globalmap_server_nodelet::initialize_params()

在程序initialize_params()中，完成了读取地图pcd文件的功能，并对该地图进行下采样，最终的globalmap是下采样的地图。

void initialize_params() {
    // read globalmap from a pcd file
    std::string globalmap_pcd = private_nh.param<std::string>("globalmap_pcd", "");
    globalmap.reset(new pcl::PointCloud<PointT>());
    pcl::io::loadPCDFile(globalmap_pcd, *globalmap);
    globalmap->header.frame_id = "map";

	//TODO：这个实际上是没有到这里来的，初步想法是没有utm文件。类似于经纬度的坐标文件。
    std::ifstream utm_file(globalmap_pcd + ".utm");
    if (utm_file.is_open() && private_nh.param<bool>("convert_utm_to_local", true)) {
        std::cout << "now utf_file is open" << std::endl;
      double utm_easting;
      double utm_northing;
      double altitude;
      utm_file >> utm_easting >> utm_northing >> altitude;
      for(auto& pt : globalmap->points) {
        pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude);
      }
      ROS_INFO_STREAM("Global map offset by UTM reference coordinates (x = " 
                      << utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")");
    }
    //endTODO

    // downsample globalmap
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    voxelgrid->setInputCloud(globalmap);

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    voxelgrid->filter(*filtered);

    globalmap = filtered;
  }

同时，每隔0.05s发布一次。（onInit定义的）

  void pub_once_cb(const ros::WallTimerEvent& event) {
    globalmap_pub.publish(globalmap);
  }

hdl_localization_nodelet

类 HdlLocalizationNodelet 继承了 nodelet::Nodelet，先看初始化。

hdl_localization_nodelet::onInit()

  void onInit() override {
    //依然是三个句柄
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();
	//这里的时间用了boost库里的 circular_buffer<double>。感兴趣的可以自己百度一下，毕竟……我也没用过。
    processing_time.resize(16);
    //这些参数，又来了。
    initialize_params();
    
    //这个默认的base_link, launch里覆盖了。实际上是velodyne。参数类的在launch里改写了一部分，这里就不一一赘述了。可以自己对比来看，比较容易。
    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");
    //是否使用imu
    use_imu = private_nh.param<bool>("use_imu", true);
    //imu是否倒置
    invert_imu = private_nh.param<bool>("invert_imu", false); 
    if(use_imu) {//如果使用imu，则定义订阅函数。
      NODELET_INFO("enable imu-based prediction");
      imu_sub = mt_nh.subscribe("/gpsimu_driver/imu_data", 256, &HdlLocalizationNodelet::imu_callback, this);
    }
    //点云数据、全局地图、初始位姿的订阅。initialpose_sub只是用于rviz划点用的。
    points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);
	//发布里程计信息，以及对齐后的点云数据。
    pose_pub = nh.advertise<nav_msgs::Odometry>("/odom", 5, false);
    aligned_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 5, false);
  }

hdl_localization_nodelet::initialize_params()

初始化参数

  void initialize_params() {
    // intialize scan matching method
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");

    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;
	//定义了ndt和glcp
    pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
    pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());
	//ndt参数与搜索方法。默认DIRECT7，作者说效果不好可以尝试改为DIRECT1.
    ndt->setTransformationEpsilon(0.01);
    ndt->setResolution(ndt_resolution);
    if(ndt_neighbor_search_method == "DIRECT1") {
      NODELET_INFO("search_method DIRECT1 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      registration = ndt;
    } else if(ndt_neighbor_search_method == "DIRECT7") {
      NODELET_INFO("search_method DIRECT7 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      registration = ndt;
    } else if(ndt_neighbor_search_method == "GICP_OMP"){
      NODELET_INFO("search_method GICP_OMP is selected");
      registration = gicp;
    }
     else {
      if(ndt_neighbor_search_method == "KDTREE") {
        NODELET_INFO("search_method KDTREE is selected");
      } else {
        NODELET_WARN("invalid search method was given");
        NODELET_WARN("default method is selected (KDTREE)");
      }
      ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      registration = ndt;
    }
    
    // initialize pose estimator
    //设置起点用。
    if(private_nh.param<bool>("specify_init_pose", true)) {
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
        ros::Time::now(),
        Eigen::Vector3f(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
        Eigen::Quaternionf(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
        private_nh.param<double>("cool_time_duration", 0.5)
      ));
    }
  }

-----------下面就是回调函数的处理---------
实际使用的回调函数就是HdlLocalizationNodelet::imu_callback、HdlLocalizationNodelet::points_callback 以及 HdlLocalizationNodelet::globalmap_callback三个。分别订阅了 “/gpsimu_driver/imu_data”、“/velodyne_points” 以及 "/globalmap"两个话题。
首先看HdlLocalizationNodelet::globalmap_callback。完成了对全局地图的订阅以及从ros消息到点云的转化。

HdlLocalizationNodelet::globalmap_callback

  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;
	//发布出来的全局地图用作配准用的目标点云。这里就是globalmap_server_nodelet发出来的。
    registration->setInputTarget(globalmap);
  }

HdlLocalizationNodelet::imu_callback

接收imu并扔到imu_data里去。会在HdlLocalizationNodelet::points_callback里用到。

  void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {
    std::lock_guard<std::mutex> lock(imu_data_mutex);
    imu_data.push_back(imu_msg);
  }

HdlLocalizationNodelet::points_callback

输入点云输出位姿。

  void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
  	//加锁
    std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);
    if(!pose_estimator) {//等待位姿估计器初始化
      NODELET_ERROR("waiting for initial pose input!!");
      return;
    }

    if(!globalmap) {//等待全局地图
      NODELET_ERROR("globalmap has not been received!!");
      return;
    }
	//将ros消息转化为点云
    const auto& stamp = points_msg->header.stamp;
    pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *pcl_cloud);
	//检查
    if(pcl_cloud->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }
	//将点云转换到odom_child_frame_id 坐标系。
	//但是这个tf是自己发的，这一个还要再看一下。TODO。鸡生蛋蛋生鸡的问题一直搞不太懂。
    // transform pointcloud into odom_child_frame_id  
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  
    if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) {
        NODELET_ERROR("point cloud cannot be transformed into target frame!!");
        return;
    } 
	//对点云下采样。这里用到的是同一文件下的函数。后面会放上。
    auto filtered = downsample(cloud);

    // predict
    if(!use_imu) {//不用imu则用0。
      pose_estimator->predict(stamp, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero());
    } else {
      std::lock_guard<std::mutex> lock(imu_data_mutex);
      auto imu_iter = imu_data.begin();
      //利用imu数据迭代。
      for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
        //若当前点云时间早于imu雷达，则跳出。imu做预测，点云观测。
        if(stamp < (*imu_iter)->header.stamp) {
          break;
        }
        //读取线加速度和角速度。判断是否倒置imu
        const auto& acc = (*imu_iter)->linear_acceleration;
        const auto& gyro = (*imu_iter)->angular_velocity;
        double gyro_sign = invert_imu ? -1.0 : 1.0;
        //利用imu数据做位姿的预测。这里用了pose_estimator→predict,进一步调用了ukf进行估计。还没具体看ukf。 TODO
        pose_estimator->predict((*imu_iter)->header.stamp, Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z));
      }
      //删除用过的imu数据
      imu_data.erase(imu_data.begin(), imu_iter);
    }

    // correct
    auto t1 = ros::WallTime::now();
    //用pose_estimator 来矫正点云。pcl库配准。获取到结果后利用ukf矫正位姿。
    auto aligned = pose_estimator->correct(filtered);
    auto t2 = ros::WallTime::now();

    processing_time.push_back((t2 - t1).toSec());
    double avg_processing_time = std::accumulate(processing_time.begin(), processing_time.end(), 0.0) / processing_time.size();
    // NODELET_INFO_STREAM("processing_time: " << avg_processing_time * 1000.0 << "[msec]");
    
	//如果有订阅才发布
    if(aligned_pub.getNumSubscribers()) {
      aligned->header.frame_id = "map";
      aligned->header.stamp = cloud->header.stamp;
      aligned_pub.publish(aligned);
    }
	//发布里程计。时间戳为当前帧雷达时间，里程计位姿为ukf校正后位姿。同时也会发布从map到odom_child_frame_id的tf
    publish_odometry(points_msg->header.stamp, pose_estimator->matrix());
  }

----------主要流程到此结束，下面是其他的一些功能函数-----------

downsample(const pcl::PointCloud::ConstPtr& cloud)

当前帧点云数据下采样

  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
  	//在函数initialize_params()里声明了。0.1，0.1，0.1网格
    if(!downsample_filter) {
      return cloud;
    }
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;
    return filtered;
  }

publish_odometry

发布里程计的tf和msg。输入为当前帧点云时间戳与pose_estimator的结果矩阵。这里还用到了matrix2transform这个函数，用于做eigen矩阵到tf的转化（取值）。

  void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
    // broadcast the transform over tf
    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
    pose_broadcaster.sendTransform(odom_trans);

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "map";

    odom.pose.pose.position.x = pose(0, 3);
    odom.pose.pose.position.y = pose(1, 3);
    odom.pose.pose.position.z = pose(2, 3);
    odom.pose.pose.orientation = odom_trans.transform.rotation;

    odom.child_frame_id = odom_child_frame_id;
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

matrix2transform

matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) 从matrix 到 geometry_msgs::TransformStamped。

  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
    Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
    quat.normalize();
    geometry_msgs::Quaternion odom_quat;
    odom_quat.w = quat.w();
    odom_quat.x = quat.x();
    odom_quat.y = quat.y();
    odom_quat.z = quat.z();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = pose(0, 3);
    odom_trans.transform.translation.y = pose(1, 3);
    odom_trans.transform.translation.z = pose(2, 3);
    odom_trans.transform.rotation = odom_quat;

    return odom_trans;
  }

include(状态估计器及ukf)

apps里的两个cpp大致内容均为以上。可以看到在points_callback里使用了pose_estimator作为位姿的估计。而该类又使用了ukf作为位姿的解算。二者的实现都在include文件夹内。

hdl_localization/pose_estimator.hpp

该文件定义了类PoseEstimator。

PoseEstimator（构造函数）

首先看构造函数。可以看到在初始化之后，最重要的是进入了ukf的处理。

  PoseEstimator(pcl::Registration<PointT, PointT>::Ptr& registration, const ros::Time& stamp, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration = 1.0)
    : init_stamp(stamp),
      registration(registration),
      cool_time_duration(cool_time_duration)
  {
  	//单位阵初始化，随后给过程噪声。
    process_noise = Eigen::MatrixXf::Identity(16, 16);
    process_noise.middleRows(0, 3) *= 1.0;
    process_noise.middleRows(3, 3) *= 1.0;
    process_noise.middleRows(6, 4) *= 0.5;
    process_noise.middleRows(10, 3) *= 1e-6;
    process_noise.middleRows(13, 3) *= 1e-6;
	//测量噪声，单位阵
    Eigen::MatrixXf measurement_noise = Eigen::MatrixXf::Identity(7, 7);
    measurement_noise.middleRows(0, 3) *= 0.01;
    measurement_noise.middleRows(3, 4) *= 0.001;
	//加权平均的位姿。
    Eigen::VectorXf mean(16);
    mean.middleRows(0, 3) = pos;
    mean.middleRows(3, 3).setZero();
    mean.middleRows(6, 4) = Eigen::Vector4f(quat.w(), quat.x(), quat.y(), quat.z());
    mean.middleRows(10, 3).setZero();
    mean.middleRows(13, 3).setZero();
	//初始化协方差
    Eigen::MatrixXf cov = Eigen::MatrixXf::Identity(16, 16) * 0.01;
    
	//声明posesystem。
    PoseSystem system;
    //初始化ukf
    ukf.reset(new kkl::alg::UnscentedKalmanFilterX<float, PoseSystem>(system, 16, 6, 7, process_noise, measurement_noise, mean, cov));
  }

pose_estimator->predict（预测）

另外在hdl_localization.cpp中用到的pose_estimator->predict等也在本文件进行了解释。

  void predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) {
  	//当前与初始化的时间间隔小于设置的时间，或prev_stamp（上次更新时间）为0（未更新），或prev_stamp等于当前时间。则更新prev_stamp并跳出。
    if((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) {
      prev_stamp = stamp;
      return;
    }
	//正常处理，首先计算dt，更新prev_stamp。
    double dt = (stamp - prev_stamp).toSec();
    prev_stamp = stamp;
	//对ukf设置噪声和处理间隔。
    ukf->setProcessNoiseCov(process_noise * dt);
    ukf->system.dt = dt;
	//利用imu数据定义控制量
    Eigen::VectorXf control(6);
    control.head<3>() = acc;
    control.tail<3>() = gyro;
	//利用ukf预测。
    ukf->predict(control);
  }

pose_estimator->correct(观测)

  pcl::PointCloud<PointT>::Ptr correct(const pcl::PointCloud<PointT>::ConstPtr& cloud) {
  	//单位阵来初始化
    Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();
    init_guess.block<3, 3>(0, 0) = quat().toRotationMatrix();
    init_guess.block<3, 1>(0, 3) = pos();
	//点云的配准。ndt
    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    registration->setInputSource(cloud);
    registration->align(*aligned, init_guess);
	//读取数据
    Eigen::Matrix4f trans = registration->getFinalTransformation();
    Eigen::Vector3f p = trans.block<3, 1>(0, 3);
    Eigen::Quaternionf q(trans.block<3, 3>(0, 0));

    if(quat().coeffs().dot(q.coeffs()) < 0.0f) {
      q.coeffs() *= -1.0f;
    }
	//填充至观测矩阵observation
    Eigen::VectorXf observation(7);
    observation.middleRows(0, 3) = p;
    observation.middleRows(3, 4) = Eigen::Vector4f(q.w(), q.x(), q.y(), q.z());
	//ukf更新
    ukf->correct(observation);
    return aligned;
  }

----------还有一些简单的函数不再说明了。直接怼上，很好理解。-----------

  /* getters */
  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;
  }

hdl_localization/pose_system.hpp

本文件定义了完成了类PoseSystem的实现。主要是实现了ukf里 矩阵f（定义了系统）和h（观测）代码实现。这是要扔到ukf中去的。
系统状态量16位，分别是位姿（3）、速度（3）、四元数（4）、加速度偏差（3）、陀螺仪偏差（3）。另还有6位控制量，加速度（3）和陀螺仪（3）。

f（系统状态方程）

  VectorXt f(const VectorXt& state, const VectorXt& control) const {
    VectorXt next_state(16);

    Vector3t pt = state.middleRows(0, 3);  //位置
    Vector3t vt = state.middleRows(3, 3);  //速度
    Quaterniont qt(state[6], state[7], state[8], state[9]);
    qt.normalize(); // 归一化四元数

    Vector3t acc_bias = state.middleRows(10, 3);  //加速度偏差
    Vector3t gyro_bias = state.middleRows(13, 3); //陀螺仪偏差

    Vector3t raw_acc = control.middleRows(0, 3);  //加速度控制
    Vector3t raw_gyro = control.middleRows(3, 3);  //陀螺仪控制
	
	//下一时刻状态
    // position 。 首先更新位置
    next_state.middleRows(0, 3) = pt + vt * dt;					//

    // velocity。 更新速度，实际上并没有利用加速度矫正速度，原因是认为加速度噪声较大，对最终的精度并没有贡献。
    Vector3t g(0.0f, 0.0f, -9.80665f);
    Vector3t acc_ = raw_acc - acc_bias;
    Vector3t acc = qt * acc_;
    next_state.middleRows(3, 3) = vt; // + (acc - g) * dt;		// acceleration didn't contribute to accuracy due to large noise

    // orientation。首先完成了陀螺仪的增量计算并归一化（直接转化为四元数形式），将其转换为下一时刻的四元数。
    Vector3t gyro = raw_gyro - gyro_bias;
    Quaterniont dq(1, gyro[0] * dt / 2, gyro[1] * dt / 2, gyro[2] * dt / 2);
    dq.normalize();
    Quaterniont qt_ = (qt * dq).normalized();
    next_state.middleRows(6, 4) << qt_.w(), qt_.x(), qt_.y(), qt_.z();
	//将当前控制量传入下一时刻的状态向量。认为加速度和角速度上偏差不变
    next_state.middleRows(10, 3) = state.middleRows(10, 3);		// constant bias on acceleration
    next_state.middleRows(13, 3) = state.middleRows(13, 3);		// constant bias on angular velocity

    return next_state;
  }

h （观测方程）

观测方程直接将当前输入状态量作为观测量。这里的输入是在更新阶段（correct）生成的带误差方差的（error variances）的扩展状态空间下的（extended state space）状态量，也就是ext_sigma_points。

  // observation equation
  VectorXt h(const VectorXt& state) const {
    VectorXt observation(7);
    observation.middleRows(0, 3) = state.middleRows(0, 3);
    observation.middleRows(3, 4) = state.middleRows(6, 4).normalized();

    return observation;
  }

kkl/unscented_kalman_filter.hpp

本文件中主要的函数也就构造函数、预测、矫正、计算sigma点、使协方差矩阵正有限（不太清楚）五个。

UnscentedKalmanFilterX2

首先，构造函数。可以看到输入了一系列包括待估计系统、状态向量维度、输入维度、观测维度、两个噪声、参数等等。完成了初始化操作。

  UnscentedKalmanFilterX(const System& system, int state_dim, int input_dim, int measurement_dim, const MatrixXt& process_noise, const MatrixXt& measurement_noise, const VectorXt& mean, const MatrixXt& cov)
    : state_dim(state_dim),
    input_dim(input_dim),
    measurement_dim(measurement_dim),
    N(state_dim),
    M(input_dim),
    K(measurement_dim),
    S(2 * state_dim + 1),
    mean(mean),
    cov(cov),
    system(system),
    process_noise(process_noise),
    measurement_noise(measurement_noise),
    lambda(1),
    normal_dist(0.0, 1.0)
  {
  	//设置长度。
    weights.resize(S, 1);
    sigma_points.resize(S, N);
    ext_weights.resize(2 * (N + K) + 1, 1);
    ext_sigma_points.resize(2 * (N + K) + 1, N + K);
    expected_measurements.resize(2 * (N + K) + 1, K);

    // initialize weights for unscented filter
    weights[0] = lambda / (N + lambda);
    for (int i = 1; i < 2 * N + 1; i++) {
      weights[i] = 1 / (2 * (N + lambda));
    }

    // weights for extended state space which includes error variances
    ext_weights[0] = lambda / (N + K + lambda);
    for (int i = 1; i < 2 * (N + K) + 1; i++) {
      ext_weights[i] = 1 / (2 * (N + K + lambda));
    }
  }

ukf->predict

通过pose_estimator->predict调用。

  void predict(const VectorXt& control) {

    // calculate sigma points. ukf的sigma点
    ensurePositiveFinite(cov);
    computeSigmaPoints(mean, cov, sigma_points);
    //sigma_points更新。用在posesystem中定义的f函数来进行。
    for (int i = 0; i < S; i++) {
      sigma_points.row(i) = system.f(sigma_points.row(i), control);
    }
    
	/*----至此，sigma_points里存储的就是当前时刻的由ukf输出的系统状态。-----*/
	
	//过程噪声，即ukf中的矩阵R
    const auto& R = process_noise;
    
    // unscented transform。定义当前的平均状态和协方差矩阵，并设置为0矩阵。
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());
    mean_pred.setZero();
    cov_pred.setZero();
    //加权平均，预测状态
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * sigma_points.row(i);
    }
    //根据状态预测协方差。
    for (int i = 0; i < S; i++) {
      VectorXt diff = sigma_points.row(i).transpose() - mean_pred;
      cov_pred += weights[i] * diff * diff.transpose();
    }
    //附加过程噪声R，在pose_estimator中给出初值
    cov_pred += R;
	//更新mean和cov
    mean = mean_pred;
    cov = cov_pred;
  }

ukf->correct

通过pose_estimator->correct调用。

  void correct(const VectorXt& measurement) {
  	//N-状态方程维度。K-观测维度
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K);
    //左上角N行1列
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    //左上角N行N列
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    //右下角K行K列。初始化为在pose_estimator输入的噪声。位置噪声0.01，四元数0.001
    ext_cov_pred.bottomRightCorner(K, K) = measurement_noise;
    
  /*---------------- 经过以上操作，现在扩展状态变量前N项为mean，扩展协方差左上角为N*N的cov，右下角为K*K的观测噪声--------------*/
  
	//验证并计算
    ensurePositiveFinite(ext_cov_pred);
    //利用扩展状态空间的参数计算sigma点
    computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points);

    // unscented transform
    //这里使用了 ukf 的h 函数来计算观测。
    //ext_sigma_points、expected_measurements是（2 * (N + K) + 1, K)的矩阵
    //没太看明白 TODO
    //取左上角前N个量，加上右下角K个量。
    expected_measurements.setZero();
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1));
      expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(K, 1));
    }
    
	//加权平均。同predict函数相似。
    VectorXt expected_measurement_mean = VectorXt::Zero(K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean;
      expected_measurement_cov += ext_weights[i] * diff * diff.transpose();
    }

    // calculated transformed covariance
    //转换方差。用于计算sigama，进而计算卡尔曼增益
    MatrixXt sigma = MatrixXt::Zero(N + K, K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred);
      auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean);
      sigma += ext_weights[i] * (diffA * diffB.transpose());
    }

    kalman_gain = sigma * expected_measurement_cov.inverse();
    const auto& K = kalman_gain;

	//更新最后的ukf。
    VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean);
    MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpose();

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

computeSigmaPoints

通过mean和cov计算sigma点。思路是将cov做Cholesky分解，用下三角矩阵L对mean做处理。得到一系列sigma_points.

  void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
    const int n = mean.size();
    assert(cov.rows() == n && cov.cols() == n);

	//llt分解。求Cholesky分解A=LL^*=U^*U。L是下三角矩阵
    Eigen::LLT<MatrixXt> llt;
    llt.compute((n + lambda) * cov);
    MatrixXt l = llt.matrixL();
    
    //mean是列向量。这里会自动转置处理。
    sigma_points.row(0) = mean;
    for (int i = 0; i < n; i++) {
      sigma_points.row(1 + i * 2) = mean + l.col(i); //奇数1357
      sigma_points.row(1 + i * 2 + 1) = mean - l.col(i); //偶数2468
    }
  }

ensurePositiveFinite

保证协方差的正有限。未实际应用。

  void ensurePositiveFinite(MatrixXt& cov) {
    return;
    //就到这里了，在上面就return掉了。
    const double eps = 1e-9;

    Eigen::EigenSolver<MatrixXt> solver(cov);
    MatrixXt D = solver.pseudoEigenvalueMatrix(); //特征值
    MatrixXt V = solver.pseudoEigenvectors(); //特征向量
    for (int i = 0; i < D.rows(); i++) {
      if (D(i, i) < eps) {
        D(i, i) = eps;
      }
    }
    cov = V * D * V.inverse();
  }

开发者涨薪指南

48位大咖的思考法则、工作方式、逻辑体系