Given an Accelerometer with 9 DOF (Accelerometer, Gyroscope and Magnetometer) I want to remove/compensate the effect of the gravity in accelerometer reading (Accelerometer can rotate freely). The sensor gives the orientation in quaternion representation relative to a (magnetic)north, west and up reference frame.
I found this http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus
but couldn't understand the basis for the given equation.
How could I achieve this given above information?
You need to rotate the accelerometer reading by the quaternion into the Earth frame of reference (into the coordinate system of the room if you like), then subtract gravity. The remaining acceleration is the acceleration of the sensor in the Earth frame of reference often referred to as linear acceleration or user acceleration.
In pseudo-code, something like this
acceleration = [ax, ay, ay] // accelerometer reading
q // quaternion corresponding to the orientation
gravity = [0, 0, -9.81] // gravity on Earth in m/s^2
a_rotated = rotate(acceleration, q) // rotate the measured acceleration into
// the Earth frame of reference
user_acceleration = a_rotated - gravity
You say that you can get q through the API. The only nontrivial step is to implement the rotate() function.
To compute the image of a vector v when rotated by q, the following formula should be applied: vrotated = qvq-1. To compute it with floating point numbers, you need to work out the formulas yourself; they are available at Using quaternion rotations.
As far as I can tell, the link you provided does exactly this, you see the expanded formulas there and now you know where they came from. Also, the linked content seems to measure gravity in g, that is, gravity is [0,0,-1].
Watch out for sign conventions (whether you consider gravity [0,0,-1] or [0,0,1]) and handedness of your coordinate systems!
I assume your accelerometer reading is in sensor body frame. First we need to represent accelerometer data with respect to inertial frame, and then subtract gravity.
If you are directly using Euler angles rather than quaternion, then you need to compute rotation matrix
R = [
ctheta*cpsi,
-cphi*spsi + sphi*stheta*cpsi,
sphi*spsi + cphi*stheta*cpsi;
ctheta*spsi, cphi*cpsi + sphi*stheta*spsi,
-sphi*cpsi + cphi*stheta*spsi;
-stheta, sphi*ctheta, cphi*ctheta
]
(It's given with MATLAB notation). Here phi stands for roll angle, theta for pitch, and psi for yaw. This R matrix is from body to inertial frame. I think in flight dynamics it's also known as transpose of Direction Cosine Matrix (DCM).
When you apply matrix multiplication operation, now you need to subtract gravity from z direction in order to eliminate static acceleration, i.e., gravity.
Related
I recently had to convert euler rotation rates to vectorial angular velocity.
From what I understand, in a local referential, we can express the vectorial angular velocity by:
R = [rollRate, pitchRate, yawRate] (which is the correct order relative to the referential I want to use).
I also know that we can convert angular velocities to rotations (quaternion) for a given time-step via:
alpha = |R| * ts
nR = R / |R| * sin(alpha) <-- normalize and multiply each element by sin(alpha)
Q = [nRx i, nRy j, nRz k, cos(alpha)]
When I test this for each axis individually, I find results that I totally expect (i.e. 90°pitch/time-unit for 1 time unit => 90° pitch angle).
When I use two axes for my rotation rates however, I don't fully understand the results:
For example, if I use rollRate = 0, pitchRate = 90, yawRate = 90, apply the rotation for a given time-step and convert the resulting quaternion back to euler, I obtain the following results:
(ts = 0.1) Roll: 0.712676, Pitch: 8.96267, Yaw: 9.07438
(ts = 0.5) Roll: 21.058, Pitch: 39.3148, Yaw: 54.9771
(ts = 1.0) Roll: 76.2033, Pitch: 34.2386, Yaw: 137.111
I Understand that a "smooth" continuous rotation might change the roll component mid way.
What I don't understand however is after a full unit of time with a 90°/time-unit pitchRate combined with a 90°/time-unit yawRate I end up with these pitch and yaw angles and why I still have roll (I would have expected them to end up at [0°, 90°, 90°].
I am pretty confident on both my axis + angle to quaternion and on my quaternion to euler formulas as I've tested these extensively (both via unit-testing and via field testing), I'm not sure however about the euler rotation rate to angular-velocity "conversion".
My first bet would be that I do not understand how euler rotation-rates axes interacts on themselves, my second would be that this "conversion" between euler rotation-rates and angular velocity vector is incorrect.
Euler angles are not good way of representing arbitrary angular movement. Its just a simplification used for graphics,games and robotics. They got some pretty hard restrictions like your rotations consist of only N perpendicular axises in ND space. That is not how rotation works in real world. On top of this spherical representation of reper endpoint it creates a lot of singularities (you know when you cross poles ...).
The rotation movement is analogy for translation:
position speed acceleration
pos = Integral(vel) = Integral(Integral(acc))
ang = Integral(omg) = Integral(Integral(eps))
That in some update timer can be rewritten to this:
vel+=acc*dt; pos+=vel*dt;
omg+=eps*dt; ang+=omg*dt;
where dt is elapsed time (Timer interval).
The problem with rotation is that you can not superimpose it like translation. As each rotation has its own axis (and it does not need to be axis aligned, nor centered) and each rotation affect the axis orientation of all others too so the order of them matters a lot. On top of all this there is also gyroscopic moment creating 3th rotation from any two that has not parallel axis. Put all of this together and suddenly you see Euler angles does not match the real geometrics/physics of rotation. They can describe orientation and fake its rotation up to a degree but do not expect to make real sense once used for physic simulation.
The real simulation would require list of rotations described by the axis (not just direction but also origin), angular speed (and its change) and in each simulation step the recomputing of the axis as it will change (unless only single rotation is present).
This can be done by using coumulative homogenous transform matrices along with incremental rotations.
Sadly the majority of programmers prefers Euler angles and Quaternions simply by not knowing that there are better and simpler options and once they do they stick to Euler angles anyway as matrix math seem to be more complicated to them... That is why most nowadays games have gimbal locks, major rotation errors and glitches, unrealistic physics.
Do not get me wrong they still have their use (liek for example restrict free look for camera etc ... but they missused for stuff they are the worse option to use for.
I want to simulate particles driven by wind on a three.js globe. The data I have is a Vector3 for the position of a particle and a Vector2 indicating wind speed and direction, think North/East. How do I get the new Vector3?
I've consulted numerous examples and read the documentation and believe the solution involves quaternions, but the axis of rotation is not given. Also, there are thousands of particles, it should be fast, however real-time is not required.
The radius of the sphere is 1.
I would recommend you have a look at the Spherical class provided by three.js. Instead of cartesian coordinates (x,y,z), a point is represented in terms of a spherical coordinate-system (θ (theta), φ (phi), r).
The value of theta is the longitude and phi is the latitude for your globe (r - sphereRadius would be the height above the surface). Your wind-vectors can then be interpreted as changes to these two values. So what I would try is basically this:
// a) convert particle-location to spherical
const sphericalPosition = new THREE.Spherical()
.setFromVector3(particle.position);
// b) update theta/phi (note that windSpeed is assumed to
// be given in radians/time, but for a sphere of size 1 that
// shouldn't make a difference)
sphericalPosition.theta += windSpeed.x; // east-direction
sphericalPosition.phi += windSpeed.y; // north-direction
// c) write back to particle-position
particle.position.setFromSpherical(sphericalPosition);
Performance wise this shouldn't be a problem at all (maybe don't create a new Spherical-instance for every particle like I did above). The conversions involve a bit of trigonometry, but we're talking just thousands of points, not millions.
Hope that helps!
If you just want to rotate a vector based on an angle, just perform a simple rotation of values on the specified plane yourself using trig as per this page eg for a rotation on the xz plane:
var x = cos(theta)*vec_to_rotate.x - sin(theta)*vec_to_rotate.z;
var z = sin(theta)*vec_to_rotate.x + cos(theta)*vec_to_rotate.z;
rotated_vector = new THREE.Vector3(x,vec_to_rotate.y,z);
But to move particles with wind, you're not really rotating a vector, you should be adding a velocity vector, and it 'rotates' its own heading based on a combination of initial velocity, inertia, air friction, and additional competing forces a la:
init(){
position = new THREE.Vector(0,0,0);
velocity = new THREE.Vector3(1,0,0);
wind_vector = new THREE.Vector3(0,0,1);
}
update(){
velocity.add(wind_vector);
position.add(velocity);
velocity.multiplyScalar(.95);
}
This model is truer to how wind will influence a particle. This particle will start off heading along the x axis, and then 'turn' eventually to go in the direction of the wind, without any rotation of vectors. It has a mass, and a velocity in a direction, a force is acting on it, it turns.
You can see that because the whole velocity is subject to friction (the multscalar), our initial velocity diminishes as the wind vector accumulates, which causes a turn without performing any rotations. Thought i'd throw this out just in case you're unfamiliar with working with particle systems and maybe were just thinking about it wrong.
I have edited this question for additional clarity and because of some of the answers below.
I have an electromagnetic motion tracker which tracks a sensor and gives me a point in global space (X, Y, Z). It also tracks the rotation of the sensor and gives Euler angles (Yaw, Pitch, Roll).
The sensor is attached to a rigid body on a baseball cap which sits on the head of a person. However, I wish to track the position of a specific facial feature (nose for example) which I infer from the motion tracker sensor's position and orientation.
I have estimated the spatial offset between the motion tracker and the facial features I want to track. I have done this by simply measuring the offset along the X, Y and Z axis.
Based on a previous answer to this question, I have composed a rotation matrix from the euler angles given to me by the motion tracker. However, I am stuck with how I should use this rotation matrix, the position of the sensor in global space and the spatial offset between that sensor and the nose to give me the position of the nose in global space.
The sensor will give you a rotation matrix (via the Euler angles) and a position (which should be that of the center of rotation).
Whatever item is rigidly fastened to the sensor, such as the nose, will undergo the same motion. Then knowing the relative coordinates of the nose and the sensor, you get the relation
Q = R.q + P
where R is the rotation matrix, P the position vector of the sensor and q the relative coordinates of the nose.
Note that the relation between the rotation matrix and the angles can be computed using one of these formulas: https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix. (You will need to read the article carefully to make sure which your cases is among the 12 possibilities.)
In principle, you determine R and P from the readings of the sensor, but you are missing the coordinates q. There are several approaches:
you determine those coordinates explicitly by measuring the distances along virtual axes located at the rotation center and properly aligned.
you determine the absolute coordinates Q of the nose corresponding to known R and P; then q is given by R'(Q - P) where R' denotes the transpose of R (which is also its inverse). To obtain Q, you can just move the sensor center to the nose without moving the head.
I have been using a 6dof LSM6DS0 IMU unit (with accelerometer and gyroscope). And I am trying to calculate the angle of rotation around all the three axes. I have tried may methods but not getting the results as expected.
Methods tried:
(i) Complementatry filter approach - I am able to get the angles using the formula provided in the link Angle comutation method.
But the problem is that angles are not at all consistent and drifts a lot. Moreover when the IMU is rotated around one axis, angles calculated over other axis are wobbling too much.
(ii) Quaternion based angle calculation : There were plenty of resources claiming the angles are calcluated very well using quaternion approach but none had a clear explanation. I have used this method in order to update the quaternion for every values taken from the IMU unit. But the link dint explain how to calculate the angles from quaternion.
I have used glm math library inorder to convert the quaternion to euler angles and also have tried the formula specified in wiki link. With this method since in pitch calculation asin returns only -90 to +90 degrees I am not able to rotate the object in 3D as the one they have been doing in the mentioned link.
Does anyone have tried the quaternion to angle conversion before?? I need to calculate the angles around all the three axis in the range 0 to 360 degrees or -180 to +180 degrees.
Any help could be really appreciated. Thanks in advance.
http://davidegironi.blogspot.com.ar/2013/02/avr-atmega-mpu6050-gyroscope-and.html#.VzjDKfnhCUk
Sq = [q0, q1, q2, q3]
//sensor quaternion can be translated to aerospace sequence of yaw/pitch/roll angles
yaw = atan2(2*q1*q2 - 2*q0*q3, 2*(q0^2) + 2*(q1^2) - 1)
pitch = -asin(2*q1*q3 + 2*q0*q2)
roll = atan2(2*q2*q3 - 2*q0*q1, 2*(q0^2) + 2*(q3^2) - 1)
I've been trying to build a filter that can successfully combine compass, geomagnetic, and gyroscopic data to produce a smooth augmented reality experience. After reading this post along with lots of discussions, I finally found out a good algorithm to correct my sensor data. Most examples I've read show how to correct accelerometers with gyroscopes, but not correct compass + accelerometer data with gyroscope. This is the algorithm I've settled upon, which works great except that I run into gimbal lock if I try to look at the scene if I'm not facing North. This algorithm is Balance Filter, only instead of only implemented in 3D
Initialization Step:
Initialize a world rotation matrix using the (noisy) accelerometer and compass sensor data (this is provided by the Android already)
Update Steps:
Integrate the gyroscope reading (time_delta * reading) for each axis (x, y, z)
Rotate the world rotation matrix using the Euler angles supplied by the integration
Find the Quaternion from the newly rotated matrix
Find the rotation matrix from the unfiltered accelerometer + compass data (using the OS provided function, I think it uses angle/axis calculation)
Get the quaternion from the matrix generated in the previous step.
Slerp between quaternion generated in step 2 (from the gyroscope), and the accelerometer data using a coefficient based on some experimental magic
Convert back to a matrix and use that to draw the scene.
My problem is that when I'm facing North and then try to look south, the whole thing blows up and it appears to be gimbal lock. After a few gimbal locks, the whole filter is in an undefined state. Searching around I hear everybody saying "Just use Quaternions" but I'm afraid it's not that simple (at least not to me) and I know there's something I'm just missing. Any help would be greatly appreciated.
The biggest reason to use quaternions is to avoid the singularity problem with Euler angles. You can directly rotate a quaternion with gyro data.
Many appologies if information is delayed or not useful specifically but may be useful to others as I found it after some research:::
a. Using a kalman (linear or non linear) filter you do following ::
Gyro to integrate the delta angle while accelerometers tell you the outer limit.
b. Euler rates are different from Gyro rate of angle change so you ll need quaternion or Euler representation::
Quaternion is non trivial but two main steps are ----
1. For Roll, pitch,yaw you get three quaternions as cos(w) +sin(v) where w is scalar part and v is vector part (or when coding just another variable)
Then simply multiply all 3 quat. to get a delta quaternion
i.e quatDelta[0] =c1c2*c3 - s1s2*s3;
quatDelta[1] =c1c2*s3 + s1s2*c3;
quatDelta[2] =s1*c2*c3 + c1*s2*s3;
quatDelta[3] =c1*s2*c3 - s1*c2*s3;
where c1,c2,c3 are cos of roll,pitch,yaw and s stands for sin of the same actually half of those gyro pre integrated angles.
2. Then just multiply by old quaternion you had
newQuat[0]=(quaternion[0]*quatDelta[0] - quaternion[1]*quatDelta[1] - quaternion[2]*quatDelta[2] - quaternion[3]*quatDelta[3]);
newQuat[1]=(quaternion[0]*quatDelta[1] + quaternion[1]*quatDelta[0] + quaternion[2]*quatDelta[3] - quaternion[3]*quatDelta[2]);
newQuat[2]=(quaternion[0]*quatDelta[2] - quaternion[1]*quatDelta[3] + quaternion[2]*quatDelta[0] + quaternion[3]*quatDelta[1]);
newQuat[3]=(quaternion[0]*quatDelta[3] + quaternion[1]*quatDelta[2] - quaternion[2]*quatDelta[1] + quaternion[3]*quatDelta[0]);
As you loop through the code it gets updated so only quatenion is a global variables not the rest
3. Lastly if you want Euler angles from them then do the following:
`euler[2]=atan2(2.0*(quaternion[0]*quaternion[1]+quaternion[2]*quaternion[3]), 1-2.0*(quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]))euler[1]=safe_asin(2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]))euler[0]=atan2(2.0*(quaternion[0]*quaternion[3]+quaternion[1]*quaternion[2]), 1-2.0*(quaternion[2] *quaternion[2]+quaternion[3]*quaternion[3]))`
euler[1] is pitch and so on..
I just wanted to outline general steps of quaternion implementation. There may be some minor errors but I tried this myself and it works. Please note that when changing to euler angles you will get singularities also called as "Gimbal lock"
An important note here is that this is not my work but I found it over the internet and wanted to thank who ever did this priceless code...Cheers