Hi all,
I'm trying to subtract gravitiy from my acceleration readings. I'm hoping to do this by converting acceleration readings from the CurieIMU to inertial reference frame, using Euler angles obtained via the rotational matrix below:
float ax_iner = (cos(roll) * cos(yaw) - sin(pitch)*sin(roll)*sin(yaw)) * ax_g + (cos(roll) * sin(yaw) + cos(yaw) * sin(pitch) * sin(roll)) * ay_g + (-cos(pitch)*sin(roll)) * az_g;
float ay_iner = (-cos(pitch) * sin(yaw)) * ax_g + (cos(pitch) * cos(yaw)) * ay_g + sin(pitch) * az_g;
float az_iner = (cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)) * ax_g + (sin(roll)*sin(yaw) - cos(roll) *cos(yaw) *sin(pitch)) * ay_g + (cos(roll) * cos(pitch)) * az_g;
given that it has a left-handed coordinate system and follows a right hand grip rotation convention. The codes basically works, however when I apply a large angle of tilt, the accelerations take quite a long time to decay back to zero with the board tilted, but stationary. This will give a large deviation when I integrate to find position. Is there a better way of doing this?
Cheers!
Yi Han
Hi Yi Han
You can check this out by CH Robotics: Understanding Euler Angles. I think this is pretty much what you are doing.
I think what you what to do is work with dynamic accelerations which is where you just adjust for the direction of gravity. You can see this post by Fabio Veresano: Simple gravity compensation for 9 DOM IMUs
Unfortunately the quaternions are not currently returned by the CurieIMU filter. If you need to do this I will send instructions on how to do this.
Mike
Hi Mike,
That'll be great, can you show me how to extract quaternions directly?
Cheers!
Yi Han
Morning Yin Han
I have attached a zip file with the updated MadgwickAHRS filter and a modified Visualize101 Arduino sketch called Visualize101Q.pde that shows you how to get the quaternions.
Just replace the MadgwickAHRS.h and Madgwick.cpp files in the Madgwick Arduino library with the ones you extracted from the zip file. Then put the Visaulize101Q folder wherever you want it. Then just load the new sketch into the IDE and you should be good to go.
Mike
MadgwickQ.zip (4.46 KB)
Hi Mike,
I had a go with Euler angles and I think I've got it to work, however when I tried to integrate it doesn't give me the distance travelled, rather it seems to drift with time.
Do you mind having a look at my uploaded code please?
Yi Han
imu_ori.ino (7.28 KB)
MadgwickAHRS.h (2.27 KB)
MadgwickAHRS.cpp (8.68 KB)
Hi Yi Han
Just did a quick look and I see what you mean - I will have to dig a little deeper to see why. I have run the filter and have not seen the kind of drifting you are getting. Going off line now so I will look at when I get a chance.
Mike
Hi Yi Han
Took a look at your sketch and for one you will get a 400 hz sample rate. Did a little playing around and you will have to stick to around 25 or so. Also, you had a few misplaced lines of code and conversion issues. Result was inaccurate orientation.
As for zero motion detection based on step detection its not working and I don't know enough about how step detection works at this point. This is why you continue to see "drifting" in your "S" values. What you are really seeing is variation in accelerometer reading from moment to moment (noise in the system). I have my own zero motion detection function. Think you would have to use the Zero Motion detect function in the BMI160 but I know of no examples floating around. Maybe some has got this to work or Intel will post an example sketch.
By the way you are using outdated madwick libraries if the files you attached are the ones in your library. Use the ones I sent you.
I am attaching the modified sketch.
Mike
imu_ori.ino (7.64 KB)
Hi Mike,
I tried running your uploaded code after replacing the new libraries and changing the time step to 0.04, the position values still drifts rapidly even after I commented out the step detection if condition. I also tried running the visualiser on Processing after changing its baud rate to 115200, however the board refreshes rapidly and gives exaggerated rotations even though my board is lying still on the table. Could this be the reason why the position drifts?
Yi Han
Hi Yi Han
Suggest you go back to 9600.
Anyway back to getting distances from a accelerometer. Suggest that you read this post calculating distances using accelerometer on StackOverflow. I am also attaching a app note that you might find interesting to read through for your project that I am attaching. Think it will help you immensely.
Mike
AN3397.pdf (312 KB)