多传感器融合定位 第七章 基于滤波的融合方法

最新推荐文章于 2024-08-16 08:53:55 发布
最新推荐文章于 2024-08-16 08:53:55 发布
阅读量4.4k 收藏 66
点赞数 11
分类专栏： 深蓝-多传感器定位融合 文章标签： 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_41281151/article/details/120904368
版权

多传感器融合定位 第七章 基于滤波的融合方法

参考博客：深蓝学院-多传感器融合定位-第7章作业

本次作业：主要参考张松鹏大佬的代码，因为大佬的解析太好了，为了保留记录，以下大部分文字，均摘自大佬的原话~

代码下载：https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/%E7%AC%AC%E4%BA%94%E7%AB%A0%20%E6%83%AF%E6%80%A7%E5%AF%BC%E8%88%AA%E5%8E%9F%E7%90%86%E5%8F%8A%E8%AF%AF%E5%B7%AE%E5%88%86%E6%9E%90/imu_tk

2021-10-09 18-43-09 的屏幕截图1.环境配置

1.1 protoc 版本问题

前几章使用的protoc 的版本为3.14， 第七章使用的proto版本为3.15

解决方法：需要安装新版本的proto 3.15x，按照第四章的方式生成对应的文件。

2021-10-08 14-43-25 的屏幕截图

按照GeYao README中的方法，重新生成基于自己基环境protobuf的proto：

打开lidar_localization/config/scan_context文件夹，输入如下命令，生成pb文件

protoc --cpp_out=./ key_frames.proto
protoc --cpp_out=./ ring_keys.proto
protoc --cpp_out=./ scan_contexts.proto
mv key_frames.pb.cc key_frames.pb.cpp
mv ring_keys.pb.cc ring_keys.pb.cpp
mv scan_contexts.pb.cc scan_contexts.pb.cpp

分别修改生成的三个.pb.cpp文件。如下，以ring_keys.pb.cpp为例。

// Generated by the protocol buffer compiler.  DO NOT EDIT!
// source: ring_keys.proto

#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
#include "ring_keys.pb.h" 替换为  #include "lidar_localization/models/scan_context_manager/ring_keys.pb.h"

#include <algorithm>

之后，用以上步骤生成的的.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager
中的同名文件。
将.pb.cpp文件替换（注意：需要剪切，确保config文件中新生成的文件都转移到对应目录下，不能重复出现）lidar_localization/src/models/scan_context_manager中的同名文件。

1.2 缺少 fmt 库

2021-10-08 14-51-24 的屏幕截图

git clone https://github.com/fmtlib/fmt
cd fmt/
mkdir build
cd build
cmake ..
make
sudo make install

编译过程中出现：error_state_kalman_filter.cpp:(.text.unlikely+0x84)：对‘fmt::v8::detail::assert_fail(char const, int, char const)’未定义的引用**

2021-10-08 15-43-05 的屏幕截图

参考网址： undefined reference to `vtable for fmt::v7::format_error‘

cd   catkin_ws/src/lidar_localization/cmake/sophus.cmake

list append 添加 fmt

#  sophus.cmake
find_package (Sophus REQUIRED)

include_directories(${Sophus_INCLUDE_DIRS})
list(APPEND ALL_TARGET_LIBRARIES ${Sophus_LIBRARIES} fmt)

1.3 glog缺少gflag的依赖

logging.cc:(.text+0x6961)：对‘google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, bool*, bool*)’未定义的引用

2021-10-08 15-46-43 的屏幕截图

#解决办法： 打开glog.cmake , 末尾改为
list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES} libgflags.a libglog.a)

2.error state kalmam filter

FILE: lidar_localization/src/model/kalman_filter/error_state_kalman_filter.cpp

2.1 流程框图

2021-10-09 18-58-36 的屏幕截图

2.2 代码结构

滤波算法主要包括预测(Update函数)和观测(Correct函数)两个部分:

  1. 预测部分接收imu数据,基于惯性解算更新名义值,基于状态方程更新误差值。
  2. 观测部分同时接收imu数据和定位数据,首先利用imu数据进行预测保证状态与定位数据
    时间同步,然后基于观测方程计算误差值,最后利用误差值对名义值进行修正,并将误
    差值清零。

2.3 初始化： Init

这里特别注意,框架是基于第一期课程,其中的旋转误差放在了导航系(n系)下,而第三期将旋转误差放在了机器人坐标系(b系)下,所以公式有所不同,特别是状态方程所用到的加速度应该是在b系下,也就是UpdateProcessEquation函数传入的linear_acc_mid应该是在b系下。所有调用到这个函数的地方都应该进行修改。

修改1：FUNCTION: ErrorStateKalmanFilter::ErrorStateKalmanFilter(const YAML::Node &node) {}

  // set process equation in case of one step prediction & correction:
  Eigen::Vector3d linear_acc_init(imu_data.linear_acceleration.x,
                                  imu_data.linear_acceleration.y,
                                  imu_data.linear_acceleration.z);
  Eigen::Vector3d angular_vel_init(imu_data.angular_velocity.x,
                                   imu_data.angular_velocity.y,
                                   imu_data.angular_velocity.z);
  // covert to navigation frame:    //  把 IMU 的 velocity     angular（flu系）转换到 导航系 下 
  linear_acc_init =  linear_acc_init - accl_bias_;            //  body 系下  
  angular_vel_init = GetUnbiasedAngularVel(angular_vel_init, C_nb);
  // init process equation, in case of direct correct step:
  UpdateProcessEquation(linear_acc_init, angular_vel_init);

修改2：FUNCTION: ErrorStateKalmanFilter::GetVelocityDelta( )

bool ErrorStateKalmanFilter::GetVelocityDelta(
    const size_t index_curr, const size_t index_prev,
    const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev, double &T,
    Eigen::Vector3d &velocity_delta, Eigen::Vector3d &linear_acc_mid) {
  if (index_curr <= index_prev || imu_data_buff_.size() <= index_curr) {
    return false;
  }

  const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
  const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);

  T = imu_data_curr.time - imu_data_prev.time;

  Eigen::Vector3d linear_acc_curr = Eigen::Vector3d(
      imu_data_curr.linear_acceleration.x, imu_data_curr.linear_acceleration.y,
      imu_data_curr.linear_acceleration.z);
  Eigen::Vector3d  a_curr = GetUnbiasedLinearAcc(linear_acc_curr, R_curr);        //  w系下的a_curr
  Eigen::Vector3d linear_acc_prev = Eigen::Vector3d(
      imu_data_prev.linear_acceleration.x, imu_data_prev.linear_acceleration.y,
      imu_data_prev.linear_acceleration.z);
  Eigen::Vector3d  a_prev = GetUnbiasedLinearAcc(linear_acc_prev, R_prev);        //  w系下的a_prev
  // mid-value acc can improve error state prediction accuracy:
  linear_acc_mid = 0.5 * (a_curr + a_prev);     //  w 系下的linear_acc_mid , 用于更新pos_w 和 vel_w
  velocity_delta = T * linear_acc_mid;

  linear_acc_mid = 0.5 * (linear_acc_curr + linear_acc_prev) - accl_bias_;      //  b 系下的linear_acc_mid

  return true;
}

这里有个之前一直忽略的点，在此mark下笔记：

inline Eigen::Vector3d ErrorStateKalmanFilter::GetUnbiasedAngularVel(
    const Eigen::Vector3d &angular_vel, const Eigen::Matrix3d &R) {
  return angular_vel - gyro_bias_;
}

inline Eigen::Vector3d
ErrorStateKalmanFilter::GetUnbiasedLinearAcc(const Eigen::Vector3d &linear_acc,
                                             const Eigen::Matrix3d &R) {
  return R * (linear_acc - accl_bias_) - g_;
}

两个函数，区别在于，惯性解算时

更新四元数时，只需要得到相对旋转，在body系下就可以得到相对旋转，所以不需要乘以R。

更新位置时，需要把速度转换到n系下，所以需要乘以R。

在init filter 初始化滤波器时，

angular_vel_init 、linear_acc_init 都是b 系下的

// 可以调用 GetUnbiasedAngularVel ，因为角速度仍然时在b系下的
angular_vel_init = GetUnbiasedAngularVel(angular_vel_init, C_nb);      

// 不可以调用 GetUnbiasedLinearAcc ，因为调用后加速度换左乘R，变换到n系下的
linear_acc_init =  linear_acc_init - accl_bias_;            //  body 系下

2.4 预测: Update

包含： 1.更新名义值UpdateOdomEstimation
2.更新误差值UpdateErrorEstimation

bool ErrorStateKalmanFilter::Update(const IMUData &imu_data) {                 //  更新
  //        
  // TODO: understand ESKF update workflow
  //
  // update IMU buff:
  if (time_ < imu_data.time) {
    // update IMU odometry:
    Eigen::Vector3d linear_acc_mid;
    Eigen::Vector3d angular_vel_mid;
    imu_data_buff_.push_back(imu_data);
    UpdateOdomEstimation(linear_acc_mid, angular_vel_mid);             //  更新名义值 ， 惯性解算
    imu_data_buff_.pop_front();

    // update error estimation:
    double T = imu_data.time - time_;                                                                   //  滤波周期
    UpdateErrorEstimation(T, linear_acc_mid, angular_vel_mid);           //  更新误差估计   

    // move forward:
    time_ = imu_data.time;
    return true;
  }
  return false;
}
2.4.1 更新名义值 UpdateOdomEstimation

这部分是上一章惯性导航解算的内容;

void ErrorStateKalmanFilter::UpdateOdomEstimation(                  //  更新名义值 
    Eigen::Vector3d &linear_acc_mid, Eigen::Vector3d &angular_vel_mid) {
  //
  // TODO: this is one possible solution to previous chapter, IMU Navigation,
  // assignment
  //
  // get deltas:
    size_t   index_curr_  = 1;
    size_t   index_prev_ = 0;
    Eigen::Vector3d  angular_delta = Eigen::Vector3d::Zero();            
    GetAngularDelta(index_curr_,  index_prev_,   angular_delta,  angular_vel_mid);           //   获取等效旋转矢量,   保存角速度中值
  // update orientation:
    Eigen::Matrix3d  R_curr_  =  Eigen::Matrix3d::Identity();
    Eigen::Matrix3d  R_prev_ =  Eigen::Matrix3d::Identity();
    UpdateOrientation(angular_delta, R_curr_, R_prev_);                         //     更新四元数
  // get velocity delta:
    double   delta_t_;
    Eigen::Vector3d  velocity_delta_;
    GetVelocityDelta(index_curr_, index_prev_,  R_curr_,  R_prev_, delta_t_,  velocity_delta_,  linear_acc_mid);             //  获取速度差值， 保存线加速度中值
  // save mid-value unbiased linear acc for error-state update:

  // update position:
  UpdatePosition(delta_t_,  velocity_delta_);
}

这里需要注意,GetVelocityDelta函数中的linear_acc_mid应该返回在b系下的测量值

2.4.2 更新误差值UpdateErrorEstimation

首先调用UpdateProcessEquation计算状态方程中的F和B,该函数进一步调用SetProcessEquation函数

2021-10-09 19-34-19 的屏幕截图

2021-10-09 19-56-37 的屏幕截图

UpdateErrorEstimation()
void ErrorStateKalmanFilter::UpdateErrorEstimation(                       //  更新误差值
    const double &T, const Eigen::Vector3d &linear_acc_mid,
    const Eigen::Vector3d &angular_vel_mid) {
  static MatrixF F_1st;
  static MatrixF F_2nd;
  // TODO: update process equation:         //  更新状态方程
  UpdateProcessEquation(linear_acc_mid ,  angular_vel_mid);
  // TODO: get discretized process equations:         //   非线性化
  F_1st  =  F_ *  T;        //  T kalman 周期
  MatrixF   F = MatrixF::Identity()  +   F_1st;
  MatrixB  B =  MatrixB::Zero();
  B.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro) * T;
  B.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro) *T;
  if(COV.PROCESS.BIAS_FLAG){
      B.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)* sqrt(T);
      B.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)  =      B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)* sqrt(T);
  }

  // TODO: perform Kalman prediction
  X_ = F * X_;
  P_ =  F * P_ * F.transpose()   +  B * Q_ * B.transpose();             //   只有方差进行了计算
}
UpdateProcessEquation()
void ErrorStateKalmanFilter::UpdateProcessEquation(
    const Eigen::Vector3d &linear_acc_mid,
    const Eigen::Vector3d &angular_vel_mid) {
  // set linearization point:
  Eigen::Matrix3d C_nb = pose_.block<3, 3>(0, 0);           //   n2b   转换矩阵
  Eigen::Vector3d f_b = linear_acc_mid + g_;                     //   加速度
  Eigen::Vector3d w_b = angular_vel_mid;                         //   角速度

  // set process equation:
  SetProcessEquation(C_nb, f_b, w_b);
}
SetProcessEquation()
void ErrorStateKalmanFilter::SetProcessEquation(const Eigen::Matrix3d &C_nb,                      //   更新状态方程  F矩阵
                                                const Eigen::Vector3d &f_b,
                                                const Eigen::Vector3d &w_b) {
  // TODO: set process / system equation:
  // a. set process equation for delta vel:
  F_.setZero();
  F_.block<3,  3>(kIndexErrorPos,  kIndexErrorVel)  =  Eigen::Matrix3d::Identity();
  F_.block<3,  3>(kIndexErrorVel,   kIndexErrorOri)  =  - C_nb *  Sophus::SO3d::hat(f_b).matrix();
  F_.block<3,  3>(kIndexErrorVel,   kIndexErrorAccel) =  -C_nb;
  F_.block<3,  3>(kIndexErrorOri,   kIndexErrorOri) =   - Sophus::SO3d::hat(w_b).matrix();
  F_.block<3,  3>(kIndexErrorOri,   kIndexErrorGyro) =   - Eigen::Matrix3d::Identity();
  // b. set process equation for delta ori:
  B_.setZero();
  B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =    C_nb;
  B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =     Eigen::Matrix3d::Identity();
  if(COV.PROCESS.BIAS_FLAG){      //  判断是否考虑随机游走
    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =     Eigen::Matrix3d::Identity();
    B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)    =     Eigen::Matrix3d::Identity();
  }
}

2.5 观测 Correct

bool ErrorStateKalmanFilter::Correct(const IMUData &imu_data,                 //  修正
                                     const MeasurementType &measurement_type,
                                     const Measurement &measurement) {
  static Measurement measurement_;

  // get time delta:
  double time_delta = measurement.time - time_;

  if (time_delta > -0.05) {                 //  时间对齐
    // perform Kalman prediction:
    if (time_ < measurement.time) {
      Update(imu_data);
    }

    // get observation in navigation frame:
    measurement_ = measurement;
    measurement_.T_nb = init_pose_ * measurement_.T_nb;

    // correct error estimation:
    CorrectErrorEstimation(measurement_type, measurement_);

    // eliminate error:
    EliminateError();        //  更新名义值

    // reset error state:
    ResetState();                //  清零误差值，方差保留

    return true;
  }
2.5.1 计算误差值 CorrectErrorEstimation

a. 首先调用CorrectErrorEstimationPose 计算 Y, G, K。

2021-10-10 11-09-28 的屏幕截图

void ErrorStateKalmanFilter::CorrectErrorEstimationPose(                    //  计算  Y ，G  ，K
    const Eigen::Matrix4d &T_nb, Eigen::VectorXd &Y, Eigen::MatrixXd &G,
    Eigen::MatrixXd &K) {
  //
  // TODO: set measurement:      计算观测 delta pos  、 delta  ori
  //
  Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);
  Eigen::Matrix3d  dR  =  T_nb.block<3,  3>(0, 0).transpose() *  pose_.block<3, 3>(0, 0) ;
  // TODO: set measurement equation:
  Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );
  YPose_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
  YPose_.block<3, 1>(3, 0)  =  dtheta;          //   失准角
  Y = YPose_;
  //   set measurement  G
  GPose_.setZero();
  GPose_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
  GPose_.block<3 ,3>(3, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();        
  G  =   GPose_;     
  //   set measurement  C
  CPose_.setZero();
  CPose_.block<3, 3>(0,kIndexNoiseAccel)   =  Eigen::Matrix3d::Identity();
  CPose_.block<3, 3>(3,kIndexNoiseGyro)    =  Eigen::Matrix3d::Identity();
  Eigen::MatrixXd  C  =   CPose_;
  // TODO: set Kalman gain:
  Eigen::MatrixXd R = RPose_;    //  观测噪声
  K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * RPose_*  C.transpose() ).inverse() ;
}

b. 更新后验，计算滤波的第4，5个公式

2021-10-10 11-12-44 的屏幕截图

void ErrorStateKalmanFilter::CorrectErrorEstimation(
    const MeasurementType &measurement_type, const Measurement &measurement) {
  //
  // TODO: understand ESKF correct workflow
  //
  Eigen::VectorXd Y;
  Eigen::MatrixXd G, K;
  switch (measurement_type) {
  case MeasurementType::POSE:
    CorrectErrorEstimationPose(measurement.T_nb, Y, G, K);
    break;
  default:
    break;
  }

  // TODO: perform Kalman correct:
  P_ = (MatrixP::Identity() -  K*G)  *  P_ ;          //  后验方差
  X_ =  X_ +  K * (Y - G*X_);                                                      //  更新后的状态量
}
2.5.2 修正名义值 EliminateError

修正平移,速度,旋转和零偏,修正方式是预测值减去误差值。

2021-10-10 11-16-25 的屏幕截图

void ErrorStateKalmanFilter::EliminateError(void) {
  //      误差状态量  X_  :   15*1
  // TODO: correct state estimation using the state of ESKF
  //
  // a. position:
  // do it!
  pose_.block<3, 1>(0, 3)  -=  X_.block<3, 1>(kIndexErrorPos, 0 );   //  减去error
  // b. velocity:
  // do it!
  vel_ -=  X_.block<3,1>(kIndexErrorVel, 0 );         
  // c. orientation:
  // do it!
  Eigen::Matrix3d   dtheta_cross =  Sophus::SO3d::hat(X_.block<3,1>(kIndexErrorOri, 0));         //   失准角的反对称矩阵
  pose_.block<3, 3>(0, 0) =  pose_.block<3, 3>(0, 0) * (Eigen::Matrix3d::Identity() - dtheta_cross);     
  Eigen::Quaterniond  q_tmp(pose_.block<3, 3>(0, 0) );
  q_tmp.normalize();        //  为了保证旋转矩阵是正定的
  pose_.block<3, 3>(0, 0) = q_tmp.toRotationMatrix();  

  // d. gyro bias:
  if (IsCovStable(kIndexErrorGyro)) {
    gyro_bias_ -= X_.block<3, 1>(kIndexErrorGyro, 0);           //  判断gyro_bias_error是否可观
  }

  // e. accel bias:
  if (IsCovStable(kIndexErrorAccel)) {
    accl_bias_ -= X_.block<3, 1>(kIndexErrorAccel, 0);          //   判断accel_bias_error是否可观 
  }
}
2.5.3 清零误差值 ResetState

2021-10-10 11-17-17 的屏幕截图

void ErrorStateKalmanFilter::ResetState(void) {
  // reset current state:
  X_ = VectorX::Zero();
}

Running

# build:
catkin_make
# set up session:
source devel/setup.bash
# launch:
roslaunch lidar_localization kitti_localization.launch

# play ROS bag, lidar-only KITTI:
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag

# evo  estimate

3.不考虑随机游走

3.1 公式推导

状态方程：
δ x ˙ = F t δ x + B t w \delta\dot{x}= F_t\delta{x} + B_tw δx˙=Ft​δx+Bt​w
状态量：
δ p ˙ = δ v δ v ˙ = − R t [ a t − b a t ] × δ θ + R t ( n a − δ b a ) δ θ ˙ = − [ ω t − b ω t ] × δ θ + n ω − δ b ω δ b ˙ a = 0 δ b ˙ w = 0 \begin{array}{ll} \delta \dot{p} = \delta \boldsymbol{v} \\ \delta \dot{\boldsymbol{v}} =-\boldsymbol{R}_{t}\left[\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{R}_{t}\left(\boldsymbol{n}_{a}-\delta \boldsymbol{b}_{a}\right) \\ \delta \dot{\boldsymbol{\theta}} =-\left[\boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega} \\ \delta \dot{\boldsymbol{b}}_{a}=0 \\ \delta \dot{\boldsymbol{b}}_{w}=0 \\ \end{array} δp˙​=δvδv˙=−Rt​[at​−bat​​]×​δθ+Rt​(na​−δba​)δθ˙=−[ωt​−bωt​​]×​δθ+nω​−δbω​δb˙a​=0δb˙w​=0​
过程噪声部分：(线性kalman)
KaTeX parse error: Expected 'EOF', got '&' at position 126: …array}\right] &̲&&& \boldsymbol…
过程噪声部分：(非线性kalman)
KaTeX parse error: Expected 'EOF', got '&' at position 126: …array}\right] &̲&&& \boldsymbol…

3.2 修改部分

在 kitti_filtering.yaml 中添加bias_flag 选项，以选择是否考虑使用随机游走

FILE: /catkin_ws/src/lidar_localization/config/filtering/kitti_filtering.yaml

 covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        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-4
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

Q矩阵

FILE: catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp

FUNCTION: ErrorStateKalmanFilter::ErrorStateKalmanFilter( )

  // c. process noise:  过程噪声  
  Q_.block<3, 3>(kIndexNoiseAccel, kIndexNoiseAccel) = COV.PROCESS.ACCEL * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(kIndexNoiseGyro, kIndexNoiseGyro) = COV.PROCESS.GYRO * Eigen::Matrix3d::Identity();
  if (COV.PROCESS.BIAS_FLAG ){
      Q_.block<3, 3>(kIndexNoiseBiasAccel, kIndexNoiseBiasAccel) = COV.PROCESS.BIAS_ACCEL * Eigen::Matrix3d::Identity();
      Q_.block<3, 3>(kIndexNoiseBiasGyro, kIndexNoiseBiasGyro) = COV.PROCESS.BIAS_GYRO * Eigen::Matrix3d::Identity();
  }

线性kalman B矩阵

FILE: catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp

FUNCTION: ErrorStateKalmanFilter::SetProcessEquation( )

  // b. set process equation for delta ori:
  B_.setZero();
  B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =    C_nb;
  B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =     Eigen::Matrix3d::Identity();
  if(COV.PROCESS.BIAS_FLAG){      //  判断是否考虑随机游走
    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =     Eigen::Matrix3d::Identity();
    B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)    =     Eigen::Matrix3d::Identity();
  }

非线性kalman B矩阵

FILE: catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp

FUNCTION: ErrorStateKalmanFilter::UpdateErrorEstimation( )

  MatrixB  B =  MatrixB::Zero();
  B.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro) * T;
  B.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro) *T;
  if(COV.PROCESS.BIAS_FLAG){
      B.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)* sqrt(T);
      B.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)  =      B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)* sqrt(T);
  }
3.3 结果
filter with bias random walkfilter without bias random walk
2021-10-09 18-51-32 的屏幕截图2021-10-09 19-12-02 的屏幕截图
2021-10-09 18-51-19 的屏幕截图2021-10-09 19-11-56 的屏幕截图
max 1.521893
mean 0.901894
median 0.899981
min 0.054675
rmse 0.919171
sse 3695.484911
std 0.177375		max 1.525035
mean 0.318024
median 0.251188
min 0.021889
rmse 0.381344
sse 638.991278
std 0.210439
3.4 结果分析

​ 从上述实验中，得知，有没有考虑随机游走影响并不是特别大，也可能是kitti数据集groundtruth 本身存在数据的问题。

4.调节ESKF Q、R参数，并进行数据对比

4.1 evo 评估指令

# set up session:
source devel/setup.bash
# save odometry:
rosservice call /save_odometry "{}"
# run evo evaluation:
# a. laser  输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt laser.txt -r full --plot --plot_mode xy  --save_results ./laser.zip
# b. fused 输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy  --save_results ./fused.zip
#c. 比较 laser  fused  一并比较评估
evo_res  *.zip -p

4.2 eskf 调参手段

本调试方法，得益于梓杰大佬的点拨，仅供参考~

主要调节 过程噪声 Q和观测噪声 R, 过程噪声与观测噪声一般在 kalman 迭代过程中保持不变。
KaTeX parse error: Expected 'EOF', got '&' at position 176: …{array}\right] &̲&&&\quad \bolds…
可通过evo_res 指令 对比观察 laser_odom 和 fused_odom 的 APE曲线。若两者的曲线大致一致，且误差较大，则证明，滤波后的轨迹大致和观测一样 。说明观测噪声®给的小了，导致滤波后，融合的轨迹对观测的置信度更高，所以出现滤波后的轨迹和观测更像，这时应该适当把观测噪声® 加大，过程噪声(Q) 减少。同理反向调节。

rosservice call /save_odometry "{}"
evo_ape kitti ground_truth.txt laser.txt -r full --plot --plot_mode xy  --save_results ./laser.zip
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy  --save_results ./fused.zip
evo_res  *.zip -p

for example : 以 parametr3 为例子

    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q
            gyro: 1.0e-5
            accel: 2.5e-4
            bias_accel: 2.5e-4
            bias_gyro: 1.0e-5
            bias_flag: true
        measurement: 			#观测噪声 R
            pose:		      
                pos: 1.0e-2
                ori: 1.0e-2
            pos: 1.0e-4
            vel: 2.5e-3

结果： 从下表可知，fused 结果误差比laser的误差要大，证明对现在对预测(IMU)的置信度较高，观测(LIDAR)的置信度较低。

调整： 提高观测的置信度，降低预测的置信度；相应地：减少观测噪声® , 增大过程噪声(Q)

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

               max      mean    median        min      rmse      sse       std
fused.txt  1.59676   0.90307  0.902652  0.0591278  0.922722   3725.8  0.189423
laser.txt  1.50034  0.901495  0.893804   0.161967  0.916559  3676.19  0.165489

2021-10-10 12-36-18 的屏幕截图

2021-10-10 12-37-47 的屏幕截图

2021-10-10 12-37-52 的屏幕截图

调整后 ： parameter 2为例子

    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q  增大
            gyro: 1.0e-4
            accel: 2.5e-3
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: true
        measurement: 			#观测噪声 R   减小
            pose:		      
                pos: 1.0e-4
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

结果：调整过后，fused 的轨迹误差降低，观测的置信度更高

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

               max      mean    median       min      rmse      sse       std
fused.txt  1.52392  0.901544  0.898788  0.054487  0.918868  3703.18  0.177588
laser.txt  1.50034   0.90192   0.89412  0.161967  0.916964  3687.85  0.1654

2021-10-10 13-41-25 的屏幕截图

2021-10-10 13-41-39 的屏幕截图

2021-10-10 13-41-44 的屏幕截图

4.3 不同参数结果对比

parameter 1
    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q
            gyro: 1.0e-5
            accel: 2.5e-4
            bias_accel: 2.5e-4
            bias_gyro: 1.0e-5
            bias_flag: true
        measurement: 			#观测噪声 R
            pose:		      
                pos: 1.0e-3
                ori: 1.0e-3
            pos: 1.0e-4
            vel: 2.5e-3

evo estimate

Lidar OnlyIMU-Lidar Fusion
2021-10-10 12-28-42 的屏幕截图2021-10-10 11-24-30 的屏幕截图
2021-10-10 12-28-35 的屏幕截图2021-10-10 11-24-13 的屏幕截图
max 1.500344
mean 0.901495
median 0.893804
min 0.161967
rmse 0.916559
sse 3676.188909
std 0.165489		max 1.542837
mean 0.902142
median 0.899435
min 0.058398
rmse 0.920858
sse 3714.999803
std 0.184716
parameter 2
    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q
            gyro: 1.0e-4
            accel: 2.5e-3
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: true
        measurement: 			#观测噪声 R
            pose:		      
                pos: 1.0e-4
                ori: 1.0e-4
            pos: 1.0e-4
            vel: 2.5e-3

evo estimate

Lidar OnlyIMU-Lidar Fusion
2021-10-10 13-39-06 的屏幕截图2021-10-10 13-35-33 的屏幕截图
2021-10-10 13-38-54 的屏幕截图2021-10-10 13-35-27 的屏幕截图
max 1.500344
mean 0.901920
median 0.894120
min 0.161967
rmse 0.916964
sse 3687.845753
std 0.165414		max 1.523916
mean 0.901544
median 0.898788
min 0.054487
rmse 0.918868
sse 3703.181677
std 0.177588
parameter 3

过程噪声调小，观测噪声调大，理论现象：IMU惯性解算的置信度更高

    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q
            gyro: 1.0e-5
            accel: 2.5e-4
            bias_accel: 2.5e-4
            bias_gyro: 1.0e-5
            bias_flag: true
        measurement: 			#观测噪声 R
            pose:		      
                pos: 1.0e-2
                ori: 1.0e-2
            pos: 1.0e-4
            vel: 2.5e-3

evo estimate

Lidar OnlyIMU-Lidar Fusion
2021-10-10 11-47-21 的屏幕截图2021-10-10 11-52-06 的屏幕截图
2021-10-10 11-47-15 的屏幕截图2021-10-10 11-52-00 的屏幕截图
max 1.500344
mean 0.901495
median 0.893804
min 0.161967
rmse 0.916559
sse 3676.188909
std 0.165489		max 1.596762
mean 0.903070
median 0.902652
min 0.059128
rmse 0.922722
sse 3725.795851
std 0.189423
parameter 4
    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q
            gyro: 1.0e-4
            accel: 2.5e-3
            bias_accel: 2.5e-3
            bias_gyro: 1.0e-4
            bias_flag: true
        measurement: 			#观测噪声 R
            pose:		      
                pos: 1.0e-5
                ori: 1.0e-5
            pos: 1.0e-4
            vel: 2.5e-3

evo estimate

Lidar OnlyIMU-Lidar Fusion
2021-10-10 14-01-26 的屏幕截图2021-10-10 14-04-16 的屏幕截图
2021-10-10 14-01-20 的屏幕截图2021-10-10 14-04-09 的屏幕截图
max 1.136680
mean 0.230984
median 0.163519
min 0.017465
rmse 0.289158
sse 367.057912
std 0.173951		max 1.166536
mean 0.244853
median 0.187717
min 0.010400
rmse 0.298805
sse 391.959520
std 0.171264
parameter 5

过程噪声调小，观测噪声调大，理论现象：IMU惯性解算的置信度更高

    covariance:
        prior:
            pos: 1.0e-6
            vel: 1.0e-6
            ori: 1.0e-6
            epsilon: 1.0e-6
            delta: 1.0e-6
        process:			#过程噪声 Q
            gyro: 1.0e-6
            accel: 2.5e-6
            bias_accel: 2.5e-6
            bias_gyro: 1.0e-6
            bias_flag: true
        measurement: 			#观测噪声 R
            pose:		      
                pos: 1.0e-2
                ori: 1.0e-2
            pos: 1.0e-4
            vel: 2.5e-3

evo estimate

Lidar OnlyIMU-Lidar Fusion
2021-10-10 14-20-26 的屏幕截图2021-10-10 14-22-39 的屏幕截图
2021-10-10 14-20-20 的屏幕截图2021-10-10 14-22-29 的屏幕截图
max 1.136680
mean 0.230928
median 0.163564
min 0.017465
rmse 0.289055
sse 367.297769
std 0.173854		max 1.596545
mean 0.317060
median 0.259280
min 0.029770
rmse 0.375757
sse 620.687408
std 0.201660

如下图 evo_res 对比可得知，过程噪声调小，观测噪声调大，理论现象：IMU惯性解算的置信度更高， fused的误差结果和曲线有别于odom，现象正确。

               max      mean    median        min      rmse      sse       std
fused.txt  1.59655   0.31706   0.25928  0.0297698  0.375757  620.687   0.20166
laser.txt  1.13668  0.230928  0.163564  0.0174648  0.289055  367.298  0.173854

2021-10-10 14-23-38 的屏幕截图

2021-10-10 14-23-44 的屏幕截图

2021-10-10 14-23-49 的屏幕截图

5. Gnss 定位原点初始化 bug 修复

现象描述：

1.多次运行第七章课程框架中kalman filter localization 时，结束后，laser_odom 的evo 精度评估数据都不太一样，ape mean 从 0.2 到 4，均有变化。

解决：

FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering_flow.cpp

初始化滤波部分，使用的是scancontext，可能是sc 初始化出现问题，导致每次数据都有差别，所以打算换成“第四章”的gnss初始化。

bool KITTIFilteringFlow::InitLocalization(void) {
  // ego vehicle velocity in body frame:
  Eigen::Vector3f init_vel = current_pos_vel_data_.vel;

   first try to init using scan context query:
   if (filtering_ptr_->Init(current_cloud_data_, init_vel,
                           current_imu_synced_data_)) {
  //   // prompt:
     LOG(INFO) << "Scan Context Localization Init Succeeded." << std::endl;
   }
  return true;
}

修改如下：

filtering_ptr_ -> Init()

在 FILE: catkin_ws/src/lidar_localization/src/filtering/kitti_filtering.cpp 框架中，已定义好 使用 scancontext 和 gnss 两种init pose的方式，无需我们再写

/*   scan contexxt  初始化  pose ，  输入 point cloud_*/
bool KITTIFiltering::Init(const CloudData &init_scan,
                          const Eigen::Vector3f &init_vel,
                          const IMUData &init_imu_data) {
  if (SetInitScan(init_scan)) {
    current_vel_ = init_vel;

    kalman_filter_ptr_->Init(current_vel_.cast<double>(), init_imu_data);

    return true;
  }

  return false;
}

/*   gnss   初始化  pose ，  输入  martix4d   current_gnss_pose_*/
bool KITTIFiltering::Init(const Eigen::Matrix4f &init_pose,
                          const Eigen::Vector3f &init_vel,
                          const IMUData &init_imu_data) {
  if (SetInitGNSS(init_pose)) {
    current_vel_ = init_vel;

    kalman_filter_ptr_->Init(current_vel_.cast<double>(), init_imu_data);

    return true;
  }

  return false;
}
修改 1 ： InitLocalization() 调用 Init() 方法

FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering_flow.cpp

bool KITTIFilteringFlow::InitLocalization(void) {
  // ego vehicle velocity in body frame:
  Eigen::Vector3f init_vel = current_pos_vel_data_.vel;

  // first try to init using scan context query:
  // if (filtering_ptr_->Init(current_cloud_data_, init_vel,
  //                          current_imu_synced_data_)) {
  //   // prompt:
  //   LOG(INFO) << "Scan Context Localization Init Succeeded." << std::endl;
  // }

  //   first try to init using gnss  init:
   if  (filtering_ptr_->Init(current_gnss_data_.pose, init_vel,
                            current_imu_synced_data_)){
    // prompt:
    LOG(INFO) << "Gnss Localization Init Succeeded." << std::endl;
  }

  return true;
}
修改2 ： 按照第四章gnss 初始化的部分修改, 初始化经纬高

通过记录获得建图时gnss 原点坐标为：

latitude   =  48.9825452359;
longitude =  8.39036610005;
altitude  = 116.382141113;

将建图原点的"经纬高" 转换为到导航系(ENU系)下的原点

FILE: lidar_localization/src/sensor_data/gnss_data.cpp

void GNSSData::InitOriginPosition() {
    geo_converter.Reset(48.982658,  8.390455, 116.396412);         //   设置原点

    origin_longitude = longitude;
    origin_latitude = latitude;
    origin_altitude = altitude;
    origin_position_inited = true;
}
修改3 ： 修复 current_gnss_data_ 没有更新问题
现象：按照如上方法，修改后，运行程序，初始化的gnss pose，发现定位有问题，通过打印 current_gnss_data_.pose，发现无论数据集在哪个地方开始播放，init gnss pose的值都是一个 4x4 的 单位阵，明显不正确。
问题所在： current_gnss_data_.pose 在本章节的框架中，只在save_odometry 保存groundtruth.txt 数据集被赋值，在其他地方没有被更新。

原框架中只在save_odometry 中更新 current_gnss_data_.pose

bool KITTIFilteringFlow::SaveOdometry(void) {
  if (0 == trajectory.N) {
    return false;
  }

  // init output files:
  std::ofstream fused_odom_ofs;
  std::ofstream laser_odom_ofs;
  std::ofstream ref_odom_ofs;
  if (!FileManager::CreateFile(fused_odom_ofs,
                               WORK_SPACE_PATH +
                                   "/slam_data/trajectory/fused.txt") ||
      !FileManager::CreateFile(laser_odom_ofs,
                               WORK_SPACE_PATH +
                                   "/slam_data/trajectory/laser.txt") ||
      !FileManager::CreateFile(ref_odom_ofs,
                               WORK_SPACE_PATH +
                                   "/slam_data/trajectory/ground_truth.txt")) {
    return false;
  }

  // write outputs:
  for (size_t i = 0; i < trajectory.N; ++i) {
    // sync ref pose with gnss measurement:
    while (!gnss_data_buff_.empty() &&
           (gnss_data_buff_.front().time - trajectory.time_.at(i) <= -0.05)) {
      gnss_data_buff_.pop_front();
    }

    if (gnss_data_buff_.empty()) {
      break;
    }

    current_gnss_data_ = gnss_data_buff_.front();

    const Eigen::Vector3f &position_ref =
        current_gnss_data_.pose.block<3, 1>(0, 3);
    const Eigen::Vector3f &position_lidar =
        trajectory.lidar_.at(i).block<3, 1>(0, 3);

    if ((position_ref - position_lidar).norm() > 3.0) {
      continue;
    }

    SavePose(trajectory.fused_.at(i), fused_odom_ofs);
    SavePose(trajectory.lidar_.at(i), laser_odom_ofs);
    SavePose(current_gnss_data_.pose, ref_odom_ofs);
  }

  return true;
}
解决

在 InitLocalization() 函数中，更新current_gnss_data_

注意：这里有个地方需要注意的是，GNSS数据一般前几帧都是不准确，所以我们舍弃前三帧, 取第四帧, 需要对 SetInitGNSS 函数进行修改一下，修改为使用第四帧的gnss数据进行gnss pose 初始化。

FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering_flow.cpp

bool KITTIFilteringFlow::InitLocalization(void) {
  // ego vehicle velocity in body frame:
  Eigen::Vector3f init_vel = current_pos_vel_data_.vel;

  // first try to init using scan context query:
  // if (filtering_ptr_->Init(current_cloud_data_, init_vel,
  //                          current_imu_synced_data_)) {
  //   // prompt:
  //   LOG(INFO) << "Scan Context Localization Init Succeeded." << std::endl;
  // }

  //   first try to init using gnss  init:
  static int gnss_count = 0;
  if(!(gnss_count  >3)){
        current_gnss_data_ =  gnss_data_buff_.at(gnss_count);            //   舍弃GNSS的第三帧数据
        // std::cout  << " gnss_data_buff_   "   <<  gnss_count  << "  "   <<  current_gnss_data_.pose << std::endl;
  }
  gnss_count  ++ ;
   if  (filtering_ptr_->Init(current_gnss_data_.pose, init_vel,
                            current_imu_synced_data_)){
    // prompt:
    LOG(INFO) << "Gnss Localization Init Succeeded." << std::endl;
  }

  return true;
}

FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering.cpp

bool KITTIFiltering::SetInitGNSS(const Eigen::Matrix4f &gnss_pose) {
  static int gnss_cnt = 0;

  current_gnss_pose_ = gnss_pose;

  // if (gnss_cnt == 0) {              //   一般gnss 数据，前几帧都不准，所以取第三帧
  //   SetInitPose(gnss_pose);
  // } else if (gnss_cnt > 3) {
  //   has_inited_ = true;
  // }
  if (gnss_cnt > 3) {
    has_inited_ = true;
  }
  SetInitPose(gnss_pose);
  gnss_cnt++;
  return true;
}

结果：

1.能够成功实现gnss 初始化，并在地图任意一点启动，初始化成功。

2.经过6次，同一套kalman 参数的实验，得出的evo 结果大致一样，可以认为修复了 每次启动evo评估结果不同的问题。

image-20211012133329089

六次同一参数 evo 评估结果 ，大致相等

2021-10-12 09-52-34 的屏幕截图
2021-10-12 10-10-35 的屏幕截图
2021-10-12 10-27-25 的屏幕截图
2021-10-12 10-56-23 的屏幕截图
2021-10-12 12-32-51 的屏幕截图
2021-10-12 13-21-40 的屏幕截图

​ edited by kaho 2021.10.12

KaHoWong
关注
专栏目录
【状态估计与多传感器数据融合】卡尔尔曼滤波(KF)、扩展卡尔曼滤波器(EKF)、 迭代扩展卡尔曼滤波器(IEKF)（附C++代码）
qq_35635374的博客
11-04 4910
系列文章目录 提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加 TODO:写完再整理 文章目录系列文章目录前言一、使用卡尔曼的作用与特点1.卡尔曼滤波的作用2.卡尔曼滤波的算法的特点二、卡尔曼中的各类矩阵的含义1.协方差矩阵P（1）协方差矩阵作用（2）协方差矩阵的含义（3）协方差矩阵的性质（4）噪声协方差矩阵P的传递2.观测矩阵H（1）观测矩阵H的作用（2）观测矩阵H举例（3）注意3.观测协方差矩阵R（1）观测协方差矩阵R的作用4.状态转移矩阵F（1）状态转移矩阵F作用5.控制矩阵B（1）
【多传感器融合定位之基于滤波的融合】
weixin_43389152的博客
05-21 1557
目前融合定位常用的方法主要分为两大块，一块是基于滤波的方法，另一块是基于图优化的方法。本节主要针对滤波方法展开详细介绍。
多传感器融合（三）
吴建明wujianming_110117
05-26 1920
多传感器融合（三） 十三．前融合与后融合 多传感器融合技术中的前融合、后融合 后融合算法典型结构 后融合算法： 1、每个传感器各自独立处理生成的目标数据。 2、每个传感器都有自己独立的感知，比如激光雷达有激光雷达的感知，摄像头有摄像头的感知，毫米波雷达也会做出自己的感知。 3、当所有传感器完成目标数据生成后，再由主处理器进行数据融合。 前融合算法典型结构 前融合算法： 1、只有一个感知的算法。对融合后的多维综合数据进行感知。 2、在原始层把数据都融合在一起，融合好的数据就好比是一个Super传感器，而且这个
Lidar_IMU_Localization 项目教程
最新发布
gitblog_00925的博客
08-16 911
Lidar_IMU_Localization 项目教程 Lidar_IMU_LocalizationLidar-IMU Localization System with Prior Map Constraint and Lio Constraint项目地址:https://gitcode.com/gh_mirrors/li/Lidar_IMU_Localization 项目介绍 Lidar_IM...
深蓝学院-多传感器融合定位-第7章作业
weixin_42113967的博客
03-29 3359
深蓝学院-多传感器融合定位-第7章作业 作业解析及代码
多传感器融合（4）——基于滤波的融合方法
weixin_51326570的博客
07-05 334
多传感器融合（4）——基于滤波的融合方法
自动驾驶中的多传感器融合
3D视觉工坊
11-24 3230
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达来源 |清研车联编辑|焉知自动驾驶多传感器融合面临的主要挑战是如何将收集来的大量数据集中在一起，并做出正确决策。多传感器融合多...
多传感器融合（5）——基于滤波的融合方法进阶
weixin_51326570的博客
07-05 289
多传感器融合（5）——基于滤波的融合方法进阶
深蓝学院 《多传感器融合定位》 第2章作业
Jevin-L的博客
11-17 3851
1. 闭环修正及精度评价 提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿情况下,闭环检测的实现难度会加大。要求使用前面讲过的scan context,实现此功能。并和已有的图优化功能完成对接,实现修正。并最终给出修正前后的轨迹精度对比。 2. 位姿初始化 提供的工程框架中已经给出了位姿初始化功能,但是是在起始位置的,并且是基于已知粗略位姿的。要求实现地图中任意位置的位姿初始化,可以从三种难度等级中任选一种,难度越高,得分越高。 ...
多传感器融合定位课程学习记录.zip
01-24
多传感器融合定位课程学习记录 Contents ...第7章-基于滤波的融合方法I 第8章-基于滤波的融合方法II 第9章-基于图优化的建图方法 第10章-基于图优化的定位方法 第11章-多传感器时空标定 第12章-大作业
多传感器信息融合matlab程序,传感器信息融合：MATLAB程序实现
weixin_29366307的博客
03-16 2250
传感器信息融合：MATLAB程序实现作者：赵小川 等著出版时间：2014年版内容简介本书全面系统地讲解了传感器信息融合的相关内容。本书在介绍传感器信息融合基本原理的同时，从遥感、医学、生活、探测、导航等领域分析了多个传感器信息融合的实例，书中每一个关于传感器信息融合的实例都附有Matlab源代码实例和详细注解。本书可作为本科毕业设计、研究生学术论文的资料；也可为从事信息处理、计算机技术、传感器等研...
多传感器融合
11-14
多传感器融合处理，课件记录。
多传感器数据融合
09-14
利用数据融合技术，在数据库的支持之下，充分发挥各传感器信息组合的潜能，为客观复现战术态势提供保障．文中给出了用于潜艇的数据融合算法的结构设计
SensorFusionWork：多传感器融合课程作业代码
03-03
基于激光的多传感器融合定位与建图 深蓝学院学习环境。 概述 本Repo为基于ROS melodic @ Ubuntu 18.04的学习环境。 为了节约安装配置的时间： 部分依赖项由于网络原因难以取得 新依赖的约会可能会导致与本地依赖的冲突 上述环境将以Docker映像的形式提供，实现与本机PC环境的隔离。本项目Native PC的操作系统选择Ubuntu 。 Windows与Mac不推荐使用，若有意尝试，请自行搜索相关解决方案。 安装Ubuntu 首先请确保您可以访问Ubuntu开发环境。如果没有Ubuntu环境，请按照单击指南，在本地PC上安装配置Ubuntu环境。 获取Docker开发环境 本课程推荐使用官方Docker环境完成课程学习。 Docker提供了一个轻量级的标准化开发环境，能够避免环境配置差异导致许多问题。 工作空间 当Native PC与Course Docker Env
基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业
dushib的博客
02-13 3295
1、更新里程计信息 void ErrorStateKalmanFilter::UpdateOdomEstimation( Eigen::Vector3d &linear_acc_mid, Eigen::Vector3d &angular_vel_mid) { // // TODO: this is one possible solution to previous chapter, IMU Na
多传感器数据融合算法综述
热门推荐
ZXQHBD的博客
04-06 7万+
多传感器数据融合是一个新兴的研究领域，是针对一个系统使用多种传感器这一特定问题而展开的一种关于数据处理的研究。多传感器数据融合技术是近几年来发展起来的一门实践性较强的应用技术，是多学科交叉的新技术，涉及到信号处理、概率统计、信息论、模式识别、人工智能、模糊数学等理论。 近年来，多传感器数据融合技术无论在军事还是民事领域的应用都极为广泛。多传感器融合技术已成为军事、工业和高技术开发等多方面关心
多传感器融合（一）
吴建明wujianming_110117
05-26 1382
多传感器融合（一） 一．概述 “传感器融合技术”号称自动驾驶中的核心技术。 传感器是汽车感知周围的环境的硬件基础，在实现自动驾驶的各个阶段都必不可少。 自动驾驶离不开感知层、控制层和执行层的相互配合。摄像头、雷达等传感器获取图像、距离、速度等信息，扮演眼睛、耳朵的角色。 控制模块分析处理信息，并进行判断、下达指令，扮演大脑的角色。车身各部件负责执行指令，扮演手脚的角色。而环境感知是这一切的基础， 因此传感器对于自动驾驶不可或缺。 二．多传感器融合的必要性 为什么一定要多传感器融合呢？主要是扬长避短、冗余设
多传感器信息融合
weixin_43677825的博客
12-10 4652
转载：https://blog.csdn.net/young951023/article/details/78418952 多传感器信息融合分为集中式融合结构，分散式融合结构和分级融合结构。 随着机器人技术的不断发展，机器人的应用领域和功能有了极大的拓展和提高。智能化已成为机器人技术的发展趋势，而传感器技术则是实现机器人智能化的基础之一。由于单一传感器获得的信息非常有限，而且，还要受到自身品质...
评论 21
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值