记录学习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的累计误差，误差会随时间越来越大。