I am working on a wearable device using the Invensense 9D (Accel+Gyro+Compass) which computes a quaternion for real-time orientation, using the embedded Digital Motion Processor running a Kalman filter. Sadly -- the DMP code is not something that Invensense allows end-users access to.
The resulting quaternions in our application suffer from some drift. But we've been able to work within that by taking the differences between the Euler angles over short time-frames. But Euler angles are obviously less than desirable because of their inherent issues (gimbal lock, etc).
Where I'm struggling is computing the difference between two quaternions -- which 'should' be Diff = q2 * conj(q1) -- then computing the Pitch and Roll changes required to get from q1 to q2. That's where my brain freezes.
Appreciate any suggestions/pointers.
UPDATE :
I do indeed need to display the angular differences -- in both pitch and roll -- to a user so that they can understand the orientation difference between the two orientations.
FS.Roll <<- atan2((2.0*(FS.Quat.Q0*FS.Quat.Q1 + FS.Quat.Q2*FS.Quat.Q3)), (1.0 - 2.0*(FS.Quat.Q1^2 + FS.Quat.Q2^2))) * M_PI;
FS.Pitch <<- asin((2.0*(FS.Quat.Q0*FS.Quat.Q2 - FS.Quat.Q3*FS.Quat.Q1))) * M_PI;
MP.Roll <<- atan2((2.0*(MP.Quat.Q0*MP.Quat.Q1 + MP.Quat.Q2*MP.Quat.Q3)), (1.0 - 2.0*(MP.Quat.Q1^2 + MP.Quat.Q2^2))) * M_PI;
MP.Pitch <<- asin((2.0*(MP.Quat.Q0*MP.Quat.Q2 - MP.Quat.Q3*MP.Quat.Q1))) * M_PI;
If I take the differences between the FS.Pitch and MP.Pitch and the corresponding FS.Roll and MP.Roll -- I get exactly what I'm after.
But I'd like to reduce the inverse trig calcs on the embedded Cortex-M0 MCU -- as well as avoiding Gimbal Lock for each conversion -- so ideally I'd like to get the Quaternion 'Difference' between the two and decompose it into the relative Pitch and Roll components.
When I try your suggestion -- it yields different results than the Euler math.
I understand that there are number of different rotation sequences to get from a Quaternion to an Euler angle. I've tried over a dozen of them, none of which yields the same answer as taking the difference of the Euler angles from the individual Quaternions.
It feels like I'm missing something obvious -- but I'm just not seeing it.
Thanks!
For two quaternion orientations p and q the quaternion that would take p into the q frame is r = p* ⊗ q.
Then when you multiply:
p ⊗ r, quaternions are associative so:
p ⊗ r = p ⊗ (p* ⊗ q) = (p ⊗ p*) ⊗ q = q.
To get the Euler angles of r, you should use a library. Here's my Python code for it, if it helps:
def quat2eulerXYZ(q):
r = atan2(2*q[0][0]*q[1][0] - 2*q[2][0]*q[3][0], q[0][0]**2 - q[1][0]**2 - q[2][0]**2 + q[3][0]**2)
p = asin(2*q[0][0]*q[2][0] + 2*q[1][0]*q[3][0])
y = atan2(2*q[0][0]*q[3][0] - 2*q[1][0]*q[2][0], q[0][0]**2 + q[1][0]**2 - q[2][0]**2 - q[3][0]**2)
return (r, p, y)
If your AHRS system is drifting, it is probably the magnetometer suffering from interference which may be solved by calibrating for soft/hard iron interference. If your AHRS system is in close proximity or fastened to any ferrous metals, try it without them. If you try this outdoors, clear of ferrous metals, I would be surprised if it did not work.
You shouldn't need to convert to Euler angles once you have a quaternion, unless it is for display purposes. If you elaborate on what you are doing with the Euler angles I may be of more assistance.
Related
I need to calculate the distance between GPS co-ordinates to calculate distance being traveled. I've tried both the Haversine and Vincenty algorithms, which work fine on my desktop PC, but when I use the same code for MSP430 chip using CCS IDE, the IDE is throwing error saying that "program will not fit into available memory".
Is there any other alternative method or code to find the distance between two GPS co-ordinates? which will fit in available memory of MSP430 Microcontroller ?
It's not surprising that you're running out of memory, because the microcontroller you are using, the Texas Instruments MSP430F2274, has only 32kB of flash, and 1kB of RAM.
There are several approaches to solving your problem, each with different tradeoffs. Here are three:
Use another microcontroller that has more memory (there are many in the MSP430 family).
Optimize your code to fit in the available space.
Use a simpler formula than the Vincenty or Haversine.
I'll address the two latter approaches below.
Optimize Your Code
Depending on the accuracy requirements of your application, optimizing your existing code might be a better approach than using a simpler formula than Vincenty or Haversine.
A Simple Way to Optimize
Perhaps simply setting the compiler to optimize for size will solve your problem. In the MSP430 toolset, use the --opt_for_speed=0 switch. According to the MSP430 Optimizing C/C++ Compiler User's Guide (v15.9.0.STS) (page 62), this switch:
enables optimizations geared towards improving the code size with a
high risk of worsening or impacting performance.
So you might very easily get things to work by using this switch, at the cost of trading away speed for memory space.
A More Involved Way to Optimize
Assuming you are using the floating point math library provided with your compiler, you might be able to still use Vincenty or Haversine if you replace the math library with a more space-efficient version. The CORDIC fixed-point algorithms provide iterative approaches to calculating the trigonometric functions, that trade away speed for space efficiency. If you roll your own math library, you might achieve a good balance between space, speed, and accuracy. A 16-bit version of the CORDIC approach for sine() and cosine() for the MSP430 is here; you will need to determine whether it provides the degree of accuracy and precision you need.
Use a Different Formula
In general, the various algorithms that calculate distance between two points on the earth represent a trade-off between accuracy and complexity. The Vincenty algorithm you cited is much more accurate than the Haversine, as it more correctly represents the earth as an oblate spheroid instead of as a sphere of radius R; hence the math is more complex.
For reference, the Haversine method (which assumes the earth is a perfect sphere) is shown here:
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin^2(dlat/2) + cos(lat1) * cos(lat2) * sin^2(dlon/2)
c = 2 * arcsin(min(1,sqrt(a)))
d = R * c
The intermediate result c is the distance in radians. The distance d is in the same units as R (radius of the earth).
As you can see, the Haversine employs an arcsin() in the calculation.
You can simplify the math further by employing the Polar Coordinate Flat-Earth method:
a = pi/2 - lat1
b = pi/2 - lat2
c = sqrt(a^2 + b^2 - 2 * a * b * cos(lon2 - lon1)
d = R * c
Notice that there is no arcsin() in this calculation, but there is a sqrt().
A discussion of the accuracy tradeoffs between the Haversine and the Polar Coordinate Flat-Earth methods is here, question Q5.1.
See also
How do I calculate distance between GPS co-ordinates on a processor with poor floating point support?
http://www.faqs.org/faqs/geography/infosystems-faq/ Question Q5.1
Geographical Distance (Wikipedia)
I require a better way of calculating an S-Curve than the below method. I'm using it to draw an S-Curve in a drawRect method as well as calculating the ease-in/ease-out of the volume of a music file for fading.
The reason I need an improved way of doing this is because it gets called roughly 100 times in a loop to calculate the curve and is highly CPU intensive.
I'm hoping that maybe one or a few vector math functions from the accelerate framework might help but I'm not sure where to start.
3 * position * (1 - position) * (1 - position) * firstControlPoint + 3 *
position * position * (1 - position) * secondControlPoint +
position * position * position * 1.0;
Where firstControlPoint equals 0.0 and secondControlPoint equals 1.0.
You may be interested in this article on Even Faster Bézier, but 100 calculations of this is not a lot. I've run thousands of such calculations per frame on first-generation iPads. For such a tiny set, you're unlikely to get much benefit from Accelerate (and Accelerate can be much slower than simple C for small data sets).
There are several things to consider, though:
If the control points are invariable, you should be able to pre-calculate the values for all positions and stick them in a table. That's going to dramatically improve performance. If they vary less often than you draw, then it's still worth pre-calculating the table whenever the inputs change. Also, make sure that you're not calculating these values more often then you actually need them (if the input values can vary quickly, you may want to wait for the inputs to settle before recalculating anything).
If this is an NEON device (i.e an iPhone or iPad), intrinsics are almost never a win (this may change as Clang gets better, but that was my finding in 2012). Hand-coded NEON can definitely be a win if you really need the performance, but it's a headache to program, so I would look everywhere else first. Assembly programming is radically different from C programming. If you could just drop an intrinsic in at a certain point and make it go faster, the compiler would have done that already (and in fact it does).
If you're doing floating point math, and you just need the results to be "almost exactly correct and perfect for drawing and animations" rather than "exactly correct and reproducible according to IEEE rules," you should turn on fast math. The easiest way to do that is to switch the compiler optimization from "Fastest, Smallest" to "Fastest, Aggressive Optimizations." It's hard to imagine a case when this is not the correct setting for iOS apps, and it's almost always the correct setting for Mac apps. This setting also turns on additional vectorization, which can make a big difference in your loops.
You should definitely look at Optimize Your Code Using LLVM from WWDC 2013. It covers how to structure you code to let the compiler help you the most. You may also want to look at The Accelerate Framework from the same videos, but it's unlikely that Accelerate is the right tool for this problem.
Rather than calculating this yourself, consider using a CAPropertyAnimation with a custom timing function. These can be used to set any value; not just layer animations. For drawing the curve, consider using a UIBezierPath rather than hand-calcuating the curve.
For an example of some of this in practice, you may find the CurvyText example from iOS Pushing the Limits to be useful. It calculates both the Bézier points and their slope to perform text layout along a moving curve.
Your S-curve is a Bezier Curve so you could use the De Casteljau's algorithm.
q0 = t * p0 + (1 - t) * p1
q1 = t * p1 + (1 - t) * p2
q2 = t * p2 + (1 - t) * p3
r0 = t * q0 + (1 - t) * q1
r1 = t * q1 + (1 - t) * q2
s0 = t * r0 + (1 - t) * r1
Then you could use SSE/AVX-intrinsics to compute multiples curves (2 -> 128 bits, 4 -> 256 bits) with a single stream of instructions.
I'm trying to create a solar system simulation, and I'm having problems trying to figure out initial velocity vectors for random objects I've placed into the simulation.
Assume:
- I'm using Gaussian grav constant, so all my units are AU/Solar Masses/Day
- Using x,y,z for coordinates
- One star, which is fixed at 0,0,0. Quasi-random mass is determined for it
- I place a planet, at a random x,y,z coordinate, and its own quasi-random mass determined.
Before I start the nbody loop (using RK4), I would like the initial velocity of the planet to be such that it has a circular orbit around the star. Other placed planets will, of course, pull on it once the simulation starts, but I want to give it the chance to have a stable orbit...
So, in the end, I need to have an initial velocity vector (x,y,z) for the planet that means it would have a circular orbit around the star after 1 timestep.
Help? I've been beating my head against this for weeks and I don't believe I have any reasonable solution yet...
It is quite simple if you assume that the mass of the star M is much bigger than the total mass of all planets sum(m[i]). This simplifies the problem as it allows you to pin the star to the centre of the coordinate system. Also it is much easier to assume that the motion of all planets is coplanar, which further reduces the dimensionality of the problem to 2D.
First determine the magnitude of the circular orbit velocity given the magnitude of the radius vector r[i] (the radius of the orbit). It only depends on the mass of the star, because of the above mentioned assumption: v[i] = sqrt(mu / r[i]), where mu is the standard gravitational parameter of the star, mu = G * M.
Pick a random orbital phase parameter phi[i] by sampling uniformly from [0, 2*pi). Then the initial position of the planet in Cartesian coordinates is:x[i] = r[i] * cos(phi[i]) y[i] = r[i] * sin(phi[i])
With circular orbits the velocity vector is always perpendicular to the radial vector, i.e. its direction is phi[i] +/- pi/2 (+pi/2 for counter-clockwise (CCW) rotation and -pi/2 for clockwise rotation). Let's take CCW rotation as an example. The Cartesian coordinates of the planet's velocity are:vx[i] = v[i] * cos(phi[i] + pi/2) = -v[i] * sin(phi[i])vy[i] = v[i] * sin(phi[i] + pi/2) = v[i] * cos(phi[i])
This easily extends to coplanar 3D motion by adding z[i] = 0 and vz[i] = 0, but it makes no sense, since there are no forces in the Z direction and hence z[i] and vz[i] would forever stay equal to 0 (i.e. you will be solving for a 2D subspace problem of the full 3D space).
With full 3D simulation where each planet moves in a randomly inclined initial orbit, one can work that way:
This step is equal to step 1 from the 2D case.
You need to pick an initial position on the surface of the unit sphere. See here for examples on how to do that in a uniformly random fashion. Then scale the unit sphere coordinates by the magnitude of r[i].
In the 3D case, instead of two possible perpendicular vectors, there is a whole tangential plane where the planet velocity lies. The tangential plane has its normal vector collinear to the radius vector and dot(r[i], v[i]) = 0 = x[i]*vx[i] + y[i]*vy[i] + z[i]*vz[i]. One could pick any vector that is perpendicular to r[i], for example e1[i] = (-y[i], x[i], 0). This results in a null vector at the poles, so there one could pick e1[i] = (0, -z[i], y[i]) instead. Then another perpendicular vector can be found by taking the cross product of r[i] and e1[i]:e2[i] = r[i] x e1[i] = (r[2]*e1[3]-r[3]*e1[2], r[3]*e1[1]-r[1]*e1[3], r[1]*e1[2]-r[2]*e1[1]). Now e1[i] and e2[i] can be normalised by dividing them by their norms:n1[i] = e1[i] / ||e1[i]||n2[i] = e2[i] / ||e2[i]||where ||a|| = sqrt(dot(a, a)) = sqrt(a.x^2 + a.y^2 + a.z^2). Now that you have an orthogonal vector basis in the tangential plane, you can pick one random angle omega in [0, 2*pi) and compute the velocity vector as v[i] = cos(omega) * n1[i] + sin(omega) * n2[i], or as Cartesian components:vx[i] = cos(omega) * n1[i].x + sin(omega) * n2[i].xvy[i] = cos(omega) * n1[i].y + sin(omega) * n2[i].yvz[i] = cos(omega) * n1[i].z + sin(omega) * n2[i].z.
Note that by construction the basis in step 3 depends on the radius vector, but this does not matter since a random direction (omega) is added.
As to the choice of units, in simulation science we always tend to keep things in natural units, i.e. units where all computed quantities are dimensionless and kept in [0, 1] or at least within 1-2 orders of magnitude and so the full resolution of the limited floating-point representation could be used. If you take the star mass to be in units of Solar mass, distances to be in AUs and time to be in years, then for an Earth-like planet at 1 AU around a Sun-like star, the magnitude of the orbital velocity would be 2*pi (AU/yr) and the magnitude of the radius vector would be 1 (AU).
Just let centripetal acceleration equal gravitational acceleration.
m1v2 / r = G m1m2 / r2
v = sqrt( G m2 / r )
Of course the star mass m2 must be much greater than the planet mass m1 or you don't really have a one-body problem.
Units are a pain in the butt when setting up physics problems. I've spent days resolving errors in seconds vs timestep units. Your choice of AU/Solar Masses/Day is utterly insane. Fix that before anything else.
And, keep in mind that computers have inherently limited precision. An nbody simulation accumulates integration error, so after a million or a billion steps you will certainly not have a circle, regardless of the step duration. I don't know much about that math, but I think stable n-body systems keep themselves stable by resonances which absorb minor variations, whether introduced by nearby stars or by the FPU. So the setup might work fine for a stable, 5-body problem but still fail for a 1-body problem.
As Ed suggested, I would use the mks units, rather than some other set of units.
For the initial velocity, I would agree with part of what Ed said, but I would use the vector form of the centripetal acceleration:
m1v2/r r(hat) = G m1 m2 / r2 r(hat)
Set z to 0, and convert from polar coordinates to cartesian coordinates (x,y). Then, you can assign either y or x an initial velocity, and compute what the other variable is to satisfy the circular orbit criteria. This should give you an initial (Vx,Vy) that you can start your nbody problem from. There should also be quite a bit of literature out there on numerical recipes for nbody central force problems.
Firstly, I have done much googling and checking other stackoverflow posts about this, but cannot get a working reply or a snippet of working code. Maths is not my strength.
I need to have a routine that takes a camera point (CX,CY,CZ) and rotate it about a lookat point (LX,LY,LZ) by three rotation angles (RX,RY,RZ). Using euler rotations leads to gimbal lock in some cases which I need to avoid. So I heard about using quaternions.
I found this to convert the rotations into a quaternion
http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
and this to convert from a quaternion back to euler XYZ rotations
http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
They seem to work fine, but I need to know how to use the quaternion to rotate the CX,CY,CZ around LX,LY,LZ and then return the new CX,CY,CZ without issues of gimbal lock.
There is so much out there about this, that I am sure a good explanation and snippet of code will help not only me but many others in the future.
So please help if you can. Many thanks.
The short answer, if your quaternion is Q and the new camera point is C':
C' = Q*(C-L)*Q^-1 + L
where the points are augmented with Cw=0 and multiplication and inverse are according to quaternion rules.
Specifically, let D = C - L. Then we let F = Q*D:
Fw = Qw*0 - Qx*Dx - Qy*Dy - Qz*Dz
Fx = Qw*Dx + Qx*0 + Qy*Dz - Qz*Dy
Fy = Qw*Dy - Qx*Dz + Qy*0 + Qz*Dx
Fz = Qw*Dz + Qx*Dy - Qy*Dx + Qz*0
Finally, we get C' = F*Q^-1 + L:
Cw' = 0
Cx' = Fw*Qx - Fx*Qw + Fy*Qz - Fz*Qy + Lx
Cy' = Fw*Qy - Fx*Qz - Fy*Qw + Fz*Qx + Ly
Cz' = Fw*Qz + Fx*Qy - Fy*Qx - Fz*Qw + Lz
However, be aware that if you're creating the quaternion from an Euler representation, you'll still end up with gimbal lock. The gimbal lock is a property of the Euler representation and the quaternion will just represent the same transformation. To get rid of gimbal lock, you'll need to avoid the Euler representation altogether, unless I misunderstand how you're using it.
I wanted to simulate the following through animation :
A ball starts with a certain velocity at the bottom most point of
a vertical circular loop and keeps rolling in it until its velocity permits.
For this, I wanted to find velocity/x/y vs. time equation.
For e.g. if the ball had mass : 5Kg, radius of the circular loop = 10m,
and initial velocity of the ball is 200 m/s, what will its velocity and (x,y) position
be after 5 seconds?
thanks.
Sliding, frictionless case with a point-particle ball
In this case we aren't worrying about rotational energy and are assuming that the ball is actually a point particle. Then, in order for the ball to stay on at the top, the centripetal force condition has to be satisfied:
m * v_top^2 / r = m * g
so
v_top = sqrt(r * g)
So the minimum initial velocity is determined by:
1 / 2 * m * v0^2 >= 1 / 2 * m * v_top^2 + m * g * 2 * r
v0 >= sqrt(5 * r * g)
This is similar to what Pete said, except that he forgot the centripetal force condition to stay on at the top.
Next, the acceleration tangential to the track is given by:
a = - g * sin(theta)
but a = r * alpha = r * d^2(theta)/dt^2 where alpha is the rotational acceleration. Thus, we get
r * d^2(theta)/dt^2 = g * sin(theta)
However, I don't know of an analytical solution to this differential equation and Mathematica was stumbling with finding one too. You can't just move the dts to the other side and integrate because theta is a function of t. I would recommend solving it by numerical means such as a Runga-Kutte or maybe the Verlet method. I solved it using Mathematica for the parameters you gave, but with the ball moving so quickly, it doesn't really slow down much in going around. When I lowered the initial velocity though, I was able to see the speeding up and slowing down by plotting theta as a function of time.
Adding in other things like a finite ball radius, rotational energy and friction are certainly doable, but I would worry about being able to solve this first case before moving on because it only gets more complicated from here. By the way, with the friction you will have to choose some kinetic coefficient of friction for your given materials which will of course be proportional to the normal force exerted on the ball by the track which can be solved for by summing the force components along the radius of the circle and don't forget to include the centripetal force condition.
If you haven't done this sort of physics before, I definitely recommend getting a introductory good book on physics (with calculus) and working through it. You only need to bother with the sections that apply to mechanics though that is a very large section of the book probably. There might be better routes to pursue though like some of the resources in this question.
If there are no acceleration (x,y) =(xstart+ vx*time ,ystart + vy*time) and speed remain the same, and it is not related to the radius
Since the velocity is constant you will have an angular velocity of omega = vel / radius. You will obtain how many radians you ball will move per second over its circular path.
To get the position at time t you just have to exploit polar coordinates:
x = x_center + sin( 3/2*PI + omega*t)*radius
y = y_center + cos( 3/2*PI + omega*t)*radius
This because you start from bottom point of the circle (so its 3/2*PI) plus how many radiants you move every second (we obtained it from tangential velocity). All multiplied for the radius, otherwise you will consider a unity circle.
EDIT: Since you wonder how to find a position of an object that is subject to many different forces I can tell you that usually a physical engine doesn't care about finding equations of moving objects. It just applies forces to objects considering their intended motions (like your circular one) or environmental factors (like gravity or friction) and calculates coordinates step by step by applying forces and using an integrator to see the results.
Ignoring friction, the forces on the ball are gravity and the track.
First, there are two main cases - is the velocity enough for the ball to loop-the-loop or not:
initial energy = 1/2 m v² = 0.5 * 5 * 200 * 200
potential energy = m g h = 5 * 9.8 * 20
so it will go round the whole loop.
Initially the ball is at the bottom of the loop, theta = 0
The acceleration on the ball is the component of g along the track
a = g⋅sin theta
The distance travelled is theta * radius. It is also the double integral of acceleration against time.
theta ⋅ radius = double integral of acceleration against time
Integrating acceleration once gives velocity, integrating velocity gives distance.
so solve this for t:
theta ⋅ r = ∫(∫ g⋅sin theta.dt).dt
then your x and y are trivial functions of theta.
Whether you solve it analytically or numerically is up to you.
With dynamic friction, friction is usually proportional to the normal force on the bodies. So this will equal the centripetal force - proportional to the square of the angular velocity, and the component of gravity normal to the track (g sin theta)
You didn't tell anything about how you want your velocity to change. Do you have any friction model? If there is no friction, then the formulas are simple:
length = velocity*t
x = sin(length)*radius
y = -cos(length)*radius
If the velocity is changing, then you have to change length to something like
length = integral over dt[0..t] (velocity dt)
The only thing I wanted to add is the if this is real ball (sphere) with mass 5kg then it must have a diameter dia=(6*m/(PI*rho))^(1/3) where rho is the density of the material. For steel (rho=7680) the diameter is dia=0.1075 meters. Therefore the pitch radius (radius at which the center of gravity of the ball rides on) is equal to R=10-(dia/2) or R=9.9466 meters.
The problem gets a little more complex when friction is included. For one you have to consider the direction of friction (assuming dry friction theory). That depends on the amount the ball rotates in its axis and that depends on moment of inertia of the ball.
When you do the simulation you might want to monitor the total kinetic energy + the total potential energy and make sure your are not adding energy to the system (or taking away). [Don't forget to include the rotational component for the kinetic energy]
Get a standard book on dynamics, and I am sure a similar problem is already described in the book.I would recommend "Vector Mechanic for Engineers - Dynamics".