StarkRG
Oct30-10, 06:56 PM
Background: I'm building a quadrotor (four-rotor helicopter). For detecting orientation and, hopefully, drift compensation I'm using a 3 axis accelerometer and a 3 axis gyroscope. Using the output of the accelerometer I've been able to create a vector of the acceleration. When left sitting still that vector points downward at a constant magnitude, in other words a vector of gravity. I need to be able to use the output of the gyroscope to rotate that vector around so I always know which way is down (and, thus, will be able to remove it from the data coming from the accelerometer). The gyro gives values for each axis that are converted to degrees per second which I have to change to radians per second to perform trig functions on. I then convert that to radians I have to rotate the gravity vector.
\Theta = radians to rotate around axis, {x,y,z} = components of current gravity vector, {x',y',z'} = components of rotated gravity vector
I know the formulas for rotating a vector in 2D:
x' = x*cos(\Theta) - y*sin(\Theta)
y' = x*sin(\Theta) + y*sin(\Theta)
I was able to construct a "rotation matrix":
x-axis : { x , y*cos(\Theta) - z*sin(\Theta) , y*sin(\Theta) + z*cos(\Theta) }
y-axis : { z*sin(\Theta) + x*cos(\Theta) , y , z*cos(\Theta) - x*sin(\Theta) }
z-axis : { x*cos(\Theta) - y*sin(\Theta) , x*sin(\Theta) + y*cos(\Theta) , z }
Great, now how do I convert this to {x',y',z'}? I thought that the columns should be the change in x, y, and z (respectively) due to the rotation about the x, y, and z axes, and thus could be combined through the Pythagorean theorem.
thus x' would be the square root of the sum of the squares of the first column (x' = sqrt(a^2+b^2+c^2)), y' the second column, and z' the last. Unfortunately this causes the vector to get really big over a few iterations.
Two things I'm sure of: the magnitude of the vector should never change, and no matter how I've twirled or tumbled it when it stops the vector should be (more or less) the same as the data coming from the accelerometer.
So far everything I do causes the vector to become zero or to become infinite.
\Theta = radians to rotate around axis, {x,y,z} = components of current gravity vector, {x',y',z'} = components of rotated gravity vector
I know the formulas for rotating a vector in 2D:
x' = x*cos(\Theta) - y*sin(\Theta)
y' = x*sin(\Theta) + y*sin(\Theta)
I was able to construct a "rotation matrix":
x-axis : { x , y*cos(\Theta) - z*sin(\Theta) , y*sin(\Theta) + z*cos(\Theta) }
y-axis : { z*sin(\Theta) + x*cos(\Theta) , y , z*cos(\Theta) - x*sin(\Theta) }
z-axis : { x*cos(\Theta) - y*sin(\Theta) , x*sin(\Theta) + y*cos(\Theta) , z }
Great, now how do I convert this to {x',y',z'}? I thought that the columns should be the change in x, y, and z (respectively) due to the rotation about the x, y, and z axes, and thus could be combined through the Pythagorean theorem.
thus x' would be the square root of the sum of the squares of the first column (x' = sqrt(a^2+b^2+c^2)), y' the second column, and z' the last. Unfortunately this causes the vector to get really big over a few iterations.
Two things I'm sure of: the magnitude of the vector should never change, and no matter how I've twirled or tumbled it when it stops the vector should be (more or less) the same as the data coming from the accelerometer.
So far everything I do causes the vector to become zero or to become infinite.