{"action":"created","issue":{"url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/325","repository_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization","labels_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/325/labels{/name}","comments_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/325/comments","events_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/325/events","html_url":"https://github.com/cra-ros-pkg/robot_localization/issues/325","id":189927397,"number":325,"title":"Rare issue caused by navsat_transform and bad IMU data","user":{"login":"msy22","id":17462459,"avatar_url":"https://avatars.githubusercontent.com/u/17462459?v=3","gravatar_id":"","url":"https://api.github.com/users/msy22","html_url":"https://github.com/msy22","followers_url":"https://api.github.com/users/msy22/followers","following_url":"https://api.github.com/users/msy22/following{/other_user}","gists_url":"https://api.github.com/users/msy22/gists{/gist_id}","starred_url":"https://api.github.com/users/msy22/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/msy22/subscriptions","organizations_url":"https://api.github.com/users/msy22/orgs","repos_url":"https://api.github.com/users/msy22/repos","events_url":"https://api.github.com/users/msy22/events{/privacy}","received_events_url":"https://api.github.com/users/msy22/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"assignees":[],"milestone":null,"comments":8,"created_at":"2016-11-17T03:40:15Z","updated_at":"2016-12-29T09:18:05Z","closed_at":null,"body":"I've identified a rare issue that I think was caused by navsat_transform and an especially bad IMU reading. It's only happened once to my knowledge so this issue can take very low priority. However I wanted to mention it here because it could lead to a simple enhancement for _navsat_transform_. \r\n\r\nI'm using a Jackal UGV in a sloped, outdoor area and localizing it using the _robot_localization_ package. Normally each data set is consistent, however one data set sticks out. The following chart shows the profile of the slope, both datasets are taken directly from the _/odometry/gps_ published by _navsat_transform_. \r\n\r\n![comparison of z2 profiles](https://cloud.githubusercontent.com/assets/17462459/20374903/647fba0a-ace0-11e6-97e2-2a29cbc01447.JPG)\r\n\r\nThe red trendline shows the correct cross-sectional profile of the slope. Every dataset except one agrees with this. The blue dataset is wrong, the raw GPS data matches the normal slope but the data in _/odometry/gps_ does not.  \r\n\r\nTo give another view of the same data, this is what _/odometry/gps_ looks like when its displayed in RViz for the correct slope profile:\r\n\r\n![20161115_normal_z2_profile _cropped](https://cloud.githubusercontent.com/assets/17462459/20406264/80ed5512-ad71-11e6-8759-8f80d6300d51.jpg)\r\n\r\nAnd this is the same view of the distorted dataset:\r\n\r\n![20161027_distorted_z2_alt_cropped](https://cloud.githubusercontent.com/assets/17462459/20406278/92fff890-ad71-11e6-9f66-e43e10dbe2f4.jpg)\r\n\r\nWhat I think has happened is this: when _navsat_transform_ launches it takes **one** reading from the IMU and uses this to determine the orientation of the _/map_ reference frame (set up by a global instance of the _ekf_localization_node_ that fuses IMU, wheel encoder and IMU data). Navsat_transform then uses the _/base_link -> /map_ transform to convert each GPS position into an equivalent pose in the _/map_ reference frame. BUT if that initial IMU reading was very wrong, the reference frame would not be level with the horizontal plane, and this could add an angular offset to every subsequent odometry pose. _Navsat_transform_ depends on, and yet also indirectly determines the position and orientation of _/map._ Because of this, it is effectively a positive feedback loop that could reinforce that angular error. I think that is what has happened here, because the erroneous profile (blue) has the same shape as the correct one (red), but with an angular offset. I also run another local instance of _ekf_localization_node_ that fuses only IMU and wheel encoder data. In the erroneous dataset, the output from the local EKF showed the correct slope profile.\r\n\r\nI think the most likely cause is this bad first IMU reading, and a possible enhancement to prevent this in future would be to have _navsat_transform_ collect multiple IMU measurements and average them, as a sort of calibration process.\r\n\r\nI think this is the cause, which is why I posted it here first. But I'm not 100% certain so if anyone thinks that there is a good chance that the cause is different, I will re-post this on ros.answers in greater detail for the wider community."},"comment":{"url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/comments/269602226","html_url":"https://github.com/cra-ros-pkg/robot_localization/issues/325#issuecomment-269602226","issue_url":"https://api.github.com/repos/cra-ros-pkg/robot_localization/issues/325","id":269602226,"user":{"login":"ayrton04","id":5042191,"avatar_url":"https://avatars.githubusercontent.com/u/5042191?v=3","gravatar_id":"","url":"https://api.github.com/users/ayrton04","html_url":"https://github.com/ayrton04","followers_url":"https://api.github.com/users/ayrton04/followers","following_url":"https://api.github.com/users/ayrton04/following{/other_user}","gists_url":"https://api.github.com/users/ayrton04/gists{/gist_id}","starred_url":"https://api.github.com/users/ayrton04/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ayrton04/subscriptions","organizations_url":"https://api.github.com/users/ayrton04/orgs","repos_url":"https://api.github.com/users/ayrton04/repos","events_url":"https://api.github.com/users/ayrton04/events{/privacy}","received_events_url":"https://api.github.com/users/ayrton04/received_events","type":"User","site_admin":false},"created_at":"2016-12-29T09:18:05Z","updated_at":"2016-12-29T09:18:05Z","body":"I don't believe it's an \"initial IMU reading\" issue, at least not directly (more on that in a moment). You can verify this by following the path of the IMU orientation data through the code.\r\n\r\n1. We receive it [here](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L493), and place it in the `transform_orientation_` variable.\r\n\r\n1. Later, when we need to compute the *map*->*utm* transform, we [use the `transform_orientation_`](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L239), *but* we immediately ignore the roll and pitch [here](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L271), and use the yaw-only orientation [here](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L278).\r\n\r\n1. The only other place that we use `transform_orientation` is in [removing the GPS offset](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L364).\r\n\r\nHowever, the one place where a bad IMU measurement can most certainly cause an issue is in the initial *odometry* message that we take in. Again, we'll follow the data path for odometry.\r\n\r\n1. We receive it [here](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L696) and store it in the `transform_world_pose_` variable.\r\n\r\n1. We use it to compute the *map*->*utm* transform [here](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L280).\r\n\r\nThis is why I think it's the initial odometry message: if we again refer to [this](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L280) line, when your robot starts, you'll presumably have some non-zero pitch orientation in that initial odometry message. If the UTM coordinates by which we're multiplying that pose are massive - say, on the order of tens of thousands of meters - then a small change in the pitch angle could make a significant difference in the transform. Note that it's still technically an initial IMU issue as you said, in that the EKF is fusing the IMU data with its state estimate, and then passing that on to `navsat_transform_node`.\r\n\r\nOne way to test this would be to look at the values that get printed out [here](https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L284), and compare them across your good bag files and the one that's off. "}}
{"actor":{"display_login":"ayrton04"}}