I am building a robot and I need to keep track of turns. In other words I need to know the amount of rotation about the Z-axis where Z points straight up into the sky. So just a single scalar number.
Unfortunately my 6050 chip is on a pcb board that is tilted. So the Z-axis of my 6050 attitude sensor is at an angle. I need to convert the quternion given by the 6050 into a simple single-number rotation.
So far I have been unable to understand the mathmatical operations needed. Another person reported he understood them after a week of studying. I'm hoping someone can just tell me what operations to perform to get the result I need. I don't want to spend a week to get there. (Too little time and I'm lazy).
I have the arduino library to read quaternions from the 6050 and to perform the basic operations on them, i.e. product (multiplication), conjugation, magnitude, normalization, and rotation. I'm hoping I only need multiplication, but even if that is true I don't know how to get the quaternion to multiply against.
Can someone much smarter than me give a hand?
EDIT: I changed "I'm hoping I only need rotation" to "I'm hoping I only need multiplication".
The MPU6050 has no yaw reference, and without one, it is not possible to determine the value you want.
All derived yaw values will always be relative to the starting yaw value, and will always drift away from that origin due to rate gyro drift.
For your vehicle it would be much easier to use a tilt compensated compass, for which you need a magnetometer and accelerometer. If you go take that path, be sure to calibrate the magnetometer, after installation in the vehicle.
The MPU6050 has no yaw reference
you need a magnetometer
I only care about relative yaw and I don't care where north is. I will zero out my rotation/position when the robot boots up and then just use relative rotation/position from there. This is just a toy.
My main need for rotation is to be able to go in a straight line. It only has two wheels and right now it wanders drunkenly because the motors don't perform the same even when given the same voltage. This make driving hard. I did not anticipate this in the initial design although I should have. A four-wheel vehicle with all wheels straight doesn't have this problem.
I will use feedback from the 6050 to adjust voltage to the motors. With my planned simple transformation it will only work on a flat floor. If it goes up a ramp the driver will have to compensate although that will be rare.
With the code below, it does not matter whether the MPU-6050 board is tilted. Yaw is relative, and calculated about an axis parallel to the gravity vector.
The gyro must be very carefully calibrated to reduce drift.
That code looks awesome. It's as if you wrote if for me. I don't understand much of the math but it doesn't matter as long as it works. I have one question and one concern.
Why are AHRS algorithms called filters? My engineering experience says a filter reduces things, like frequency in an audio filter or removing some variable. It seems to me that AHRS does PID stuff (minus the D) to update an equivalent number of vars.
My concern is how much effort will be involved to calibrate each 6050 in a production environment for a cheap toy. In yuor experience can the calibration be somewhat automated of at least quickly done manualy if not? I admit I haven't figured out how to do the calibration. I will need to do some studying. Maybe you could point me to something on the web about performing the calibration?
I really appreciate your code and your time helping me.
Ignore my question abut why they are called filters. I assume it is because they are reducing the 6 vars, 3 each for rotation and acceleration, to just 3 for yaw, roll, and pitch.
The Mahony AHRS and similar algorithms are called filters because they combine the contributions of several orientation sensors to produce one "average" orientation, which can be represented by three angles or one quaternion.
Gyros can be calibrated upon program start as long as the sensor is not moving or being reoriented. Simply collect a couple of hundred measurements, sum and average to calculate the X, Y and Z offsets to be subtracted. See the calibration code in the above Github link for the details.
The MPU-6050 has not been manufactured for several years, and what you get on the market today are most likely Chinese clones (not guaranteed to work as expected), rejects or outright fakes.
For a commercial product you will need to design for and buy a newer 6DOF or 9DOF sensor.
So I can have a maintenance procedure in production that does this and stores the results in flash. Right?
The MPU-6050 has not been manufactured for several years
Unfortunately the factory in China that makes our boards has a specific list of parts we can use and the 6050 is the only attitude sensor on the list. It is just a toy though.
You could, but the gyro drift rate depends on temperature, age of part, supply voltage, etc.
If you expect the toy to actually work, you should ask about data sheets and quality control guarantees. The Chinese clones of parts that are sold cheaply on Alibaba, Amazon and the like are notoriously unreliable.
I now understand your code well (except for the specific quaternion math). Your code gives me rotation info including yaw which answers my original question perfectly.
Sorry to bother you again but I have another question.
In addition to my need for yaw info to control turns I need acceleration info to control speed. Specifically I need it in forward (X-axis) direction. I might be wrong, but I don't see acceleration relative to body frame in your output. Is this something simple to calculate?
Here's my thinking so far. The Euler vector you calculate is effectively the gravity vector relative to the body, correct? Can I just subtract that from the raw acceleration vector to eliminate the gravity component leaving an actual motion vector? So the body sitting still would have zero values. Then I can just use the remaining X component as my forward acceleration.
This code calculates the estimated direction of the gravity vector, based on the current value of the quaternion.
// Estimated direction of gravity in the body frame (factor of two divided out)
vx = q[1] * q[3] - q[0] * q[2];
vy = q[0] * q[1] + q[2] * q[3];
vz = q[0] * q[0] - 0.5f + q[3] * q[3];
To obtain the so-called "linear acceleration" you subtract this from the measured acceleration (eg. ax, ay, az) after scaling the two vectors appropriately.
After they are both scaled in terms of m/s^2 ( 1 g = 9.8 m/s^2) then you can subtract the individual terms directly, e.g. the x acceleration in the body frame is (ax-vx) m/s^2.
As an overall check I can make sure the resulting acceleration vector is zero when sitting still. Thanks again for the thoughtful accurate quick responses.
I have been working with the chip parallel to the ground which is working. When I put the pcb in my toy I realized I'm back to the quesion in my original post, which was ...
My 6050 chip is on a pcb board that is tilted. So the Z-axis of my sensor is at an angle. I need to convert the quaternion given by the tilted 6050 into a simple single-number yaw. I need the yaw relative to my toy not my 6050 chip.
Ah. I rotate the quaternion and then convert that into the euler vectors using the math at the bottom of your code and take the yaw from that. Shouldn't be hard.
