[人工智能]多传感器融合定位 第八章 基于滤波的融合方法进阶

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

参考博客：深蓝学院-多传感器融合定位-第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

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 融合编码器模型

编码器可以额外提供一个轮速里程计，在不是很高频的编码器情况下，可以做为一个约束边，通过提供线速度(b系) 到上一章节的观测方程中，进行kalman 融合。

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

2.2 添加运动约束模型(伪观测)

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

2.3 代码编写

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

2.3.1 调用 CorrectErrorEstimation

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

2.3.2 CorrectErrorEstimationPoseVel 计算 Y ，G ，K

注意：

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() ;
}

2.3.2 将更新后的vel_b 写到txt 中，进行比较

/*********************write  data  to  txt********************/
#include <list>
#include <sstream>
#include <fstream>
#include <iomanip>
#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

2.4 EVO 评估 及 update后的v_b 比较

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    

2.4.1 ape 比较

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

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

fused 无运动约束	fused_cons 运动约束

2.4.2 使用运动模型后 vel_y vel_z (body系) 比较

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

2.4.1 matplotlib 可视化数据

为了方便可视化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()

2.4.2 vel_y 比较

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

2.4.3 vel_z 比较

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

2.4.4 结论

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

3. GNSS + Oodm 观测模型

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

3.1 公式推导

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

观测量 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??\begin{array}{c}{\mathbf{v}}_m\\ 0\\ 0\end{array}?$$
观测方程
$$\mathbf{y}={\mathbf{G}}_t\delta \mathbf{x}+{\mathbf{C}}_t\mathbf{n}$$

$$\begin{array}{rl}{\mathbf{G}}_t& =\left[\begin{array}{ccccc}{\mathbf{I}}_3& 0& 0& 0& 0\\ 0& {\mathbf{R}}_{bw}& {\left[{\mathbf{v}}^b\right]}_{\times }& 0& 0\end{array}\right]\\ & \\ & \\ {\mathbf{C}}_t& =\left[\begin{array}{cc}{\mathbf{I}}_3& 0\\ 0& {\mathbf{I}}_3\end{array}\right]\end{array}$$

$$\mathbf{n}={\left[\begin{array}{llllll}{n}_{\delta {\bar{p}}_x}& n_{\delta {\bar{p}}_y}& n_{\delta {\bar{p}}_z}& n_{\delta {\bar{v}}_x^b}& n_{\delta {\bar{v}}_y^b}& n_{\delta {\bar{v}}_z^b}\end{array}\right]}^T$$

3.2 CorrectErrorEstimationPosiVel ， 因为只是少了ori这个观测量，所以在(lidar + encoder)的观测上删掉orii这一行的观测就可以。

void ErrorStateKalmanFilter::CorrectErrorEstimationPosiVel(       //  position + velocity
    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  velocity
    //
    Eigen::Vector3d  v_b_  =  {v_b[0],  0,  0};           //  measurment   velocity  (body  系) ， 伪观测 (vy 、vz  = 0)

    Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);
    Eigen::Vector3d  dv  =   pose_.block<3,  3>(0, 0).transpose() *vel_  -  v_b ;                  //  delta v  ,  v_x 来自轮速里程计
    // TODO: set measurement equation:
    YPosiVel_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
    YPosiVel_.block<3, 1>(3, 0)  =  dv;           //   delta   velocity  
    Y = YPosiVel_;
    //   set measurement  G
    GPosiVel_.setZero();
    GPosiVel_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
    GPosiVel_.block<3, 3>(3, kIndexErrorVel)   =   pose_.block<3,  3>(0, 0).transpose();
    GPosiVel_.block<3, 3>(3, kIndexErrorOri)   =   Sophus::SO3d::hat( pose_.block<3,  3>(0, 0).transpose() *vel_  ) ;
    G  =   GPosiVel_;     
    //   set measurement  C
    CPosiVel_.setIdentity();
    Eigen::MatrixXd  C  =   CPosiVel_;
    // TODO: set Kalman gain:
    Eigen::MatrixXd R = RPosiVel_;    //  观测噪声
    K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * R*  C.transpose() ).inverse() ;
}

3.3 参数调整，及重力参数修改

3.3.1 pos vel 观测误差参数调整

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

按照GeYao 助教所说，

	最后这里在运行的时候需要提醒一下大家, 左边的两张图是gnss-ins-sim生成测量数据时候的误差等级, 课程中默认使用的都是中等精度, 也就是说gps位置的协方差应该在2.5e-1左右, 而编码器应该在2.5e-3左右, 而右边是我们的作业使用的默认配置文件中的误差等级. 可以看出gps的实际误差与我们给的先验值的差距比较巨大, 这就是为什么有些同学在程序正确的情况下rviz中显示的姿态抖动非常厉害的原因. 建议大家在左边这两个值的基础上进行调参.

所以，我们把 pos 和 vel 的measurment cov 调整为

        measurement:
            pose:
                pos: 1.0e-4
                ori: 1.0e-4
            pos: 2.5e-1 # 1.0-4
            vel: 2.5e-3

3.3.2 重力参数调整

这里生成的仿真数据的重力加速度和kitti的重力加速度方向不太一致，原因，仿真的数据集中传感器的Z轴正方向是朝上的，所以imu_sim_ins accel_z 读出来的数值是 -g。通过 指令rqt_bag 查看传感器读取的重力加速度，写到yaml中。

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

gravity_magnitude: -9.794216 

3.3.3 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 xyz  --save_results ./fused.zip
# b. fused_vel 速度观测  输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt gnss.txt -r full --plot --plot_mode xyz  --save_results ./gnss.zip
#e. 比较 laser  fused  一并比较评估
evo_res  *.zip --use_filenames -p    

GNSS + Odom	GNSS Only

3.4 结论通过对比上述实验

可得知，融合了odom轮速里程计后，数据波动得到大幅度抑制。

? edit by kaho 2021.10.22