ROS Ethzasl MSF Framework （多传感器扩展卡尔曼融合框架 ）安装编译使用教程（已测试成功）

Reference

MSF Framework简介

本教程是Ethzasl MSF Framework的简介。 使用离线数据集，了解框架的工作原理。
Ethzasl MSF Framework 是基于扩展卡尔曼滤波器（EKF）的多传感器融合框架。 该框架本质上分为两个EKF步骤：预测和更新。 预测是基于系统模型（即微分方程）和IMU读数进行的。 我们不将IMU读数作为更新度量，因为这将需要IMU速率（在某些情况下高达1kHz！）下的矩阵求逆和乘法。

在上图中，红色部分是传感器读数（来自IMU进行预测或来自另一个传感器进行更新）。 蓝色部分是更新传感器类型更改的部分。 黑色部分是恒定（核心）部分，只要使用IMU进行状态传播和使用相同的状态向量，它们在分析上都保持不变。在代码中，

p for pwi：世界框架中的imu位置

v表示vwi：世界框架中的imu速度

q for qwi：世界框架中的imu态度

b_w for bw：陀螺仪偏置

b_a表示ba：加速度计偏差

L代表λ：pmetric *的视觉比例因子*λ= psensor

q_wv for q ,, vw：更新传感器参考系与世界参考系之间的姿态

q_ci for qic：IMU和更新传感器之间的态度

p_ci for pic：IMU和更新传感器之间的距离

如ICRA11论文所述，在EKF中使用错误状态表示法来简化四元数的计算。 误差四元数由δq= [1θxθyθz] T表示，其中在误差状态表示中将3矢量θ用作状态。 这导致了25个状态的状态向量。

从软件的角度来看，该框架将所有常量（核心）部分封装在msf_core库中。 然后，我们通过msf_updates中的模块添加依赖于更新传感器的所有零件。
对于本教程，除了下载此数据集（3.7MB）外，不需要任何特殊准备。 它包含用于EKF预测步骤的IMU数据（线性加速度和角速度）。 对于更新步骤，它包含来自6DoF全局姿态传感器的数据（在这种情况下，它是从Vicon系统读取的位置）。

框架的核心部分（msg_core）设计用于最通用的更新传感器：相对于其自己的参考系的任意缩放的6DoF姿势测量，相对于（重力对齐）固定导航框的位置和姿态会发生漂移。 例如，可以通过使用来自单眼视觉测距框架（例如ethzasl_ptam）的6DoF姿势来表示这种传感器。

上图描述了该框架的框架设置：每个框架都与特定的平移p和旋转q（从四元数）连接到另一个框架。 世界框架是我们的机器人将在其中导航的框架。 IMU和摄像机框架位于机器人上相应传感器的中心。 视觉框架是视觉部分（例如ethzasl_ptam）对3D特征进行三角剖分并相对于视觉部分计算6DoF相机姿态的框架。 为了表示在每个视觉里程表框架中发生的漂移（即视觉范围，位置和姿态中的漂移），在位置pvw和姿态qvw中引入了所谓的漂移状态。

由于该框架没有全局位置测量值，因此无法观察到位置漂移。因此，不在此框架中使用此状态。偏航中的姿态漂移也是如此。这意味着框架中的的“固定”世界框架将漂移并且与视觉框架一起偏航。但是，可以观察到滚动和俯仰漂移，并且可以确保世界框架至少保持重力对齐-这对于MAV平台至关重要。请参阅本作者的博士学位论文中有关可观察性的更详细讨论（也适用于多传感器系统）。
由于无法观察到位置漂移，因此仅在此框架中将其删除。但是，我们正在使用四元数，并且无法通过移除状态变量将无法观察到的偏航简单地设置为零。因此，我们引入了一种人工测量，以使世界框架的偏航与视觉框架保持一致。根据您使用的更新传感器的类型，可能需要进行其他人工测量，因为其他状态可能无法观察到。
例如，在本教程中以6DoF位置更新传感器为例，IMU与“摄像机”（即位置传感器）之间的姿态是无关紧要的，因此无法观察。此外，假设Vicon框架是重力对齐的并且没有漂移，则“视觉”框架（即Vicon框架）和世界框架之间的姿态漂移状态也是多余的。 “视觉”比例状态也是如此，但是，为了进行健全性检查，我们将该状态保留在状态向量中-应该收敛到1。

编译MSF Viconpos传感器框架

1.我的编译环境
Ubuntu 16.04+ROS Kinetic
2.新建工作空间

mkdir -p /MSF/src
cd /MSF/src
catkin_init_workspace  #初始化工作空间
cd ..
catkin_make    #编译
cd MSF/src/

3.下载所有的依赖库和MSF框架到工作空间src目录下

git clone https://github.com/ethz-asl/glog_catkin.git   #下载 glog

git clone https://github.com/catkin/catkin_simple.git   #下载 catkin_simple

git clone https://github.com/ethz-asl/asctec_mav_framework.git  #下载asctec_mav_framework
git clone  https://github.com/ethz-asl/mav_comm.git  #安装mav_comm包

git clone https://github.com/ethz-asl/ethzasl_msf.git  #最后下载 Ethzasl MSF Framework 框架源代码

git clone https://github.com/google/glog  #下载glog
sudo apt-get install autoconf automake libtool  #安装automake工具
#进入源码根目录（glog_master文件夹）编译和安装
./autogen.sh
./configure
 make -j 24
sudo make install

接下来再编译一次， 编译过程中会报错，基本是权限问题，chmod 777 packagename命令改变权限即可:

chmod 777 /src/ethzasl_msf/msf_core/cfg/MSF_Core.cfg
chmod 777 /home/lufeng/MSF/src/ethzasl_msf/msf_core/cfg/MSF_Core.cfg
chmod 777 /home/lufeng/MSF/src/ethzasl_msf/msf_distort/cfg/MSF_Distort.cfg
chmod 777 /home/lufeng/MSF/src/ethzasl_msf/msf_updates/cfg/SinglePoseSensor.cfg
chmod 777 /home/lufeng/MSF/src/ethzasl_msf/msf_updates/cfg/PositionPoseSensor.cfg
chmod 777 /home/lufeng/MSF/src/ethzasl_msf/msf_updates/cfg/SphericalPositionSensor.cfg
chmod 777 /home/lufeng/MSF/src/ethzasl_msf/msf_updates/cfg/SinglePositionSensor.cfg

每次编译之前记得刷新环境

cd 工作空间下
source devel/setup.bash

最后在编译一次：

cd MSF
catkin_make

显示如下结果说明没问题了

运行MSF Viconpos传感器框架

1）首先下载 Vicon 的数据集：
将其放置在 MSF/src/data 目录下面。
2）修改 src/ethzasl_msf/msf_updates/viconpos_sensor_fix.yaml 文件：
将其中所有的：

/pose_sensor/pose_sensor/

替换为：

/msf_viconpos_sensor/pose_sensor/

找到：

/pose_sensor/core/data_playback: false

修改成：

/pose_sensor/core/data_playback: true

3）修改 src/ethzasl_msf/msf_updates/launch/viconpos_sensor.launch 文件：
找到：

<rosparam file="$(find msf_updates)/viconpos_sensor_fix.yaml"/>

在这一行的前面加入两行 remap 操作，将传感器的 topic 与引擎的 topic 对应上：

<remap from="/msf_core/imu_state_input" to="/auk/fcu/imu"  />
<remap from="msf_updates/transform_input" to="/vicon/auk/auk" />

找到：

</node>

在其之后添加（这一步是初始化卡尔曼滤波器的，非常重要）：

<node pkg="rosservice" type="rosservice" name="initialize" args="call --wait /msf_viconpos_sensor/pose_sensor/initialize_msf_scale 1"/>

4）启动 ros 内核：
在一个窗口打开　roscore：

roscore

5）启动 MSF pose_sensor 节点：
在 MSF 目录下执行如下命令打开　pose_sensor 节点：

source devel/setup.bash 
roslaunch msf_updates viconpos_sensor.launch

6）打开动态配置参数功能（可选）：

rosrun rqt_reconfigure rqt_reconfigure

在菜单中即可动态设置参数。
7）播放 vicon 的 bag 文件：
在MSF 目录下执行如下命令：

rosbag play data/dataset.bag --pause -s 25

这一行命令是暂停并从第 25s 后开始播放 bag 文件，文档中说这是为了等待 MAV 硬件系统站稳并处于非观察模式（不理解）。总之，如果你准备好运行了，就可以开始点击空格键进行数据播放了，播放的数据大约剩余 86s 左右。

数据模拟：

刚才跑成功了数据融合节点，但是并没有任何可视化的输出可以给我们看到。ethzasl msf 提供了一些脚本来进行数据模拟的功能，可以让我们更直观地看到结果。

1）修改 src/ethzasl_msf/msf_core/scripts/plot_relevant 文件：
找到：

rxplot msf_core/state_out/data[0]:data[1]:data[2] msf_core/state_out/data[3]:data[4]:data[5] -b $T -t "position & velocity" -l px,py,pz,vx,vy,vz &
rxplot msf_core/state_out/data[13]:data[14]:data[15] msf_core/state_out/data[16] -b $T -t "acc bias & scale" -l x,y,z,L 

修改成：

rqt_plot msf_core/state_out/data[0]:data[1]:data[2]
#rxplot msf_core/state_out/data[0]:data[1]:data[2] msf_core/state_out/data[3]:data[4]:data[5] -b $T -t "position & velocity" -l px,py,pz,vx,vy,vz &
#rxplot msf_core/state_out/data[13]:data[14]:data[15] msf_core/state_out/data[16] -b $T -t "acc bias & scale" -l x,y,z,L 

2）启动 plot_relevant 脚本：
在 MSF 目录下执行如下命令打开　plot_relevant 脚本：

source devel/setup.bash 
rosrun msf_core plot_relevant

另外也可以直接在命令行运行：

rqt_plot msf_core/state_out/data[0]:data[1]:data[2]

如果一切正常，即可看到如下曲线绘制，这样就表示成功运行起来了：

不知道为什么我的数据曲线没有显示出来。但是数据已经加载了，如果有大佬看到这里，麻烦评论告知一下，感激不尽！