Gyros, Accelerometers and quaternions

curt2go · Joined: 21 Nov 2003 Posts: 200

I have been beating my head against the wall for 2 weeks now. I am using a LSM6DS1 ACC and Gyro. I am getting proper readings from them and I am using a complimentary filter to put them together. I'm using a 24EP256GP206 running at 140MHz as well. BUT my filteredAngleZ keeps zeroing out. But I don't think that is my fundamental issue with the calculations.

I do have the option of adding a Mag sensor as well if I need to in the future. But I don't think I need it for what I am doing.

My project is to sense bat position and calculate swings. I have been reading on quaternions ALOT and think this is the way to go. I am struggling with the math on a few things.

Pitch, roll and Yaw. I have been using ATAN2 calculations and I am getting the proper x,y but z is not right.

I have been trying to go from Euler to quaternions but something is not right. I have used a simulator to track the x,y and if I don't use Z then those coordinates are right but obviously that does not really give me 3d space.

I am adding the gyro info every 0.01 seconds for the math.
My tau is 0.5.

Any help would be totally awesome.

curt2go · Joined: 21 Nov 2003 Posts: 200

I should add that I do have the quaternion calculations and I can verify that they work by substituting known numbers into them.

temtronic · Posted: Wed Oct 12, 2016 2:44 pm

OK, I don't use that 'fancy math' but ...probably stating the obvious....

hmm....

curt2go · Joined: 21 Nov 2003 Posts: 200

I do agree with you on all fronts. I have done separate Accelerometer x,y,z measurements and all come out correct. I also do gyro x,y and z readings and they all come out. It is definitely working.

Does someone know the for sure Yaw ATAN2 calculation?

The trigonometry on this project is killing me.

But lets say I do end up with the correct roll. pitch and ,yaw. And then turn those into quaternions then multiply that by a new quaternion. What the heck does the output give me. As I read it , the output is the difference between the 2. So does that give me how far x has moved , y has moved and then z has moved? This is also where my visualisation is breaking down for this stuff.

I will do some tests again and show the results. I will add that my gyro is PIN 1 at the bottom left not at top left like datasheet. There is a gyro offset byte that can be set. I have done that. That also makes my roll, on the x axis and pitch on the y axis. z stays the same.

http://www.st.com/content/ccc/resource/technical/document/datasheet/group0/38/96/79/af/ab/62/42/c3/DM00229854/files/DM00229854.pdf/jcr:content/translations/en.DM00229854.pdf

page 17 shows the axis of rotation and the acc readings.

I have set it up with 010 for gyro readings.
pitch = Y roll = X yaw = Z

Orient [2:0] 000 001 010 011 100 101
Pitch X X Y Y Z Z
Roll Y Z X Z X Y
Yaw Z Y Z X Y X

curt2go · Joined: 21 Nov 2003 Posts: 200

Here is the raw data form the XL.

X,Y,Z format.
Flat, Back.
0,0,1
90 to the right.
1,0,0
Flat front
0,0,-1
90 to the left
-1,0,0
Straight up
0,1,0
Straight down
0,-1,0
XL facing up but board down at 45
0,-0.7,0.7
XL facing up but board up at 45
0,0.7,0.7
90 to right and up at 45
0.7,0.7,0
90 to right and up and back at 45
-0.7,0.7,0

Here is the raw Gyro data.
I will rotate it on each axis..
X,Y,Z
Tilt up
-moves,0,0 (- moves means it moved in negative direction)
Tilt down
+moves,0,0
Roll right
0,+moves,0
Roll left
0,-moves,0
Yaw right
0,0,-moves
Yaw left
0,0,+moves.

I really hope someone can help.. It seems my hardware is working properly.

curt2go · Joined: 21 Nov 2003 Posts: 200

Well it looks like I have accomplished something. I have made my euler angles work. I had a dumb issue with not using a float on my integration of the rate. Very dumb mistake!
Also I have found that my rotation is YZX not XYZ.

Now I have to translate all this into a swing and I thought the best way to do that is to convert to quaternions. Then take a sample every .2 seconds and multiply that quaternions by the last one and that should give me the difference. BUT the difference of what?

I will try on the simulator once I start outputting quaternions and see what I can come up with. But if anyone can help me with my next move it would be a great help. Thank you.