精华内容
下载资源
问答
  • 卡尔曼融合算法论文

    2018-09-10 17:06:14
    卡尔曼滤波融合算法,2013 IROS论文单目视觉传感器与IMU的融合算法modular_sensor_fusion
  • ROS下robot_pose_ekf扩展卡尔曼融合包的使用,robot_pose_ekf的融合包
  • 多传感器卡尔曼融合框架的编译与使用 参考上述连接,是比较完整的连接,但是仍然会遇到不少的错误,很多坑。我把自己遇到的错误贴出来,供大家参考。 首先你每次更改文件,或者路径,或者别的什么东西的时候,记得...

     

    多传感器卡尔曼融合框架的编译与使用

    参考上述连接,是比较完整的连接,但是仍然会遇到不少的错误,很多坑。我把自己遇到的错误贴出来,供大家参考。

    首先你每次更改文件,或者路径,或者别的什么东西的时候,记得重新执行source devel/setupbash一下,相当于刷新工作环境。

    如果你遇到这个错误,说明系统没有找到你这个文件,看一下缺少的是什么文件,我发现缺少的文件并没有在src文件夹下,那么在根目录下搜索这个文件,复制到src目录下(后来发现是在glog_catkin目录下)

    cannot open /home/minghua/MSF/src/fix-unused-typedef-warning.patch: No such file
    

    其次,如果你遇到这个错误:

     fatal error: mav_msgs/RollPitchYawrateThrust.h: No such file or directory

     

    那么说明,你src文件夹里面并没有下载mav_msgs这个文件夹。这在多数指导书中并没有提到。可以执行下面的命令下载:

    git clone  https://github.com/ethz-asl/mav_comm.git

    其实任何涉及mav_msgs的错误,可能就是因为你没下载这个东西,所以提示你找不到这个文件。

    如果还有这样的错误:
     

    1: autoreconf: not found

     那么说明没有安装这个库,重新安装:

     sudo apt-get install autoconf automake libtool

    或者干脆把下面链接里面的库都给安装一下:

    https://blog.csdn.net/x356982611/article/details/70856470 

    然后重新source  devel/setup.bash

    然后执行

    catkin_make

    就行了。

    展开全文
  • ROS导航功能包:ros-planning/...一、csdn:ROS下robot_pose_ekf扩展卡尔曼融合包的使用 二、ROS wiki:带外部传感器的robot_pose_ekf 三、创客智造:ROS与navigation教程-robot_pose_ekf 四、csdn:22.IMU和里.

    ROS导航功能包:ros-planning/navigation

    中国大学mooc:Navigation 工具包说明

    目录

    ROS导航功能包:ros-planning/navigation

    中国大学mooc:Navigation 工具包说明

    一、csdn:ROS下robot_pose_ekf扩展卡尔曼融合包的使用

    二、ROS wiki:带外部传感器的robot_pose_ekf

    三、创客智造:ROS与navigation教程-robot_pose_ekf

    四、csdn:22.IMU和里程计融合

    五、csdn:使用 robot_pose_ekf 对imu和odom进行融合


    一、csdn:ROS下robot_pose_ekf扩展卡尔曼融合包的使用

    ROS下robot_pose_ekf扩展卡尔曼融合包的使用

    有一个imu,而且校准的好(博主使用的是razor 9dof的imu,通过ros包pub一个Imu的data类型)

    原先的tf_tree中可以看到这个单单使用里程计的tf图片:(由arduion转换的,底层通信的node里发布的)

    由于ekf包会为我们处理好这部分tf,所以不需要我们发布变换了,所以需要将上面这部分发布的代码注释掉!

    之后我们需要加一个imu的link(由于我们的比赛中已经有了这个link,所以这一步就不做处理:静态发布imu的tf)

    现在开始进行使用ekf包来融合:

    • 开启底盘的launch发布odom的topic
    • 开启imu的launch发布imu的topic
    • 开启运行robot_pose_ekf的launch

    为了查看ekf包是否正常工作,使用以下代码查看:

    rosservice call /robot_pose_efk/get_status
    

    查看Odometry sensor和IMU sensor 是否is used ,is active来确保ekf包的是否正确使用

    如果成功,那么tf图是这样的,odom–base_footprint的转换就是由/robot_pose_ekf发布的

    至于融合后的效果,就要看imu的校准度了!!!看效果的话可以用rviz或者导出rosbag包用matlab的plot函数绘出来,matlab可能更直观点,可以看这个:使用Matlab与ROS端通信以及绘制Odom里程计信息

    二、ROS wiki:带外部传感器的robot_pose_ekf

    带外部传感器的robot_pose_ekf

    三、创客智造:ROS与navigation教程-robot_pose_ekf

    ROS与navigation教程-robot_pose_ekf

    • Robot Pose EKF 包用于评估机器人的3D位姿,基于来自不同来源的位姿测量信息。它使用带有6D(3D position and 3D orientation)模型信息的扩展卡尔曼滤波器来整合来自轮式里程计,IMU传感器和视觉里程计的数据信息。基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。

    如何使用:EKF节点的默认启动文件位于robot_pose_ekf包目录中​​​​​​​

    <launch>
      <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
        <param name="output_frame" value="odom"/>
        <param name="freq" value="30.0"/>
        <param name="sensor_timeout" value="1.0"/>
        <param name="odom_used" value="true"/>
        <param name="imu_used" value="true"/>
        <param name="vo_used" value="true"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>
      </node>
    </launch>

    参数:

    • freq: 滤波器更新和发布频率。注意:频率高仅仅意味着一段时间可以获得更多机器人位姿信息,但是并不表示可以提高每次位姿评估的精度。
    • sensor_timeout: 当传感器停止向滤波器发送信息时,滤波器在没有传感器的情况下等待多长时间才重新开始工作
    • odom_used, imu_used, vo_used: 确认是否输入

    编译运行:

    rosdep install robot_pose_ekf
    roscd robot_pose_ekf
    rosmake
    
    roslaunch robot_pose_ekf.launch

    节点:

    订阅的话题:

    odom ([nav_msgs/Odometry][2])
    2D pose (用于轮式里程计):其包含机器人在地面中的位置(position)和方位(orientation)以及该位姿的协方差。 发送此2D位姿的消息实际上表示3D位姿,但z,pitch和roll分量被简单忽略了。
    imu_data ([sensor_msgs/Imu][3]) 
    3D orientation (用于IMU):3D方位提供机器人基座相对于地图坐标系的Roll, Pitch and Yaw偏角。Roll and Pitch角是绝对角度(因为IMU使用了重力参考),而YAW角是相对角度。协方差矩阵指定的方位测量的不确定度。当仅仅收到这个主题消息时,机器人位姿ekf还不会启动,因为它还需要来自主题'vo'或者'odom'的消息。
    vo ([nav_msgs/Odometry][4]) 
    3D pose (用于视觉里程计):3D位置表示机器人的完整位置和方位以及该位姿的协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。

    发布的话题:

    robot_pose_ekf/odom_combined ([geometry_msgs/PoseWithCovarianceStamped][6]) 
    滤波器输出 (评估的3D机器人位姿)。

    tf转换:odom_combined → base_footprint​​​​​​​

    四、csdn:22.IMU和里程计融合

    22.IMU和里程计融合

    实际使用中会出现轮子打滑和累计误差的情况,这里单单使用编码器得到里程计会出现一定的偏差,虽然激光雷达会纠正,但一个准确的里程对这个系统还是较为重要。

    IMU即为 惯性测量单元,一般包含了三个单轴的加速度计和三个单轴的陀螺仪,简单理解通过加速度二次积分就可以得到位移信息、通过角速度积分就可以得到三个角度

    查看话题:/robot_pose_ekf/odom_combined

    rostopic info /robot_pose_ekf/odom_combined

    类型需要注意下是PoseWithCovarianceStamped并非Odometry,后面会用到这个作为显示,所以还需要一个转换

    查看该topic信息可以看到odom_ekf订阅了该topic,再次查看该节点信息可以看到

    他会发出一个Odometrytopic,发出一个tf

    查看订阅滤波器输出话题的节点:/odom_ekf

    rosnode info /odom_ekf

     

    robot_pose_ekf配置时,做了些映射处理,这样可以保证导航层在使用和不用imu的时候无需修改就可以工作,有无imu输出的tf都是odom → base_footprint​​​​​​​,发出的里程都是odom,所以将带有imu的里程topic映射为wheel_odom

    五、csdn:使用 robot_pose_ekf 对imu和odom进行融合

    使用 robot_pose_ekf 对imu和odom进行融合

    注意发布消息时 topic 的名称要对应,否则会起不到滤波作用!!!

    robot_pose_ekf.launch 文件如下:

    <launch>
    	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    		<param name="output_frame" value="odom_combined"/>
    		<param name="base_footprint_frame" value="base_footprint"/>
    		<param name="freq" value="30.0"/>
    		<param name="sensor_timeout" value="1.0"/>
    		<param name="odom_used" value="true"/>
    		<param name="imu_used" value="true"/>
    		<param name="vo_used" value="false"/>
    		<param name="debug" value="false"/>
    		<param name="self_diagnose" value="false"/>
    	</node>
    </launch>
    
    • output_frame, base_footprint_frame:用于指定输出 tf 变换中坐标系的名字,默认为 odom_combined 和 base_footprint。

    注意使用robot_pose_ekf 进行滤波时传感器的协方差矩阵信息不能空着,否则可能会出现错误,因此要设置合理的值。

    • imu 数据的协方差矩阵设置可以参考:

    https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py

    self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
    self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
    • 底盘运动时 odom 的协方差矩阵如下,参考:

    https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/covariances.py

    ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
    						0, 1e-3, 0, 0, 0, 0,
    						0, 0, 1e6, 0, 0, 0,
    						0, 0, 0, 1e6, 0, 0,
    						0, 0, 0, 0, 1e6, 0,
    						0, 0, 0, 0, 0, 1e3]
    ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
    						0, 1e-3, 0, 0, 0, 0,
    						0, 0, 1e6, 0, 0, 0,
    						0, 0, 0, 1e6, 0, 0,
    						0, 0, 0, 0, 1e6, 0,
    						0, 0, 0, 0, 0, 1e3]
    

    注意 imu 信息的协方差矩阵中代表机器人航向角的分量方差为 1e-6,而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3,两个值相差很大。在进行 EKF 融合时,会更“相信”imu 提供的姿态信息,因为其方差更小。

    比如机器人在转动过程中轮子发生了打滑,用编码器推算出的姿态一直在旋转,而实际姿态(主要由 IMU 测量得到)却没发生太大变化,这种情况就需要使用信息融合方法来减小误差协方差矩阵中的参数设置非常重要,要根据传感器手册或者实际使用测量来确定

    我们得到了/robot_pose_ekf/odom_combined 消息:

    在 movebase 需要订阅/odom 的话题,想让这个消息被 movebase使用

    这里主要是看 rbx_bringup 包里提供的一个节点:odom_ekf.py,odom_ekf.py 就是将它们转化用的

    #!/usr/bin/env python
    import rospy
    from geometry_msgs.msg import PoseWithCovarianceStamped
    from nav_msgs.msg import Odometry
    
    class OdomEKF():
       def __init__(self):
           # Give the node a name
           rospy.init_node('odom_ekf', anonymous=False)
    
           # Publisher of type nav_msgs/Odometry
           self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
           
           # Wait for the /odom_combined topic to become available
           rospy.wait_for_message('input', PoseWithCovarianceStamped)
           
           # Subscribe to the /odom_combined topic
           rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
           
           rospy.loginfo("Publishing combined odometry on /odom_ekf")
           
       def pub_ekf_odom(self, msg):
           odom = Odometry()
           odom.header = msg.header
           odom.header.frame_id = '/odom'
           odom.child_frame_id = 'base_footprint'
           odom.pose = msg.pose
           
           self.ekf_pub.publish(odom)
           
    if __name__ == '__main__':
       try:
           OdomEKF()
           rospy.spin()
       except:
           pass
    

    很简单, /odom/robot_pose_ekf/odom_combined它们消息类型是不同的,前者是nav_msgs/Odometry,后者是geometry_msgs/PoseWithCovarianceStamped ,两者的区别:后者的内容是前者的一部分

    odom_ekf.py 就是将它们转化用的。

    转化后还要在发布融合信息的 launch 文件里将/robot_pose_ekf/odom_combined 话题 remap 成/odom_ekf.就可以给 movebase 使用了!

    <launch>
       <node pkg="rbx1_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
       	<remap from="input" to="/robot_pose_ekf /odom_combined"/>
       	<remap from="output" to="/odom_ekf"/>
       </node>
    </launch>
    

    这个节点就订阅/robot_pose_ekf /odom_combined 下的位置估计信息然后发布/odom_ekf

    /odom_ekf 和/odom 的信息类型相同

    回复:

    <node pkg="move_base" type="move_base" name="move_base" output="screen"> 
      <remap from="odom" to="odom_ekf" />
     </node> 
     
    在move_base将odom改为odom_ekf,主要是在源码中回调函数的消息类型是nav::Odometry,消息类型需要一致,如果没有用odom_ekf.py,应该会报错

     

     

     

     

     

    展开全文
  • ROS下robot_pose_ekf扩展卡尔曼融合包的使用

    万次阅读 多人点赞 2017-10-12 21:35:30
    最近在使用imu进行数据融合,使用的是robot_pose_ekf的融合包,发现网上的教程太不详细,折腾了1个月,终于搞定了。于是乎,想写一个关于ekf包的使用教程。里程计和惯导模块imu的数据融合来得到一个不易丢失的机器人...

    最近在使用imu进行数据融合,使用的是robot_pose_ekf的融合包,发现网上的教程太不详细,折腾了1个月,终于搞定了。于是乎,想写一个关于ekf包的使用教程。里程计和惯导模块imu的数据融合来得到一个不易丢失的机器人姿态。首先,你得有一个imu,而且得校准好,我使用的是razor 9dof的imu,通过ros包pub一个Imu的data类型。
    这是我google的比较有用的一个ekf包的使用教程
    首先,先给大家看下我的tf!

    单里程计的tf
    这张图是单使用里程计的tf图片,你可以通过

    rosrun rqt_tf_tree rqt_tf_tree

    来查看自己的tf,维护好自己的tf对我们来说至关重要,稍有不慎就会出现很多麻烦,别问我是怎么知道的,都是泪,从上面的图片可以看到odom–base_footprint是通过arduino转换的,这一部分一般都是在我们与底盘通信的node里发布的。
    这里写图片描述
    就是这里,这部分代码就是实现上面tf转换的源代码,为什么要把这部分提出来呢,我们之后再说。好了,下面我们开始说怎么使用ekf包了。 odom–base_footprint的tf转换,所以我们使用这个包第一步就是要将我们上面提到的那一段代码注释掉, 因为ekf包会为我们处理好这部分tf,所以不需要我们发布变换了,这一步很重要! 之后我们需要加一个imu的link,我推荐直接使用静态tf发布吧。

     `   <node name="base_imu_link" pkg="tf" type="static_transform_publisher" args="0 0 0 0 3.1415926 0  /base_link /base_imu_link 50"/>
    

    `

    这里就OK了,之后我们就可以使用ekf包来融合了,首先开启底盘的launch,来发布odom的topic,然后开启imu的launch来发布imu的topic,确保这两个topic有数据,可以rostopic echo 来查看,之后运行robot_pose_ekf的launch文件,为了查看ekf包是否正常工作,可以用下面代码:

    rosservice call /robot_pose_efk/get_status

    查看Odometry sensor和IMU sensor 是否is used ,is active来确保ekf包的是否正确使用,下面贴出我的融合成功后的tf:

    这里写图片描述

    可以看到这里的odom–base_footprint的转换就是由/robot_pose_ekf发布的了,至于融合后的效果就是要看imu的校准度了,看效果的话可以用rviz或者导出rosbag包用matlab的plot函数绘出来,matlab可能更直观点, 至于如何使用matlab绘制里程计data,有时间我再贴出来吧。上面的内容是我自己总结出来的,有什么错误或不足大家可以指出来,一起学习,共同进步哈。

    展开全文
  • Ethzasl MSF Framework 是基于扩展卡尔曼滤波器(EKF)的多传感器融合框架。 该框架本质上分为两个EKF步骤:预测和更新。 预测是基于系统模型(即微分方程)和IMU读数进行的。 我们不将IMU读数作为更新度量,因为这...

    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]
    

    如果一切正常,即可看到如下曲线绘制,这样就表示成功运行起来了:
    在这里插入图片描述
    不知道为什么我的数据曲线没有显示出来。但是数据已经加载了,如果有大佬看到这里,麻烦评论告知一下,感激不尽!

    展开全文

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 4,235
精华内容 1,694
关键字:

卡尔曼融合