多传感器融合定位

多传感器融合定位 课程概述
2020-10-10 21:04:27
多传感器融合定位（1）1-课程概述
```
多传感器融合定位（1）1-课程概述
```
收起
展开全文
SensorFusion:深蓝学院-多传感器融合定位-第二期-源码
2021-03-26 02:00:47
记录深蓝学院《多传感器融合定位与建图》课程的作业
收起
C++

在本地运行多传感器融合定位模块

2019-09-10 14:26:44

如何在本地运行多传感器融合定位模块定位技术横跨好几个专业，包括测绘、导航、计算机视觉知识、以及点云处理的知识。业界所说的“多传感器融合”，都是指对摄像头、激光雷达、毫米波雷达、超声波雷达等多种...

如何在本地运行多传感器融合定位模块

定位技术横跨好几个专业，包括测绘、导航、计算机视觉知识、以及点云处理的知识。业界所说的“多传感器融合”，都是指对摄像头、激光雷达、毫米波雷达、超声波雷达等多种传感器各自分别收集到的数据所做的数据融合。

定位模块依赖的硬件以及数据，包括惯性测量单元 IMU、车端天线、基站、LiDAR、以及定位地图；

GNSS定位以及激光点云定位模块，GNSS定位（基站和车端天线辅助）输出位置及速度信息，点云定位（LiDAR和定位地图）输出位置及航向角信息；

融合框架：惯性导航解算、Kalman滤波（卡尔曼滤波器/)是核心模块）；融合定位输出是一个6-dof的位置和姿态，以及协方差矩阵，其结果会反过来用于GNSS定位和点云定位的预测。

配置定位模块

为了使定位模块正确运行，需要对地图路径和传感器外参进行配置。假设下载的定位数据的所在路径为DATA_PATH。在进行以下步骤前，首先确定你在docker容器中。

配置传感器外参

将定位数据中的传感器外参拷贝至指定文件夹下。

cp DATA_PATH/params/ant_imu_leverarm.yaml /apollo/modules/localization/msf/params/gnss_params/  
cp DATA_PATH/params/velodyne64_novatel_extrinsics_example.yaml /apollo/modules/localization/msf/params/velodyne_params/  
cp DATA_PATH/params/velodyne64_height.yaml /apollo/modules/localization/msf/params/velodyne_params/

各个外参的意义：

ant_imu_leverarm.yaml：杆臂值参数，GNSS天线相对Imu的距离

velodyne64_novatel_extrinsics_example.yaml：Lidar相对Imu的外参

velodyne64_height.yaml： Lidar相对地面的高度

设置地图路径

在/apollo/modules/localization/conf/localization.conf中添加关于地图路径的配置：
# Redefine the map_dir in global_flagfile.txt--map_dir=DATA_PATH
这将会覆盖global_flagfile.txt中的默认值。

运行多传感容和定位模块

./scripts/localization.sh

定位程序将在后台运行，可以通过以下命令进行查看。

ps -e | grep localization

在/apollo/data/log目录下，可以看到定位模块输出的相关文件。

localization.INFO : INFO级别的log信息

localization.WARNING : WARNING级别的log信息

localization.ERROR : ERROR级别的log信息

localization.out : 标准输出重定向文件

localizaiton.flags : 启动localization模块使用的配置

播放演示rosbag

cd DATA_PATH/bag 
rosbag play *.bag

从播放数据到定位模块开始输出定位消息，大约需要30s左右。

记录可视化定位结果

记录定位结果

该脚本会在后台运行录包程序，并将存放路径输出到终端上。

./scripts/record_bag.sh

可视化定位结果

./scripts/localization_online_visualizer.sh

该可视化工具首先根据定位地图生成用于可视化的缓存文件，存放在/apollo/data/map_visual目录下。

然后接收以下topic并进行可视化绘制。

/apollo/sensor/velodyne64/compensator/PointCloud2
/apollo/localization/msf_lidar
/apollo/localization/msf_gnss
/apollo/localization/pose

可视化效果如下： [外链图片转存失败(img-3p9BaRn1-1568096776319)(/home/zx/文档/o4YBAFwRxPyAbFd8AAU50JUQQEU029.png)]
如果发现可视化工具运行时卡顿，可使用如下命令重新编译可视化工具：
```
cd /apollo bazel build -c opt //modules/localization/msf/local_tool/local_visualization/online_visual:online_local_visualizer
```
编译选项-c opt优化程序性能，从而使可视化工具可以实时运行。

结束定位模块

./scripts/localization.sh stop

如果之前有运行步骤5的录包脚本，还需执行

./scripts/record_bag.sh stop

验证定位结果

假设步骤5中录取的数据存放路径为OUTPUT_PATH，杆臂值外参的路径为ANT_IMU_PATH

运行脚本：

./scripts/msf_local_evaluation.sh OUTPUT_PATH ANT_IMU_PATH

该脚本会以RTK定位模式为基准，将多传感器融合模式的定位结果进行对比。注意只有在GNSS信号良好，RTK定位模式运行良好的区域，这样的对比才是有意义的。

获得如下统计结果：

[外链图片转存失败(img-bDyH4HBG-1568096776319)(/home/zx/文档/o4YBAFwRxP-AJ5KKAAKBgsAJhGY865.png)]
可以看到三组统计结果，第一组是组合导航(输出频率200hz)的统计结果，第二组是点云定位(输出频率5hz)的统计结果，第三组是GNSS定位(输出频率约1hz)的统计结果。

表格中各项的意义：

error：平面误差，单位为米

error lon：车前进方向的误差，单位为米

error lat：车横向方向的误差，单位为米

error roll：翻滚角误差，单位为度

error pit：俯仰角误差，单位为度

error yaw：偏航角误差，单位为度

mean：误差的平均值

std：误差的标准差

max：误差的最大值

<30cm：距离误差少于30cm的帧所占的百分比

<1.0d：角度误差小于1.0d的帧所占的百分比

con_frame()：满足括号内条件的最大连续帧数

2019年09月10日14:23:06

收起

展开全文

深蓝学院-多传感器融合定位-第3章作业

2021-02-10 06:00:54

深蓝学院-多传感器融合定位-第三章作业


深蓝学院-多传感器融合定位-第三章作业
题目内容
第一题：残差模型推导
第二题: Ceres解析求导
Ceres的使用
Ceres解析求导：线特征残差
Ceres解析求导：面特征残差
Ceres和Eigen的坑点
第三题：EVO结果分析

题目内容

第一题：残差模型推导




补充：鉴于写代码时发现A-LOAM的更新的参数块是四元数param_q和平移矢量param_t，所以我重新写了关于R，t的李代数求导，这样就可以分别对R和t进行残差更新：


第二题: Ceres解析求导

大师兄：Ceres库有自动求导、数值求导和解析求导，A-LOAM中用的是自动求导 但是效率会比较低。
本题代码就是要实现Ceres的解析求导，所以我们主要的工作是输入第一题中得到的Jacobian。

（听起来很简单对吧？）

首先我们可以看到在alom_laser_odometry_node.cpp中Line 382左右是线特征的CostFunction:
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);


大师兄：这就是我们要着手的地方了，接着进入LidarEdgeFactor，看看这个它自动求导是怎么写的，以及我们如何自己手写解析求导。

小萝卜：大师兄，且慢！problem.AddResidualBlock()每个参数的含义是什么？

大师兄：小萝卜，这就对了，懂得问问题啦！

大师兄：这里的cost_function的我们要定义残差Residual和导数Jacobian的地方，loss_function其实就是Ceres的核函数（暂且不管他），para_q和para_t分别是我们要优化的参数块。


小萝卜：谢谢大师兄，我还有一问！为什么我们一开始手写的Jacobian是用了李代数，算Residual对Transformation矩阵的导数，现在怎么变成对q和t求导了呢？对四元数求导？我可不会呀！

大师兄：这个问题问得很好！老纳一开始也在这个问题上栽了根头。我就在纳闷儿，我们公式是对T求导，然后咱们分成了R和t求导，再后来还变成了q和t求导了呢！

大师兄：其实啊，就是咱们这里把Ceres对T的优化，拆分开了分别对R和t进行优化(SO3)，然而由于咱们的输入parameters是四元数q和t，所以咱们还要把R里面3维的旋转向量v转成4维的q。（当然也可以不拆分开用SE(3)）

大师兄：这一步，其实Ceres自带的LocalParameterization已经帮我们解决了！¹

那我们再来看看他是怎么定义的自动求导：
struct LidarEdgeFactor
{
	LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
					Eigen::Vector3d last_point_b_, double s_)
		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{

		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
		Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
		q_last_curr = q_identity.slerp(T(s), q_last_curr);
		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

		Eigen::Matrix<T, 3, 1> lp;
		lp = q_last_curr * cp + t_last_curr;

		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
		Eigen::Matrix<T, 3, 1> de = lpa - lpb;

		residual[0] = nu.x() / de.norm();
		residual[1] = nu.y() / de.norm();
		residual[2] = nu.z() / de.norm();

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
									   const Eigen::Vector3d last_point_b_, const double s_)
	{
		return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
	}

	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	double s;
};

Ceres的使用

小萝卜：大师兄大师兄，这个Ceres的代码好复杂，我看不懂呀！

大师兄：看不懂就对了！我也是参考了好几个大佬的博客才明白呢。

请各位同学参考这几个博客网页¹ ²来学习Ceres。
Ceres解析求导：线特征残差
// EdgeAnalyticCostFunction
class LidarEdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 4, 3> {
public:
    LidarEdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
								Eigen::Vector3d last_point_b_, double s_)
		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

  	virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const 
	{
		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
		Eigen::Vector3d lp;
		Eigen::Vector3d lp_r;
		lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_by_dr
		lp = q_last_curr * curr_point + t_last_curr; //new point
		Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
		Eigen::Vector3d de = last_point_a - last_point_b;

		residuals[0] = nu.x() / de.norm();
		residuals[1] = nu.y() / de.norm();
		residuals[2] = nu.z() / de.norm();

		if(jacobians != NULL)
		{
			if(jacobians[0] != NULL)
			{
				Eigen::Vector3d re = last_point_b - last_point_a;
				Eigen::Matrix3d skew_re = skew(re);
				//  Rotation
				Eigen::Matrix3d skew_lp_r = skew(lp_r);
				Eigen::Matrix<double, 3, 3> dp_by_dr;
				dp_by_dr.block<3,3>(0,0) = -skew_lp_r;
				Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
				J_so3_r.setZero();
				J_so3_r.block<3,3>(0,0) = skew_re * dp_by_dr / de.norm();

				// Translation
				Eigen::Matrix<double, 3, 3> dp_by_dt;
				(dp_by_dt.block<3,3>(0,0)).setIdentity();
				Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
				J_so3_t.setZero();
				J_so3_t.block<3,3>(0,0) = skew_re * dp_by_dt / de.norm();	
			}
		}

		return true;
 
	}

protected:
	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	double s;
};

这里写完以后，记得改problem的costFunction:
ceres::CostFunction *cost_function = new LidarEdgeAnalyticCostFunction(curr_point, last_point_a, last_point_b, s);  
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

Ceres解析求导：面特征残差
// PlaneAnalyticCostFunction
class LidarPlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3> {
public:
    LidarPlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
								Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
		: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_) {}

  	virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const 
	{
		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
		Eigen::Vector3d lp;
		Eigen::Vector3d lp_r;
		lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_dr
		lp = q_last_curr * curr_point + t_last_curr; //new point
		Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
		double nu = (lp-last_point_j).dot(de);
		
		residuals[0] = nu / de.norm();

		if(jacobians != NULL)
		{
			if(jacobians[0] != NULL)
			{
				Eigen::Vector3d dX_dp = de / de.norm();
				double X = nu / de.norm();
				Eigen::Vector3d ddh_dp = X * dX_dp / std::abs(X);
				//  Rotation
				Eigen::Matrix3d skew_lp_r = skew(lp_r);
				Eigen::Matrix<double, 3, 3> dp_dr;
				dp_dr.block<3,3>(0,0) = -skew_lp_r;
				Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
				J_so3_r.setZero();
				J_so3_r.block<1,3>(0,0) = ddh_dp.transpose() * dp_dr;

				// Translation
				Eigen::Matrix<double, 3, 3> dp_dt;
				(dp_dt.block<3,3>(0,0)).setIdentity();
				Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
				J_so3_t.setZero();
				J_so3_t.block<1,3>(0,0) = ddh_dp.transpose() * dp_dt;
			}
		}

		return true;
 
	}

protected:
	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
	double s;
};

Ceres和Eigen的坑点

Eigen Library requires the data type to be consistent, eg.

// Since de is Vector3d, thus nu must be double
Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
double nu = (lp-last_point_j).dot(de);


Eigen Vector product must use explicitly .dot() pr .cross()

第三题：EVO结果分析


APE






RPE





RVIZ中的结果，熟悉的Kitti图，可以看到在最后转弯的时候还是有点偏离绿色的GNSS轨迹，不过整体效果还是不错！







yuntian_li, Ceres详解（一） CostFunction ↩︎ ↩︎

leeayu, Ceres Tutotial —— 最小二乘建模 ↩︎

收起

展开全文

自动驾驶

深蓝学院-多传感器融合定位-第7章作业

2021-03-21 17:57:30

深蓝学院-多传感器融合定位-第7章作业作业解析及代码


深蓝学院-多传感器融合定位-第7章作业
1. 及格题
2. 良好题
Parameter - verison 1
Parameter - verison 2
Parameter - verison 3
Parameter - verison 4
Parameter - verison 5
3. 优秀题
Parameter - verison 6 (without Rand-Walk Bias)


Github:    https://github.com/WeihengXia0123/LiDar-SLAM


1. 及格题

(大概整了一两天，终于可以编译了…关键问题在于，docker的环境可能被很多次作业打乱了。在助教葛大佬的提示下，新创建了一个assignments/来放第七章作业的代码，神奇地解决了初始代码的编译问题)

及格部分的代码就是按照深蓝07章课件中的Error-State Kalman Filter的公式来。

注意： 和EKF有点区别。

//
// TODO: perform Kalman prediction
//
X_ = F * X_;                                          // fix this
P_ = F * P_ * F.transpose() + B * Q_ * B.transpose(); // fix this


//
// TODO: set measurement:
// 误差：预测值　－　观测值
//
Eigen::Vector3d P_nn_obs = pose_.block<3,1>(0,3) - T_nb.block<3,1>(0,3);             // fix this
Eigen::Matrix3d R_nn_obs = T_nb.block<3,3>(0,0).transpose() * pose_.block<3,3>(0,0); // fix this

YPose_.block<3, 1>(0, 0) = P_nn_obs;
YPose_.block<3, 1>(3, 0) = Sophus::SO3d::vee(R_nn_obs - Eigen::Matrix3d::Identity());

Y = YPose_;

// set measurement equation:
G = GPose_;

//
// TODO: set Kalman gain:
//
MatrixRPose R = RPose_; // fix this
MatrixCPose C = Eigen::MatrixXd::Identity(6,6);
K = P_ * G.transpose() * (G*P_*G.transpose() + C*R*C.transpose()).inverse(); // fix this               

//
// TODO: set measurement:
// 误差：预测值　－　观测值
//
Eigen::Vector3d P_nn_obs = pose_.block<3,1>(0,3) - T_nb.block<3,1>(0,3);             // fix this
Eigen::Matrix3d R_nn_obs = T_nb.block<3,3>(0,0).transpose() * pose_.block<3,3>(0,0); // fix this

YPose_.block<3, 1>(0, 0) = P_nn_obs;
YPose_.block<3, 1>(3, 0) = Sophus::SO3d::vee(R_nn_obs - Eigen::Matrix3d::Identity());

Y = YPose_;

// set measurement equation:
G = GPose_;

//
// TODO: set Kalman gain:
//
MatrixRPose R = RPose_; // fix this
MatrixCPose C = Eigen::MatrixXd::Identity(6,6);
K = P_ * G.transpose() * (G*P_*G.transpose() + C*R*C.transpose()).inverse();   

//
// TODO: perform Kalman correct:
//

P_ = (MatrixP::Identity() - K*G) * P_; // fix this
X_ = X_ + K * (Y - G*X_);              // fix this


void ErrorStateKalmanFilter::EliminateError(void) {
  //
  // TODO: correct state estimation using the state of ESKF
  //
  // a. position:
  pose_.block<3, 1>(0, 3) =
          pose_.block<3, 1>(0, 3) - X_.block<3,1>(INDEX_ERROR_POS, 0); // fix this
  // b. velocity:
  vel_ = vel_ - X_.block<3,1>(INDEX_ERROR_VEL, 0);                     // fix this
  // c. orientation:
  Eigen::Matrix3d R_nn =
      Sophus::SO3d::hat(X_.block<3, 1>(INDEX_ERROR_ORI, 0)).matrix();
  pose_.block<3, 3>(0, 0) = pose_.block<3, 3>(0, 0) * (Eigen::MatrixXd::Identity(3,3) - R_nn); // fix this

  // d. gyro bias:
  if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_GYRO)) {
    gyro_bias_ += X_.block<3, 1>(INDEX_ERROR_GYRO, 0);
  }

  // e. accel bias:
  if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_ACCEL)) {
    accl_bias_ += X_.block<3, 1>(INDEX_ERROR_ACCEL, 0);
  }
}

滤波功能基本正常。
2. 良好题
Parameter - verison 1
        process:
            gyro: 1.0e-4
            accel: 2.5e-3
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: True
        measurement:
            pose:
                pos: 1.0e-3
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

Result - laser

APE w.r.t. full transformation (unit-less)
   max	1.500344
  mean	0.902466
median	0.894143
   min	0.161967
  rmse	0.917469
   sse	3696.964333
   std	0.165240

Result - fused

APE w.r.t. full transformation (unit-less)
   max	1.549884
  mean	0.898618
median	0.898169
   min	0.034132
  rmse	0.916721
   sse	3690.936975
   std	0.181281

Parameter - verison 2
        process:
            gyro: 1.0e-3
            accel: 2.5e-2
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: True
        measurement:
            pose:
                pos: 1.0e-3
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

Result - laser

APE w.r.t. full transformation (unit-less)
   max	1.500344
  mean	0.901874
median	0.893900
   min	0.161967
  rmse	0.916922
   sse	3690.876634
   std	0.165440

Result - fused
APE w.r.t. full transformation (unit-less)
   max	1.539430
  mean	0.898373
median	0.897470
   min	0.058437
  rmse	0.916334
   sse	3686.143600
   std	0.180540

Parameter - verison 3
        process:
            gyro: 1.0e-5
            accel: 2.5e-6
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: True
        measurement:
            pose:
                pos: 1.0e-3
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

Result - laser

APE w.r.t. full transformation (unit-less)

(not aligned)
   max	1.136680
  mean	0.231626
median	0.164014
   min	0.017465
  rmse	0.289811
   sse	368.550415
   std	0.174183

Result - fused
APE w.r.t. full transformation (unit-less)

(not aligned)
   max	1.013023
  mean	0.258347
median	0.200838
   min	0.019931
  rmse	0.309749
   sse	421.004733
   std	0.170884

Parameter - verison 4
        process:
            gyro: 1.0e-5
            accel: 2.5e-6
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: True
        measurement:
            pose:
                pos: 1.0e-4
                ori: 1.0e-5
            pos: 1.0e-4
            vel: 2.5e-3

Result - laser

APE w.r.t. full transformation (unit-less)
   max	1.500344
  mean	0.901922
median	0.893900
   min	0.161967
  rmse	0.916961
   sse	3691.186379
   std	0.165389

Result - fused
APE w.r.t. full transformation (unit-less)
   max	1.530646
  mean	0.900078
median	0.896358
   min	0.033429
  rmse	0.917532
   sse	3695.785994
   std	0.178115

Parameter - verison 5
        process:
            gyro: 1.0e-6
            accel: 2.5e-7
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: True
        measurement:
            pose:
                pos: 1.0e-3
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

Result - laser

APE w.r.t. full transformation (unit-less)
   max	1.500344
  mean	0.902180
median	0.894120
   min	0.161967
  rmse	0.917157
   sse	3692.766704
   std	0.165068

Result - fused

APE w.r.t. full transformation (unit-less)
   max	1.554055
  mean	0.898355
median	0.896839
   min	0.058776
  rmse	0.916404
   sse	3686.702189
   std	0.180979

3. 优秀题
我加了个bias_flag，只要设为Flase就可以不考虑随即游走的bias。
  // d. gyro bias:
  if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_GYRO)) {
    gyro_bias_ += X_.block<3, 1>(INDEX_ERROR_GYRO, 0);
  }

  // e. accel bias:
  if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_ACCEL)) {
    accl_bias_ += X_.block<3, 1>(INDEX_ERROR_ACCEL, 0);
  }

由于跑出来太花时间，这里直只放了一组数据：
Parameter - verison 6 (without Rand-Walk Bias)
       process:
            gyro: 1.0e-5
            accel: 2.5e-6
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: False
        measurement:
            pose:
                pos: 1.0e-3
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

Result - laser

APE w.r.t. full transformation (unit-less)
   max	1.136680
  mean	0.231211
median	0.164014
   min	0.017465
  rmse	0.289257
   sse	367.644384
   std	0.173813

Result - fused
APE w.r.t. full transformation (unit-less)
   max	1.012434
  mean	0.257726
median	0.201350
   min	0.008808
  rmse	0.309306
   sse	420.375118
   std	0.171020

下面是KITTI数据集跑出来的效果图。
黄色线表示ground-truth，蓝色和红色分别是LiDar单独和LiDar/IMU融合的轨迹。（这里几乎重合，原因是这个参数调得很好，红红火火哈哈哈哈）

收起

展开全文

自动驾驶

多传感器融合定位在高速铁路的应用
2020-10-26 05:03:51
针对高速铁路列车运行控制系统中的列车定位问题，提出了GPS／DR／MM组合定位的方案，利用卡尔曼滤波对多传感器的数据信息进行融合之后再与电子地图匹配，实时提供列车的定位信息。与单一传感器定位方式相比，可以...
收起
多传感器融合定位-3D激光里程计（一）
2021-04-15 16:30:22
多传感器融合定位-3D激光里程计整体流程激光里程计方案基于SVD的ICP介绍进一步推导旋转部分求解整体流程点云地图构建过程分为前端后端两部分，其中的前端过程可以看做激光里程计过程。激光里程计方案前端里程计...
收起
深蓝学院-多传感器融合定位-第6章作业
2021-03-09 06:25:07
深蓝学院-多传感器融合定位-第6章作业1. 及格题目2. 良好题目3. 优秀题目 1. 及格题目 2. 良好题目 3. 优秀题目
收起
深蓝学院-多传感器融合定位-第4章作业
2021-02-20 02:40:51
深蓝学院-多传感器融合定位-第4章作业一. 及格部分1. 代码跑通二. 良好部分三. 优秀部分Scan Context版本GNSS版本一. 及格部分 1. 代码跑通直接编译，跑通： catkin config --install && catkin build ...
收起
自动驾驶
多传感器融合定位2（封装KF和EKF）
2020-03-28 18:58:28
下面做一个多传感器融合定位实验，采集到的数据为两种交替出现的数据。代码整体框架如图所示：前言无论是KF处理线性问题还是EKF处理非线性问题，它都只涉及单一传感器的障碍物跟踪。激光雷达测量位置精度更高...
收起
多传感器融合定位（GPS+Wheel+camera）（1）-读取传感器数据
2021-02-09 21:37:43
多传感器融合定位（GPS+Wheel+camera）（1）-读取传感器数据文章目录1、读取Kaist数据集到融合系统中 1、读取Kaist数据集到融合系统中 int main(int argc, char** argv) { if (argc != 3) { LOG(ERROR) << ...
收起
多传感器融合定位（4-基于滤波的融合方法）kitti数据集 IMU频率改为100HZ
千次阅读 2020-11-21 00:12:30
多传感器融合定位（4-基于滤波的融合方法）kitti数据集 IMU频率改为100HZ 本博文参考GEYAO 大神的github 到kitti 官网下载 2011_10_03_drive_0027 的extract数据集（原来的 sync数据集为去除相机畸变的数据集，但是...
收起
多传感器融合定位1（激光雷达+毫米波雷达）
千次阅读 2020-03-28 13:07:15
前言 LZ最近在看Udacity的无人...本节将用最简洁的话讲解卡尔曼滤波KF、非线性卡尔曼滤波EKF等知识点，并就此实现一个多传感器融合定位的小demo，后面会就粒子滤波PF专门开一个章节讲解。一、卡尔曼滤波 KF 1、引子...
收起
多传感器融合定位（2-点云地图构建及基于地图定位）2-后端优化与点云地图构建
2020-10-21 14:08:08
多传感器融合定位（2-点云地图构建及基于地图定位）2-后端优化与点云地图构建后端优化点云地图的建立
收起
多传感器融合定位（1-3D激光里程计）1-前端里程计ICP
2020-10-11 19:01:24
多传感器融合定位（2-3D激光里程计）1-前端里程计ICP 机械激光雷达与固态激光雷达整体流程 ICP point2point ICP推导把旋转和平移进行解耦去除与R无关项 trace 不等式证明
收起
多传感器融合定位（4-点云地图构建及基于地图定位）4-通过GNSS 实现地图定位
2020-10-26 17:14:21
多传感器融合定位（4-点云地图构建及基于地图定位）4-通过GNSS 实现地图定位 eg:因为个人水平有限，这次作业全靠DK大哥的指导，方法不唯一目的：调用 GNSS的位姿，实现初始化调整前，默认初始gps位姿为 0 ， 0 ，0...
收起
多传感器融合定位（1-3D激光里程计）3-前端里程计LOAM系列
2020-10-12 16:45:34
多传感器融合定位（2-3D激光里程计）3 -前端里程计LOAM系列 LOAM 多线雷达线与线之间有两度的夹角特征点的选取
收起
多传感器融合定位（1-3D激光里程计）2-前端里程计NDT
2020-10-12 14:35:34
多传感器融合定位（2-3D激光里程计）2-前端里程计NDT NDT 计算分布于分布之间的差异 NDT的优点： 1.鲁棒性较强 2.相对于ICP而言，运算量更少转化为优化问题
收起
多传感器融合定位（1-3D激光里程计）6-实现手写icp
2020-11-01 22:21:43
多传感器融合定位（1-3D激光里程计）6-实现手写icp ps: 因为个人能力不足，本次代码和伪代码框图主要是参考 go on 大佬的，本人只是搬运工做记录 ps：本文来自 go on 助教的讲解ppt 伪代码框图 ps：图来自 go ...
收起
多传感器融合定位（3-点云地图构建及基于地图定位）3-实现ScanContext 回环检测
2020-10-26 17:09:13
多传感器融合定位（3-点云地图构建及基于地图定位）3-实现ScanContext 回环检测参考博客从零开始做自动驾驶定位(十一): 闭环修正自动驾驶系统进阶与项目实战（四）自动驾驶高精度地图构建中的三维场景识别和闭环...
收起
车载多传感器融合定位方案：GPS +IMU+MM
千次阅读 2020-11-08 00:00:00
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达导读高德定位业务包括云上定位和端上定位两大模块。其中，云上定位主要解决Wifi指纹库、AGPS定位、轨迹挖掘和聚类等问题；端上定位解...
收起
算法编程语言机器学习
多传感器融合定位（2-点云地图构建及基于地图定位）1-回环检测及代码实现
2020-10-19 20:32:08
多传感器融合定位（2-点云地图构建及基于地图定位）1-回环检测及代码实现 scan context 核心思想：三维点云匹配下，计算量巨大，基于三维匹配，初值问题无法解决，scan context 对三维进行将将维度，并且scan ...
收起
多传感器融合定位（GPS+Wheel+camera）（2）**FilterFusionSystem类**介绍
2021-02-16 21:33:18
多传感器融合定位（GPS+Wheel+camera）（2）FilterFusionSystem类介绍 1、系统初始化读取参数 FilterFusionSystem::FilterFusionSystem(const std::string& param_file) : initialized_(false), odom_G_R_O_...
收起
多传感器融合定位（3-惯性导航原理及误差分析）6-Allan方差进行随机误差分析
2020-11-08 16:06:37
多传感器融合定位（3-惯性导航原理及误差分析）6-Allan方差进行随机误差分析参考博客：组合导航系列文章（四）：Allan方差分析使用imu_utils工具生成IMU的Allan方差标定曲线手写VIO-使用Allen方差工具标定IMU ...
收起
算法分析的目的_UCI研究人员利用漏洞攻破多传感器融合定位算法_物联网新闻_新闻中心...
2020-10-20 12:53:54
自动驾驶领域目前最强的MSF(多传感器融合)定位算法，再次...连多传感器融合定位算法达到SOTA的百度Apollo，在仿真环境也中了招。这项最新研究，来自加州大学尔湾分校(UCI)，目前已发表在信息安全领域四大顶会之一的...
收起
算法分析的目的
《多传感器融合定位》点云地图构建及基于地图定位
2021-05-08 14:36:10
点云地图构建及基于地图定位习题一、闭环修正及精度评价二、位姿初始化一、闭环修正及精度评价提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿...
收起
c++ 自动驾驶
多传感器融合定位（3-惯性导航原理及误差分析）2-IMU误差分析及处理：随机误差理论分析&allan方差分析及...
2020-10-27 22:08:12
多传感器融合定位（3-惯性导航原理及误差分析）2-IMU误差分析及处理：随机误差理论分析&allan方差分析及实现
收起
多传感器融合定位（1-3D激光里程计）6-实现调用pcl-icp-3 evo里程计精度评价
2020-10-17 16:00:54
多传感器融合定位（2-3D激光里程计）6-实现调用pcl-icp、ndt -3 evo里程计精度评价参考博客：从零开始做自动驾驶定位(七): 里程计精度评价下载evo 工具 pip install evo --upgrade --no-binary evo evo用法，...
收起