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

参考博客：深蓝学院-多传感器集成定位-第7章操作

这个作业:主要参考张松鹏大佬的代码，因为大佬的分析太好了。为了保留记录，以下大部分文字都摘自老板的原话~

代码下载：https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/第五章惯性导航原理及误差分析/imu_tk

1.环境配置

1.1 protoc 版本问题

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

解决方案：需要安装新版本proto 3.15x，以第四章的形式生成相应的文件。

按照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 库

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)未定义的引用**

参考网址： 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*)未定义的引用

#解决方案： 打开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 流程框图

2.2 代码结构

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

预测部分接收imu数据,基于惯性解算更新名义值,基于状态方程更新误差值。
观测部分同时接收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函数

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。

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个公式

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_ ;           
        标签： 传感器refqae26传感器tk7480传感器sc25v传感器
 锐单商城拥有海量元器件数据手册、
          IC替代型号，打造
          电子元器件IC百科大全！

 热门文章
          
  单片电力系统(MPS) EVQ1922评估板的介绍、特性、及应用
 
                          单片电源系统(MPS) EVQ1922评估板以半桥结构驱动两个n通道功率mosfet，并在...
                        
PIC16F18056 & PIC16F18076 8位微控制器的介绍、特性、及应用
微芯片技术AVR DD mcu的介绍、特性、及应用
Telink B91通用入门套件的介绍、特性、及应用
Littelfuse 0805L系列表面贴装PTC熔断器的介绍、特性、及应用
Littelfuse 1210L系列可复位PPTC熔断器的介绍、特性、及应用
Melexis MLX90397微功率三轴 3D磁强计的介绍、特性、及应用
单片电力系统(MPS) EVQ3367A评估板的介绍、特性、及应用
单片电源系统(MPS) MPQ3367A 6通道Boost WLED驱动器的介绍、特性、及应用
Sensirion SLF3C-1300F紧凑型液体流量传感器的介绍、特性、及应用
TRACO Power TMR 12WI隔离12W DC/DC转换器模块的介绍、特性、及应用
 热门型号
          
 UCC383TDKTTT-5
LP2981AIM5X-3.0/NOPB
S-1132B32-U5T1G
LP2989IMM-5.0/NOPB
ADP3335ARMZ-5-REEL
TPS70916DBVT
R1191N027B-TR-FE
ADP3335ARMZ-5-REEL
S-1132B33-U5T1G
MIC2211-SSYML-TR
 锐单商城 - 一站式电子元器件采购平台

 首页 选型 品牌 购物车 我的
   
   
  
  
 
  搜索
 最近热搜
 
 历史搜索 清除历史记录

资讯详情

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