{"id":15,"date":"2016-08-20T11:37:01","date_gmt":"2016-08-20T09:37:01","guid":{"rendered":"http:\/\/blog.bisig.fr\/?p=15"},"modified":"2016-12-29T19:19:24","modified_gmt":"2016-12-29T18:19:24","slug":"lsm9ds1-madgwicks-ahr-filter-and-robot-orientation","status":"publish","type":"post","link":"https:\/\/blog.bisig.fr\/index.php\/2016\/08\/20\/lsm9ds1-madgwicks-ahr-filter-and-robot-orientation\/","title":{"rendered":"LSM9DS1, Madgwick’s AHR filter and robot orientation"},"content":{"rendered":"

My original plan was to do several posts about general information and then go deeper into some topics, but as I\u2019m currently working on getting the ST iNEMO\u00a0LSM9DS1 intertial module to work, I thought that I might as well start this blog on this subject.<\/p>\n

One of the thing I would like to achieve with this robot is to get it as close as possible of being capable of navigating and avoiding obstacles autonomously so it can, for example, returns to its starting position by itself.<\/p>\n

As it will be used mostly indoor, using a GPS for precise positioning is not possible, so we have to rely on less accurate sensors.\u00a0A 9DOF inertial module\u00a0in the form of a LSM9DS1 from ST(datasheet<\/a>), will be the main sensors to evaluate the robot position. This chip is in reality\u00a0two components into a single package with a\u00a03D accelerometer \/ gyroscope on one side, and a 3D magnetometer (compass) on the other side.<\/p>\n

This kind of sensors are cheap, and are able to give the acceleration, the angular speed, as well as measuring the magnetic field but they are also not very\u00a0accurate. For instance, apart from the usual \u201cnoise\u201d of the measurement, gyroscope has an output\u00a0drift which is dependant of the chip temperature and the magnetometer is subject to several measurement impairment due to the presence of other magnetic components (especially in a robot were motors are close\u00a0!!!). \u00a0It is possible to\u00a0 mitigate some of the errors \u00a0by performing a sensor calibration without completely removing them. Also if we want to be able to evaluate the speed and position of the robot based on the accelerometer, we need to remove the acceleration due to the gravity which means we need to be able to evaluate precisly the direction of the gravity in the sensor reference frame.<\/p>\n

That\u2019s where the filter proposed by Sebastian Madgwick comes into play (more info<\/a>). \u00a0This filter combines data from the gyroscope, accelerometer and magnetometer to estimate the orientation (in the form a quaternion<\/a>) of the device according to the earth reference (magnetic north and gravity). This algorithm acheive a good accuracy even if the device is subject to linear accelerations and is robust to sensors bias and drift. It is also open source and\u00a0 C code is\u00a0 available!<\/p>\n

So, everything perfect, right? Not really and I have been struggling a little bit to get the correct output from the filter. Main reason for that the source code of the filter does not provide any documentation about the convention used for the input values: unit and axis orientation. So I had to dig into Sebastian Madgwick’s paper<\/a> to find some info.<\/p>\n

Units are simples. The filter needs to have the gyroscope data expressed as rad.s-1<\/sup>. For the acceleration and magnetic field, however, you can use any unit you like, because the values are normalized in the filter (only the vector orientation is important).<\/p>\n

However, getting a clear answer about the axis orientation was difficult. On some discussion I found that the madgwick filter\u2019s is using a NED<\/a>\u00a0 (x pointing north, y pointing east and z pointing down) convention just like in aviation. \u00a0This is not completely correct. Madgwick\u2019s filter is indeed using a right handed axis convention but as show in Madgwick paper on page 5, the Z axis is pointing up and not down.<\/p>\n

\"madgwick_axis\"<\/p>\n

This can also be confirmed by the Xsens MTX user Manual<\/a>\u00a0\u00a0which was used to perform the algorithm evaluation and is using the same convention.<\/p>\n

Where it gets a little bit confusing, is that Madgwick is giving the formulae to get the euler angles from the quaternion representation in his paper.<\/p>\n

\"madgwick_euler_angles\"<\/p>\n

Where \u03c8<\/em> is yaw, \u03b8<\/em> is pitch and \u03a6<\/em> is roll.<\/p>\n

If you use theses equations, the calculated roll, pitch and yaw will be according to the aircraft\u00a0convention (NED), but for the roll which need to be inverted.<\/strong><\/p>\n

\"Yaw_Axis_Corrected.svg\"<\/p>\n

Finally, Madgwick is using variables q0, q1, q2 and q3 (or q1, q2, q3 and q4 in his paper) to represent the quaternion. But sometimes you can find quaternion represented as x, y, z and w (as in unity 3D). If for some reason, you want to use the filter output for something else, just be aware that w is equivalent to q0, not q3.<\/p>\n

So now that we know what is expected by the filter, what about the output of the LSM9DS1 ? For accelerometer\/gyroscope, the datasheet is giving the following:<\/p>\n

\"LSM9DS1_accelgyro_orient\"<\/p>\n

Orientation reference is left handed, so we can\u2019t use the output as it is but swapping from left handed to right handed axis is easy, we just have to swap the X and Y axis for gyro and accelerometer.\u00a0 Finally we have to align the X axis point to the front of the robot (and Y to be the left side). For Yapibot, I got lucky as the Y axis of the sensor is pointing to the front of the robot so I can just apply:<\/p>\n

Yapibot.ax = LSM9DS1.ay
\nYapibot.ay = LSM9DS1.ax
\nYapibot.az = LSM9DS1.az<\/p>\n

And for the gyro:<\/p>\n

Yapibot.gx = LSM9DS1.gy
\nYapibot.gy = LSM9DS1.gx
\nYapibot.gz = LSM9DS1.gz<\/p>\n

The same way, we need to adapt the ouput of the magnetometer according to our reference.<\/p>\n

\"LSM9DS1_mag_orient\"<\/p>\n

Be careful here because for some reason, ST did not use the same orientation of the diagram (dot on the chip is up, while for accel\/gyro it is on the left) (IT’S A TRAP!). So finally, compared to the accel\/gyro, only the X direction is inverted.<\/p>\n

Yapibot.mx = LSM9DS1.my
\nYapibot.my = -LSM9DS1.mx
\nYapibot.mz = LSM9DS1.mz<\/p>\n

Using this convention, I have been able to correctly get the orientation of the robot.<\/p>\n

\"20160820_112329\"<\/p>\n","protected":false},"excerpt":{"rendered":"

My original plan was to do several posts about general information and then go deeper into some topics, but as I\u2019m currently working on getting the ST iNEMO\u00a0LSM9DS1 intertial module to work, I thought that I might as well start this blog on this subject. One of the thing I would like to achieve with […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"_jetpack_newsletter_access":"","_jetpack_newsletter_tier_id":0,"footnotes":""},"categories":[6,5,4],"tags":[17,15,16],"jetpack_sharing_enabled":true,"jetpack_featured_media_url":"","jetpack_shortlink":"https:\/\/wp.me\/pfpN30-f","_links":{"self":[{"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/posts\/15"}],"collection":[{"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/comments?post=15"}],"version-history":[{"count":10,"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/posts\/15\/revisions"}],"predecessor-version":[{"id":181,"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/posts\/15\/revisions\/181"}],"wp:attachment":[{"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/media?parent=15"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/categories?post=15"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/blog.bisig.fr\/index.php\/wp-json\/wp\/v2\/tags?post=15"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}