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

代码下载：https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/第八章 基于滤波的融合方法2

使用前几章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  

之后，使用上述步骤生成.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager
同名文件。
将.pb.cpp文件更换(注:需要剪切，以确保config新生成的文件转移到相应的目录中，不能重复）lidar_localization/src/models/scan_context_manager同名文件。

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) 

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) 

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

编码器可以提供额外的轮速里程计，在不是很高频的编码器下，通过提供线速(b系) 在上一章的观测方程中进行kalman 融合。

注意： 1. 编码器提供的线度是比较准确的，但是角速度不太准确(转弯存在打滑现象)，角速度不宜用作观测边。
2.编码器参与的融合,还有另外一种融合方式,即编码器不当做观测使用,而是和IMU一起进行状态预测,然后再与
其他传感器提供的观测进行滤波融合。具体思路为IMU提供角速度,编码器提供线速度,假设二者频率相同、时间戳已对齐,且外参已标定,那么它们可以直接认为是一个可以通过解算得到姿态、位置的新传感器。

车子坐标系(前左上),在没有编码器硬件的基础上，可以使用四轮底盘本身的运动属性进行约束，正常情况下，车子只有前向(x)的速度，观测上，y 和 z 向的观测都为0。所以可在观测中添加 Vy 、 Vz 两个约束边。

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

  case MeasurementType::POSE_VEL:
    CorrectErrorEstimationPoseVel(
        measurement.T_nb, 
        measurement.v_b, measurement.w_b,
         Y, G, K
    );
    break;

注意：

1. CorrectErrorEstimationPoseVel 函数中输入的v_b 取自 惯导，因为KITTI数据集并没有轮速里程计信息，所以，如果在做融合运动约 束模型时，输入的measurment velocity 为v_b ,可以认为观测是取自ins的。

2. 用车子的运动约束(vy = 0;vz = 0）做伪观测约束， 如下所示：

   // Eigen::Vector3d v_b_ = {v_b[0], 0, 0}; // measurment velocity (body 系) ， 伪观测 (vy 、vz = 0)
   Eigen::Vector3d  v_b_  =  v_b;           // measurment velocity (body 系) ， 融入速度 (vx 取自 惯导)
   

void ErrorStateKalmanFilter::CorrectErrorEstimationPoseVel(          // 计算 Y ，G ，K
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &w_b,
    Eigen::VectorXd &Y, Eigen::MatrixXd &G, Eigen::MatrixXd &K
) { 
        
    //
    // TODO: set measurement: 计算观测 delta pos 、 delta ori
    //
    // Eigen::Vector3d v_b_ = {v_b[0], 0, 0}; // measurment velocity (body 系) ， 伪观测 (vy 、vz = 0)
    Eigen::Vector3d  v_b_  =  v_b;           // measurment velocity (body 系) ， 融入速度 (vx 取自 惯导)

    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) ;
    Eigen::Vector3d  dv  =   T_nb.block<3,  3>(0, 0).transpose() *vel_  -  v_b_ ;                  // delta v 严格意义上来说，这里的观测是，惯导给的vx
    // TODO: set measurement equation:
    Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );
    YPoseVel_.block<3, 1>(0, 0)  =  dp;          // delta position 
    YPoseVel_.block<3, 1>(3, 0)  =  dv;           // delta velocity s
    YPoseVel_.block<3, 1>(6, 0)  =  dtheta;          // 失准角
    Y = YPoseVel_;
    // set measurement G
    GPoseVel_.setZero();
    GPoseVel_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
    GPoseVel_.block<3, 3>(3, kIndexErrorVel)   =   T_nb.block<3,  3>(0, 0).transpose();
    GPoseVel_.block<3, 3>(3, kIndexErrorOri)   =   Sophus::SO3d::hat( T_nb.block<3,  3>(0, 0).transpose() *vel_  ) ;
    GPoseVel_.block<3 ,3>(6, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();        
    G  =   GPoseVel_;     
    // set measurement C
    CPoseVel_.setIdentity();
    Eigen::MatrixXd  C  =   CPoseVel_;
    // TODO: set Kalman gain:
    Eigen::MatrixXd R = RPoseVel_;    // 观测噪声
    K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * RPoseVel_*  C.transpose() ).inverse() ;
}

/*********************write data to txt********************/
#include 
#include 
#include 
#include 
#define DEBUG_PRINT 

std::ofstream  fused;
std::ofstream  fused_vel;
std::ofstream  fused_cons;

bool CreateFile(std::ofstream& ofs, std::string file_path) { 
        
    ofs.open(file_path, std::ios::out);                          // 使用std::ios::out 可实现覆盖
    if(!ofs)
    { 
        
        std::cout << "open csv file error " << std::endl;
        return  false;
    }
    return true;
}
/* write2txt */
void WriteText(std::ofstream& ofs, Eigen::Vector3d data){ 
        
    ofs << std::fixed  <<  data[0] << "\t" <<  data[1]  << "\t "  <<  data[2] <<  "\t"  <<  std::endl;
}

调用

  /*init debug print file */
  #ifdef DEBUG_PRINT
      char fused_path[] = "/home/kaho/shenlan_ws/src/lidar_localization/slam_data/trajectory/fused.txt";
      char fused_vel_path[] = "/home/kaho/shenlan_ws/src/lidar_localization/slam_data/trajectory/fused_vel.txt";
      char fused_cons_path[] = "/home/kaho/shenlan_ws/src/lidar_localization/slam_data/trajectory/fused_cons.txt";
      CreateFile(fused, fused_path );
  #endif 
  ------------------------------------------------------------------------------------
      /*print vel(body)*/
  #ifdef DEBUG_PRINT 
      Eigen::Vector3d  vel_print_ =   pose_.block<3, 3>(0, 0).transpose() *  vel_;       // convert kalman filter velocity to body axis
      WriteText(fused, vel_print_ );   // 写进文件夹
  #endif

evo评估指令

# set up session:
source devel/setup.bash
# save odometry:
rosservice call /save_odometry "{}"
# run evo evaluation:
# a. fused 没有输入运动模型 输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy  --save_results ./fused.zip
# b. fused_vel 速度观测 输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy  --save_results ./fused_vel.zip
# c. fused_cons 运动约束伪观测 输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy  --save_results ./fused_cons.zip
#e. 比较 laser fused 一并比较评估
evo_res  *.zip --use_filenames -p    

fused 为无运动约束， fused_vel 为加入惯导速度观测，fused_cons为加入运动约束(伪观测)

通过观察ape曲线，觉得并无太大差别

fused 为无运动约束， fused_vel 为加入惯导速度观测，fused_cons为加入运动约束(伪观测)

为了方便可视化v_x、v_z ，这里使用matplotlib 可视化数据

FILE: /home/kaho/shenlan_ws/visual/ main.py

# import necessary module
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import numpy as np

fused = np.loadtxt("fused_withoust_cons.txt")
fused_vel_data = np.loadtxt("fused_vel.txt")
fused_cons_data = np.loadtxt("fused_cons.txt")

# x = fused_vel_data[:,0]
y_fused = fused[:,1]
z_fused = fused[:,2]

y_fused_vel = fused_vel_data[:,1]
z_fused_vel = fused_vel_data[:,2]

y_fused_cons_data = fused_cons_data[:,1]
z_fused_cons_data = fused_cons_data[:,2]

plt.plot(y_fused, c='r', label ="y_fused")
plt.plot(y_fused_vel, c='g', label ="Y_fused_vel")
# plt.plot(y_fused_cons_data, c='b', label ="y_fused_cons")
plt.legend();
plt.title('y velocity ',fontsize=18,color='y')
plt.show()

plt.plot(z_fused, c='r', label ="z_fused")
# plt.plot(z_fused_vel, c='g', label ="z_fused_vel")
plt.plot(y_fused_cons_data, c='b', label ="y_fused_cons")
plt.legend();
plt.title('z velocity ',fontsize=18,color='y')
plt.show()

红色为无运动约束，绿色为fused 加入速度观测，蓝色为fused_cons为加入运动约束，可明显看出蓝色曲线大部分时间处于红色曲线之下，说明使用运动约束后，数据的波动性更小。

红色为无运动约束，绿色为fused 加入速度观测，蓝色为fused_cons为加入运动约束，可明显看出蓝色曲线大部分时间处于红色曲线之下，说明使用运动约束后，数据的波动性更小。

实验结果大致和理论的一致，添加了运动模型后，整体效果有了轻微的改善，但是速度的波动得到了大幅度的抑制！！！

参照 (Estimate: imu, Measurment: lidar、odom ) 模型，推导出(Estimate: imu, Measurment: gnss、odom )，观测方程中，gnss 提供position、odom

状态量
$$\delta \mathbf{x}=\left[\begin{array}{l}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_{\omega }\end{array}\right]$$

观测量 GPS + Encoder 做观测， GPS 提供position， Encoder 提供velocity
$$\mathbf{y}=\left[\begin{array}{c}\delta \overline{\mathbf{p}}\\ \delta {\overline{\mathbf{v}}}^b\end{array}\right]$$
观测值的获取
$$\delta {\overline{\mathbf{v}}}_b={\tilde{\mathbf{v}}}^b-{\mathbf{v}}^b={\tilde{\mathbf{R}}}_{bw}{\tilde{\mathbf{v}}}^w-\left[\begin{array}{c}{\mathbf{v}}_m\\ 0\\ 0\end{array}\right]$$
观测方程
$$\mathbf{y}={\mathbf{G}}_t\delta \mathbf{x}+$$