{
  "id": 237400,
  "title": "Making sense of quaternions",
  "url": "/competitions/indoor-location-navigation/discussion/237400",
  "author_name": "SuryaJR_Rafl",
  "post_date": "2021-05-08T15:00:20.625000",
  "votes": 3,
  "comment_count": 0,
  "views": 0,
  "content": "<p>Hi,</p>\n<p>I was exploring IMU data and wanted to make sense of AHRS data. The coordinate frame of the output is critical as this data is used to get linear accleration as well in recursive updates of different filters. I thought of validating it by comparing the AHRS yaw angle vs yaw angle obtained from consecutive waypoint positions. Unfortunately, I couldn't make the AHRS data align with path orientation using get_orientation from <a href=\"https://github.com/location-competition/indoor-location-competition-20/blob/master/compute_f.py\" target=\"_blank\">official github script </a>. </p>\n<p>So, I looked up at the <a href=\"https://developer.android.com/guide/topics/sensors/sensors_motion\" target=\"_blank\">android documentation</a> for the coordinate frame and convention used. I realised that the output is in compass convention (north direction is 0 deg), while i was expecting output in cartesian convention (right direction is 0 deg) as paths are generally in euclidean coordinates.</p>\n<p>I used the following code to align path orientation to AHRS output <a href=\"https://github.com/daniel-s-ingram/self_driving_cars_specialization/blob/master/2_state_estimation_and_localization_for_self_driving_cars/c2m5_assignment_files/rotations.py\" target=\"_blank\">reference quat to euler function</a></p>\n<pre><code>def NormalizeAngle(angle): #input angle [-pi, pi] mapped to [0, 2* pi]\n    if angle &lt; 0.0:\n        return (angle + 2*np.pi)\n    if angle &gt; (2*np.pi):\n        return (angle - 2*np.pi)\n    else:\n        return angle\n\ndef compass_to_cart(compass_orientation):\n    #input  : compass heading in radians\n    #output : cartesian heading in radians\n\n    if compass_orientation == 0.0:\n        compass_orientation = 2*np.pi\n\n    t1 = NormalizeAngle(compass_orientation - (0.5*np.pi))\n    cartesian_orientation = NormalizeAngle(2*np.pi - t1)\n    return cartesian_orientation\n\ndef cart_to_compass(cartesian_orientation):\n    #input  : cartesian heading in radians\n    #output : compass heading in radians\n\n    if cartesian_orientation == 0.0:\n        cartesian_orientation = 2*np.pi\n\n    t1 = NormalizeAngle(2*np.pi - cartesian_orientation)\n    compass_orientation = NormalizeAngle(t1 + (0.5*np.pi))\n    return compass_orientation\n\ndef getYawFromEuler(qx, qy, qz):\n    qw = np.sqrt(1 - (qx**2 + qy**2 + qz**2))\n    roll  = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))\n    pitch = np.arcsin( 2 * (qw * qy - qz * qx))\n    yaw   = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))\n\n    ## convert from compass to cartesian coordinates\n    yawDegrees = np.rad2deg(compass_to_cart((-yaw) + np.pi))\n    return yawDegrees\n</code></pre>\n<p>Output comparison</p>\n<ol>\n<li><a href=\"https://ibb.co/MhWGXsD\" target=\"_blank\">comparison using above function</a></li>\n<li><a href=\"https://ibb.co/yq6Yvv8\" target=\"_blank\">comparison using official git repo</a></li>\n</ol>\n<p>Any feedback and suggestion is most welcome.</p>",
  "messages": [
    {
      "id": 1298112,
      "postDate": "2021-05-08T15:00:20.627Z",
      "content": "<p>Hi,</p>\n<p>I was exploring IMU data and wanted to make sense of AHRS data. The coordinate frame of the output is critical as this data is used to get linear accleration as well in recursive updates of different filters. I thought of validating it by comparing the AHRS yaw angle vs yaw angle obtained from consecutive waypoint positions. Unfortunately, I couldn't make the AHRS data align with path orientation using get_orientation from <a href=\"https://github.com/location-competition/indoor-location-competition-20/blob/master/compute_f.py\" target=\"_blank\">official github script </a>. </p>\n<p>So, I looked up at the <a href=\"https://developer.android.com/guide/topics/sensors/sensors_motion\" target=\"_blank\">android documentation</a> for the coordinate frame and convention used. I realised that the output is in compass convention (north direction is 0 deg), while i was expecting output in cartesian convention (right direction is 0 deg) as paths are generally in euclidean coordinates.</p>\n<p>I used the following code to align path orientation to AHRS output <a href=\"https://github.com/daniel-s-ingram/self_driving_cars_specialization/blob/master/2_state_estimation_and_localization_for_self_driving_cars/c2m5_assignment_files/rotations.py\" target=\"_blank\">reference quat to euler function</a></p>\n<pre><code>def NormalizeAngle(angle): #input angle [-pi, pi] mapped to [0, 2* pi]\n    if angle &lt; 0.0:\n        return (angle + 2*np.pi)\n    if angle &gt; (2*np.pi):\n        return (angle - 2*np.pi)\n    else:\n        return angle\n\ndef compass_to_cart(compass_orientation):\n    #input  : compass heading in radians\n    #output : cartesian heading in radians\n\n    if compass_orientation == 0.0:\n        compass_orientation = 2*np.pi\n\n    t1 = NormalizeAngle(compass_orientation - (0.5*np.pi))\n    cartesian_orientation = NormalizeAngle(2*np.pi - t1)\n    return cartesian_orientation\n\ndef cart_to_compass(cartesian_orientation):\n    #input  : cartesian heading in radians\n    #output : compass heading in radians\n\n    if cartesian_orientation == 0.0:\n        cartesian_orientation = 2*np.pi\n\n    t1 = NormalizeAngle(2*np.pi - cartesian_orientation)\n    compass_orientation = NormalizeAngle(t1 + (0.5*np.pi))\n    return compass_orientation\n\ndef getYawFromEuler(qx, qy, qz):\n    qw = np.sqrt(1 - (qx**2 + qy**2 + qz**2))\n    roll  = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))\n    pitch = np.arcsin( 2 * (qw * qy - qz * qx))\n    yaw   = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))\n\n    ## convert from compass to cartesian coordinates\n    yawDegrees = np.rad2deg(compass_to_cart((-yaw) + np.pi))\n    return yawDegrees\n</code></pre>\n<p>Output comparison</p>\n<ol>\n<li><a href=\"https://ibb.co/MhWGXsD\" target=\"_blank\">comparison using above function</a></li>\n<li><a href=\"https://ibb.co/yq6Yvv8\" target=\"_blank\">comparison using official git repo</a></li>\n</ol>\n<p>Any feedback and suggestion is most welcome.</p>",
      "rawMarkdown": "Hi,\n\nI was exploring IMU data and wanted to make sense of AHRS data. The coordinate frame of the output is critical as this data is used to get linear accleration as well in recursive updates of different filters. I thought of validating it by comparing the AHRS yaw angle vs yaw angle obtained from consecutive waypoint positions. Unfortunately, I couldn't make the AHRS data align with path orientation using get_orientation from [official github script ](https://github.com/location-competition/indoor-location-competition-20/blob/master/compute_f.py). \n\nSo, I looked up at the [android documentation](https://developer.android.com/guide/topics/sensors/sensors_motion) for the coordinate frame and convention used. I realised that the output is in compass convention (north direction is 0 deg), while i was expecting output in cartesian convention (right direction is 0 deg) as paths are generally in euclidean coordinates.\n\n\nI used the following code to align path orientation to AHRS output [reference quat to euler function](https://github.com/daniel-s-ingram/self_driving_cars_specialization/blob/master/2_state_estimation_and_localization_for_self_driving_cars/c2m5_assignment_files/rotations.py)\n\n\n```python\ndef NormalizeAngle(angle): #input angle [-pi, pi] mapped to [0, 2* pi]\n    if angle < 0.0:\n        return (angle + 2*np.pi)\n    if angle > (2*np.pi):\n        return (angle - 2*np.pi)\n    else:\n        return angle\n\ndef compass_to_cart(compass_orientation):\n    #input  : compass heading in radians\n    #output : cartesian heading in radians\n\n    if compass_orientation == 0.0:\n        compass_orientation = 2*np.pi\n    \n    t1 = NormalizeAngle(compass_orientation - (0.5*np.pi))\n    cartesian_orientation = NormalizeAngle(2*np.pi - t1)\n    return cartesian_orientation\n\ndef cart_to_compass(cartesian_orientation):\n    #input  : cartesian heading in radians\n    #output : compass heading in radians\n\n    if cartesian_orientation == 0.0:\n        cartesian_orientation = 2*np.pi\n    \n    t1 = NormalizeAngle(2*np.pi - cartesian_orientation)\n    compass_orientation = NormalizeAngle(t1 + (0.5*np.pi))\n    return compass_orientation\n\ndef getYawFromEuler(qx, qy, qz):\n    qw = np.sqrt(1 - (qx**2 + qy**2 + qz**2))\n    roll  = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))\n    pitch = np.arcsin( 2 * (qw * qy - qz * qx))\n    yaw   = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))\n    \n    ## convert from compass to cartesian coordinates\n    yawDegrees = np.rad2deg(compass_to_cart((-yaw) + np.pi))\n    return yawDegrees\n```\n\nOutput comparison\n\n1. [comparison using above function](https://ibb.co/MhWGXsD)\n2. [comparison using official git repo](https://ibb.co/yq6Yvv8)\n\n\nAny feedback and suggestion is most welcome.",
      "votes": 3
    }
  ],
  "comments": [],
  "raw_markdown_by_id": {
    "1298112": "Hi,\n\nI was exploring IMU data and wanted to make sense of AHRS data. The coordinate frame of the output is critical as this data is used to get linear accleration as well in recursive updates of different filters. I thought of validating it by comparing the AHRS yaw angle vs yaw angle obtained from consecutive waypoint positions. Unfortunately, I couldn't make the AHRS data align with path orientation using get_orientation from [official github script ](https://github.com/location-competition/indoor-location-competition-20/blob/master/compute_f.py). \n\nSo, I looked up at the [android documentation](https://developer.android.com/guide/topics/sensors/sensors_motion) for the coordinate frame and convention used. I realised that the output is in compass convention (north direction is 0 deg), while i was expecting output in cartesian convention (right direction is 0 deg) as paths are generally in euclidean coordinates.\n\n\nI used the following code to align path orientation to AHRS output [reference quat to euler function](https://github.com/daniel-s-ingram/self_driving_cars_specialization/blob/master/2_state_estimation_and_localization_for_self_driving_cars/c2m5_assignment_files/rotations.py)\n\n\n```python\ndef NormalizeAngle(angle): #input angle [-pi, pi] mapped to [0, 2* pi]\n    if angle < 0.0:\n        return (angle + 2*np.pi)\n    if angle > (2*np.pi):\n        return (angle - 2*np.pi)\n    else:\n        return angle\n\ndef compass_to_cart(compass_orientation):\n    #input  : compass heading in radians\n    #output : cartesian heading in radians\n\n    if compass_orientation == 0.0:\n        compass_orientation = 2*np.pi\n    \n    t1 = NormalizeAngle(compass_orientation - (0.5*np.pi))\n    cartesian_orientation = NormalizeAngle(2*np.pi - t1)\n    return cartesian_orientation\n\ndef cart_to_compass(cartesian_orientation):\n    #input  : cartesian heading in radians\n    #output : compass heading in radians\n\n    if cartesian_orientation == 0.0:\n        cartesian_orientation = 2*np.pi\n    \n    t1 = NormalizeAngle(2*np.pi - cartesian_orientation)\n    compass_orientation = NormalizeAngle(t1 + (0.5*np.pi))\n    return compass_orientation\n\ndef getYawFromEuler(qx, qy, qz):\n    qw = np.sqrt(1 - (qx**2 + qy**2 + qz**2))\n    roll  = np.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))\n    pitch = np.arcsin( 2 * (qw * qy - qz * qx))\n    yaw   = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))\n    \n    ## convert from compass to cartesian coordinates\n    yawDegrees = np.rad2deg(compass_to_cart((-yaw) + np.pi))\n    return yawDegrees\n```\n\nOutput comparison\n\n1. [comparison using above function](https://ibb.co/MhWGXsD)\n2. [comparison using official git repo](https://ibb.co/yq6Yvv8)\n\n\nAny feedback and suggestion is most welcome."
  }
}