# LSM9DS1 and balancing math

Jwolf
Posts:

**74**
Hello,

I have the LSM9 working with raw outputs... but im having trouble with the math.

How do I link the gyro + accel to create a stable orientation output?

I just want to get this to work so it can be used for any type of project, whether it be an rc aircraft, drone, balancing, or general heading output.

I'm using the Elev8 source from Jesse Burt.

Accel outputs at rest output +16,400 to -16,400, increased output due to g-forces... I 'think' its reading at 400hz

gyro is about 2x that when rotated quickly.

I guess I need a formula to deal with 3 different accelero axi's correlated with each other, minus g-forces, checked against 3 different gyro axi's, at the sampled rate.

I've been studying for a while now on basic concept... (((rate of change) - noise) / units)) = amount of correction needed to stay level in specified units

Cannot figure out how to make the LSM9 output usable.... please help!

I have the LSM9 working with raw outputs... but im having trouble with the math.

How do I link the gyro + accel to create a stable orientation output?

I just want to get this to work so it can be used for any type of project, whether it be an rc aircraft, drone, balancing, or general heading output.

I'm using the Elev8 source from Jesse Burt.

Accel outputs at rest output +16,400 to -16,400, increased output due to g-forces... I 'think' its reading at 400hz

gyro is about 2x that when rotated quickly.

I guess I need a formula to deal with 3 different accelero axi's correlated with each other, minus g-forces, checked against 3 different gyro axi's, at the sampled rate.

I've been studying for a while now on basic concept... (((rate of change) - noise) / units)) = amount of correction needed to stay level in specified units

Cannot figure out how to make the LSM9 output usable.... please help!

## Comments

130There are two main filters you can use to fuse gyroscope and accelerometer measurements - the extended Kalman filter and the complementary filter. The complementary filter is far simpler to implement but does not account for system dynamics so it will not work well for fast moving vehicles such as fixed wing aircraft. For balancing applications, the complementary filter works fine. It can be implemented in code by:

where accelAngle is the angle computed from the accelerometer, ax is the acceleration value in g's in the x direction, az is the acceleration value in g's in the z direction, theta is the pitch angle you are estimating, gy is the gyroscope output about the y axis, and Ts is the loop sample time. You may have to map your sensor outputs according to a north, east, down coordinate system where north is in the x direction and points out of the front of your vehicle, east is in the y direction and points out the right wing of your vehicle, and down is in the z direction and points out of the bottom of your vehicle towards earth. This way, if you are estimating theta (pitch), the gy measurement is about the the y axis. Once you are able to estimate theta, you can easily implement an estimation of phi (roll).

See my thread here: http://forums.parallax.com/discussion/169330/is-there-a-faster-way-to-read-sensor-data#latest

if you are interested in implementing the extended Kalman filter. Oh and make sure you are using the LMM main RAM memory model. Your code will run about 5 times faster than if you use the CMM RAM memory model. It took me a while to figure this out. If you have any questions, just ask and I'll try to help.

Thanks,

David

74Will get back to ya when I have time to absorb that into the ol'melon

74The kalman filter in your above post, which is in C, is a bit too wild for me to follow.

I cannot figure out how to port this to spin, nor can i figure out what in the world is goin on.

math like this in C, which is unusable in spin and shows no basic intuition, blows my mind: xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);

please dont think i only need clarification on this one function... this is just a prime example to illustrate the level of confusion im dealin with... like why are you multiplying all axis by 9.81 in there?

I must have read a dozen or two in-depth kalman filter studies, but I was unable to extract any usable info on how to get the data output from accel/gyro, translated to degrees or a scaled output.

when I first started on this i thought the output would be a simple linear scale... like a ball in a bowl, tip or move in any direction an it would be a simple scale... but I found that its more like a ball in a box with only 3 points being truly discernible (0 for level, max output value equals completely vertical, and max output value times .71 equals 45deg angle).

I cannot find a math function to create an angle from these output values. Pythagorean doesnt work with the raw outputs, nor with output converted to milli g's

I'm lost right now..

Let say i just want to do something as simple as take these accel readings and turn them into a single angle to tell how far the Z-axis is from being level.

With the Y-axis being orientated up, raw output values at 'level': Y=16,400, X=0, Z=0

X = 0

Y = +15,000

Z = + 6,500

Exactly how many degrees is the Z-axis tipped? Assume Z-axis is 0deg at level/horizontal, 90deg when vertical, 180deg when flipped over... so a 360deg scale

*I may be mistaken on the max output value at rest... I think instead of 16,400 it may actually be 16,480?... i forgot how to calculate max on a 16bit chip

8,68474Can you please provide proof by answering my question?

I have translated the raw output values from above, to G-force so you can elaborate with the Trig functions and Pythagoras theorem to provide an angle in degrees.

X= 0

Y= .910

Z= .400

8,684I am assuming the Y axis is the vertical axis of the robot. Is that correct?

Edit to provide answer:

When using G force the Y reading is the sin() of the angle, and the Z is the cos()

Looking up 0.910 gives an angle of 66 degrees, as does the 0.400 cos() value.

Gets a little trickier when you have both x and y non-zero values since both affect the y angle.

74Visibly, its more around 20deg

1,981atan2(0.4, 0.91) = 26.4 degrees.

Nope, no combination of those values gives 66 degrees!

74How do I go about using atan2 in SPIN?

1,424obex.parallax.com/object/233

dgately

74atan2(0.4, 0.9) returns 1054220629

what is goin on here?

1,424Give an example of how you are using the library?

1,4241,981arctan of 0.4/0.9

74So right now im trying to get the current angle in degrees that it is tipped fwd/bck/lft/rght from the accelerometer.... thats it.

unfortunately there are no good resources to learn how to do this... every knowledge source I have read, is in individual pieces of interest which always use unrelated and unusable examples.

So I unfortunately have to do this from scratch. I really wish there was a single formula for use with IMU's to get results in deg from an IMU with accel/gyro output.

Seems to me the best place to start is with simply getting angles based on 'stable' orientation (0 g's representing 'stable' except for the one axis on vertical (Y in this case))...

then later on, use gyroscope outputs to increase accuracy.

The accelerometer outputs raw values from +16,480 to -16,480. included in the LSM9 IMU object from 'Jesse Burt' in OBEX is a way to translate to milli-g's.

The output from the milli-g routine gives a result in one thousandths of a G-force... ie 1000 = 1G

So just to try and get some working math, I have provided the raw outputs along with converted milli-g outputs of a static position.

RAW

X = 0

Y = +15,000

Z = + 6,500

Milli-G

X= 0

Y= .910

Z= .400

So far, Mark_T you have been the closest to providing an actual step forward... but, I am completely unfamiliar with atan2, and have been unable to duplicate your results.

Here is my current code... what am I doing wrong?

Thanks for the help, this is a real lonely project

74915None of the solutions are gong to be written in SPIN.

Balancing math is much simpler that copter math. In copter math they use Quaternions to represent the position of the copter in 3D space. Using that model they can determine the Roll, Pitch and Yaw of the copter to make adjustments.

You can use Quaternions to balance a robot, and some examples do that.

Simply stated if you use the accelerometer and the object is not moving the math works and you can determine the angle that the robot is at. If however the robot is moving then you need to use the gyro's to determine at what rate the object is moving at and subtract it for the accelerometer to get the corrected angle.

I for one have not been able to get this to works using a number of different builds and I used C examples from a number of these robots.

There are some chips out there that have the math built into the chip so that all you need to do is read the values and determine the angle. I have a BNO055 chip but have not used it yet. I'm kind of burnt out on this right now.

Mike

915Here is a video link of it in action

Mike

8,684You are right, that angle is wrong. I am getting the vertical/horizontal axis mixed up. Also , I am not combining the two readings, just using the cos() of one and sin() of the other to look up the angle. With the correct axis cos of 0.910 is between 24 and 25 degrees, while sin of 0.400 is between 23 and 24 degrees, which is close to what using the atan2 result produces.

While it is possible to do these types of calculations using trig I have to admit that using the atan2 function makes them so much easier. Thanks for the correction and the enlightenment.

74915Mike

1,981conversion factor by heart once you use trig.

74That gives = 123041749546.46190664773073918307

74915Now if you set the accel to 1g scale that means that a reading of 16393 = 1g of force.

Doing the ATAN2(AX,AZ) gives you the angle of the accelerometer. bearing in mind that you need to calibrate the accelerometer on a flat surface before you use it.

So with the unit sitting flat on the table the AX should return close to 0 and AZ will be close 16393 unless you have it upside down then it would be -16393.

So if AX = 5 and AZ = 16393 then ATAN2 returns 1.570491 or 89.98 degrees.

Mike

8,68474iseries:

"ATAN2 returns 1.570491 or 89.98 degrees."

When I use atan2 like this (atan2(5,16393)), i get a result of 966840320

I am using atan2 from obex: http://obex.parallax.com/object/233 in file 'Float32Full.spin'

how come you get a valid result when I'm a few hundred million degrees off?

Is that a bad obex?

74but of course when I try to apply 966840320 to ATAN in spin from that obex... I once again get a multi-hundred million result.

I'm guessing at this point I need to look for another Atan2 function obex... its as if this one only performs half of the calculations

8,684I have attached a PDF of the spreadsheet.

PS - You could install LibreOffice. Open source, free, and an excellent office suite. It's what I use.

Are you converting the integer values from the accelerometers to floating point before calling the atan2 subroutine?

74another thing that make me think atan2 isnt working properly... if the accel value is a negative, atan2 returns -1