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
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 have a 3D point in space, and I need to know how to pitch/yaw/roll my current heading (in the form of a 3d unit vector) to face a point. I am familiar with quaternions and rotation matrices, and I know how to represent the total rotation necessary to get my desired answer.
However, I only have control over pitch, yaw, and roll velocities (I can 'instantaneously' set their respective angular velocities), and only occasional updates on my new orientation (once every second or so). The end goal is to have some sort of PID controller (or three separates ones, but I suspect it won't work like that) controlling my current orientation. the end effect would be a slow (and hopefully convergent) wobble towards a steady state in the direction of my destination.
I have no idea how to convert the current desired quaternion/rotation matrix into a set of pitch-yaw-roll angular velocities (some sort of quaternion derivative or something?). I'm not even sure what to search for. I'm also uncertain how to apply a PID controller to this system, because I suspect there will need to be one controller for the trio as opposed to treating them each independently (although intuitively I feel this should be possible). Can anyone offer any guidance?
As a side note, if there is a solution that just involves a duo (pitch/yaw, roll/pitch, etc), then that works just fine too. I should only need 2 rotational degrees of freedom for this, but that is further from a realm that I am familiar with so I was less confident forming the question around it.
First take a look if your problem can be solved using quaternion SLERP [1], which can let you specify a scalar between 0 and 1 as the control to move from q1-->q2.
If you still need to control using the angular rotations then you can calculate the error quaternion as Nico Schertler suggested.
From that error quaternion you can use the derivative property of the quaternion (Section 4 of http://www.ecsutton.ece.ufl.edu/ens/handouts/quaternions.pdf [2]) to work out the angular rates required.
I'm pretty sure that will work, but if it does not you can also look at using the SLERP derivative (eq. 23 of http://www.geometrictools.com/Documentation/Quaternions.pdf [3]) and equating that to the Right-Hand-Side of the equation in source [2] to again get angular rates. The disadvantage to this is that you need code implementations for the quaternion exponentiation and logarithm operations.
I have an input device that gives me 3 angles -- rotation around x,y,z axes.
Now I need to use these angles to rotate the 3D space, without gimbal lock. I thought I could convert to Quaternions, but apparently since I'm getting the data as 3 angles this won't help?
If that's the case, just how can I correctly rotate the space, keeping in mind that my input data simply is x,y,z axes rotation angles, so I can't just "avoid" that. Similarly, moving around the order of axes rotations won't help -- all axes will be used anyway, so shuffling the order around won't accomplish anything. But surely there must be a way to do this?
If it helps, the problem can pretty much be reduced to implementing this function:
void generateVectorsFromAngles(double &lastXRotation,
double &lastYRotation,
double &lastZRotation,
JD::Vector &up,
JD::Vector &viewing) {
JD::Vector yaxis = JD::Vector(0,0,1);
JD::Vector zaxis = JD::Vector(0,1,0);
JD::Vector xaxis = JD::Vector(1,0,0);
up.rotate(xaxis, lastXRotation);
up.rotate(yaxis, lastYRotation);
up.rotate(zaxis, lastZRotation);
viewing.rotate(xaxis, lastXRotation);
viewing.rotate(yaxis, lastYRotation);
viewing.rotate(zaxis, lastZRotation);
}
in a way that avoids gimbal lock.
If your device is giving you absolute X/Y/Z angles (which implies something like actual gimbals), it will have some specific sequence to describe what order the rotations occur in.
Since you say that "the order doesn't matter", this suggests your device is something like (almost certainly?) a 3-axis rate gyro, and you're getting differential angles. In this case, you want to combine your 3 differential angles into a rotation vector, and use this to update an orientation quaternion, as follows:
given differential angles (in radians):
dXrot, dYrot, dZrot
and current orientation quaternion Q such that:
{r=0, ijk=rot(v)} = Q {r=0, ijk=v} Q*
construct an update quaternion:
dQ = {r=1, i=dXrot/2, j=dYrot/2, k=dZrot/2}
and update your orientation:
Q' = normalize( quaternion_multiply(dQ, Q) )
Note that dQ is only a crude approximation of a unit quaternion (which makes the normalize() operation more important than usual). However, if your differential angles are not large, it is actually quite a good approximation. Even if your differential angles are large, this simple approximation makes less nonsense than many other things you could do. If you have problems with large differential angles, you might try adding a quadratic correction to improve your accuracy (as described in the third section).
However, a more likely problem is that any kind of repeated update like this tends to drift, simply from accumulated arithmetic error if nothing else. Also, your physical sensors will have bias -- e.g., your rate gyros will have offsets which, if not corrected for, will cause your orientation estimate Q to precess slowly. If this kind of drift matters to your application, you will need some way to detect/correct it if you want to maintain a stable system.
If you do have a problem with large differential angles, there is a trigonometric formula for computing an exact update quaternion dQ. The assumption is that the total rotation angle should be linearly proportional to the magnitude of the input vector; given this, you can compute an exact update quaternion as follows:
given differential half-angle vector (in radians):
dV = (dXrot, dYrot, dZrot)/2
compute the magnitude of the half-angle vector:
theta = |dV| = 0.5 * sqrt(dXrot^2 + dYrot^2 + dZrot^2)
then the update quaternion, as used above, is:
dQ = {r=cos(theta), ijk=dV*sin(theta)/theta}
= {r=cos(theta), ijk=normalize(dV)*sin(theta)}
Note that directly computing either sin(theta)/theta ornormalize(dV) is is singular near zero, but the limit value of vector ijk near zero is simply ijk = dV = (dXrot,dYrot,dZrot), as in the approximation from the first section. If you do compute your update quaternion this way, the straightforward method is to check for this, and use the approximation for small theta (for which it is an extremely good approximation!).
Finally, another approach is to use a Taylor expansion for cos(theta) and sin(theta)/theta. This is an intermediate approach -- an improved approximation that increases the range of accuracy:
cos(x) ~ 1 - x^2/2 + x^4/24 - x^6/720 ...
sin(x)/x ~ 1 - x^2/6 + x^4/120 - x^6/5040 ...
So, the "quadratic correction" mentioned in the first section is:
dQ = {r=1-theta*theta*(1.0/2), ijk=dV*(1-theta*theta*(1.0/6))}
Q' = normalize( quaternion_multiply(dQ, Q) )
Additional terms will extend the accurate range of the approximation, but if you need more than +/-90 degrees per update, you should probably use the exact trig functions described in the second section. You could also use a Taylor expansion in combination with the exact trigonometric solution -- it may be helpful by allowing you to switch seamlessly between the approximation and the exact formula.
I think that the 'gimbal lock' is not a problem of computations/mathematics but rather a problem of some physical devices.
Given that you can represent any orientation with XYZ rotations, then even at the 'gimbal lock point' there is a XYZ representation for any imaginable orientation change. Your physical gimbal may be not able to rotate this way, but the mathematics still works :).
The only problem here is your input device - if it's gimbal then it can lock, but you didn't give any details on that.
EDIT: OK, so after you added a function I think I see what you need. The function is perfectly correct. But sadly, you just can't get a nice and easy, continuous way of orientation edition using XYZ axis rotations. I haven't seen such solution even in professional 3D packages.
The only thing that comes to my mind is to treat your input like a steering in aeroplane - you just have some initial orientation and you can rotate it around X, Y or Z axis by some amount. Then you store the new orientation and clear your inputs. Rotations in 3DMax/Maya/Blender are done the same way.
If you give us more info about real-world usage you want to achieve we may get some better ideas.
I have an object w/ and orientation and the rotational rates about each of the body axis. I need to find a smooth transition from this state to a second state with a different set of rates. Additionally, I have constraints on how fast I can rotate/accelerate about each of the axis.
I have explored Quaternion slerp's, and while I can use them to smoothly interpolate between the states, I don't see an easy way to get the rate matching into it.
This feels like an exercise in differential equations and path planning, but I'm not sure exactly how to formulate the problem so that the algorithms that are out there can work on it.
Any suggestions for algorithms that can help solve this and/or tips on formulating the problem to work with those algorithms would be greatly appreciated.
[Edit - here is an example of the type of problem I'm working on]
Think of a gunner on a helicopter that needs to track a target as the helicopter is flying. For the sake of argument, he needs to be on the target from the time it rises over the horizon to the time it is no longer in view. The relative rate of this target is not constant, but I assume that through the aggregation of several 'rate matching' maneuvers I can approximate this tracking fairly well. I can calculate the gun orientation and tracking rates required at any point, it's just generating a profile from some discrete orientations and rates that is stumping me.
Thanks!
First of all your rotational rates about each axis should compose into a rotational rate vector (i.e. w = [w_x w_y w_z]^T). Then you can separate the magnitude of the rotation from the axis of the rotation. The magnitude is w_mag = w/|w|. Then the axis is the unit vector u = w/w_mag. You can then update your gross rotation by composing an incremental rotation using your favorite representation (i.e. rotation matrices, quaternions). If your starting rotation is R_0 and your incrementatl rotation is defined by R_inc(w_mag*dt, u) then you follow the following composition rules:
R_1 = R_0 * R_inc
R_k+1 = R_k * R_inc
enjoy.
So I have written a Quaternion based 3D Camera oriented toward new programmers so it is ultra easy for them to integrate and begin using.
While I was developing it, at first I would take user input as Euler angles, then generate a Quaternion based off of the input for that frame. I would then take the Camera's Quaternion and multiply it by the one we generated for the input, and in theory that should simply add the input rotation to the current state of the camera's rotation, and things would be all fat and happy. Lets call this: Accumulating Quaternions, because we are storing and adding Quaternions only.
But I noticed that there was a problem with this method. The more I used it, even if I was only rotating on one Euler angle, say Yaw, it would, over some iterations, begin bleeding over into another, say Pitch. It was slight, but fairly unacceptable.
So I did some more research and found an article stating it was better to accumulate Euler angles, so the camera stores it's current rotation as Euler angles, and input is simply added to them each frame. Then I generate a Quaternion from them each frame, which is in turn used to generate my rotation matrix. And this fixed the issue of rotation bleeding into improper axes.
So do any Stackoverflow members have any insight into this problem? Is that a proper way of doing things?
Multiplying quaternions is going to suffer from accumulation of floating-point roundoff issues (even simple angles like 45 degrees won't be exact). It's a great way to composite rotations, but the precision of each of your quaternion components is going to drop-off over time. The bleed-through is one side-effect, a visually worse one though is your quaternion could start incorporating a scale factor - to recover that, you'd have to renormalize back to Euler angles in any case. A fixed-point Euler angle isn't going to accumulate roundoff.
Recalculating the quaternion per-frame is minimal. I wouldn't bother trying to optimize it out. You could probably allow a few quaternions to accumulate before you renormalized to get the accuracy back, but it really isn't worth the effort.
Accumulation is an inexact process. Accumulating lots of incremental rotations will accumulate roundoff error whether you do it with quaternions or matrices.
I imagine something like this: you got your code up and running, but noticed that after a certain amount of navigation your camera was heeling over annoyingly -- violating an invariant you hadn't thought of in advance. Effectively, you've realized you don't want to accumulate rotations; instead you want to do something else.
You can look at this as more of an interface design issue than a numerical accuracy issue. Basically, people expect a camera to navigate according to pitch, yaw, and roll, so choosing to control and represent the angles directly can avoid a lot of problems.
The bummer here is that the quaterions seem to have become redundant (for this particular usage, at least). You still want the quaternions, though -- interpolating with the raw pitch/yaw/roll angles can be ugly. Again, it's an interface design question: you need to figure out where you'll need the quaternions, and how to get them in and out...
I've seen both argued for. I think the real question you'll have to deal with is flexibility in your camera system down the line; IMO yaw is generally more interesting in a third-person view (because you're going to rotate about the character's vertical axis). While you can arguably "yaw" around the vertical in first-person view as well, I'm not sure it's really the same thing.
However, I do think it's kind of a waste to recalculate your quaternions per-frame. Perhaps it would be better to store the latest quaternions and mark them dirty if your frame receives input?