记录学习pose_ekf融合gps/imu
sensor_simulator_node.cpp 里发布的话题： /imu /magnetic /fix /fix_velocity /odometry /pose path
pose_ekf_node.cpp 里订阅的话题： imu magnetic_field magnetic_field_v3 fix sonar_height fix_velocity fix_velocity_v3 发布的话题： path gps_path /odom /est_pose /gps_pose /delta_pos /delta_vel /delta_theta_mag /delta_theta_acc 现在要把仿真的数据换成自己的传感器数据。 1.先改IMU 我用的imu是sanchi 100D4 它的话题名称：/imu/data_raw 和 /imu/mag 只需要在pose_ekf_node.cpp文件里将订阅的/imu和/magnetic_field_v3 改为/imu/data_raw 和 /imu/mag。 关于磁力计： pose_ekf_node.cpp里面订阅了两个话题magnetic_field和magnetic_field_v3 magnetic_field是库函数里给的，然后订阅他取名为magnetic_field_v3。
改好了： 现在 把/fix话题发布的是啥看明白。知道了，/fix发布的是LLA经纬度坐标。用自己的rtk话题（经纬度）来替换仿真数据。 但是由于我在室内获取不到GPS数据，今天还下雨，等天晴了出去做实验试一试。
三天过去了，因为我的gps接收机出现了各种bug，今天才刚刚收到GPGGA格式的数据，然后rostopic echo /rtk/data 得到了话题数据。 但是，我推着小车走，rviz中并没有轨迹生成，明天看一看程序吧，看看如何把rtk的path加进去，imu、gps的融合位姿是怎么得到的。今日感悟：水很深，不是简简单单改一下传进去的数据就完事了的。 我看了一下程序， /rtk/data 输出经纬度——>转成ecef（地心坐标系）——>转成eun（东北天坐标系）——>ned （北东地）——>/gps_pos话题输出。 ned——>publish_gps_pos()得到gps_path。这里！！我看到四元数方向角orientation突然感觉找到了没有轨迹生成的原因！就是我的gps没有方向角，只有单点位置。看来/fix和我的/rtk/data 数据内容并不一样。（好吧问题不在这…） 现在，把rtk/data的frame_id和时间戳（仿照sensor_simulator写的，不知道对不对）加上了。
pos()是仿真出来的数据，类型是Vector3d，world——>base_link坐标系的tf转化，就是用pos()来模拟一个真实轨迹。pos_raw=pos+噪声=GPS数据
录了一个/rtk/data的bag，不用推车下去了～～ 全部启动以后，/gps_pose没有数据。 tf仅仅是从sensor_simulator发出的，而且没有和其他节点产生联系。这也是一个问题。
我录的rtk 没有frame_id 和时间戳 也是个问题。
rosrun rqt_tf_tree rqt_tf_tree 查看tf树。 我感觉在tf上有很大问题。 2021.7.22 昨天终于有了进展！！！（是因为rtk的frame_id)黄色轨迹是RTK，绿色是imu和gps融合后的轨迹，至于它为什么这么飘，我感觉是因为那个里程计/odometry 是不是加入仿真数据了，相当于故意加了噪声？可是rqt_graph里显示/odometry仅由sensor_simulator_node 发布，没人用。今天的任务就是看看/odom和/odometry是怎么回四。
重跑一下昨天录的bag： 1.仅rtk： 看到黄色是估计的path，（我没连imu）轨迹是平滑的 但是跟不上rtk的里程。 现在连上imu试试。 跟上了。但是波动很大干扰很大。 选or不选/odom和/odometry的话题都差不多，我这里的imu没有动，是放在桌上静止的。
下面看一下imu+rtk的bag： 也跟不上？？？有没有imu都跟不上？？？我怀疑这个bag里就没有imu的数据。验证一下：果然。。。。没录上imu的话题。。。
那看一下全部话题的bag吧： ？？？？？？？？？？？乱七八糟？？？？
我现在能想到的改进有：1.标定imu 2.看他们的融合机制，是不是加入了什么干扰
去除仿真加的noise以后，明显波动减小了～～ 上图是50hz的imu 上图是200HZ的imu，感觉差别不大，不能确定我改的hz有作用，那就试试改成500hz。 好吧，我改的hz没有用。
又发现一个noise没去掉，去掉以后好像又好一点。发现一个问题，rtk数据停了的时候，imu还在走，朝着一个方向：然后，我感觉是我的imu标定问题，rostopic echo /imu/data_raw，看一下要补多少： 给acc_x+0.276，给acc_y-0.046。再看一下效果，ennn，是有点效果的，只不过无法彻底消除imu的累计误差，误差会随时间越来越大。
