Related
So I have an array of N 1D-signals (e.g. time series) with same number of samples per signal (all in equal resolution) and I want to define a differentiable loss function to penalize asynchrony among them and therefore be zero if all N 1D signals will be equal to each other. I've been searching the literature to find something but haven't had luck yet.
Few remarks:
1 - since N (number of signals) could be quite large I can not afford to calculate Mean squared loss between every single pair which could grow combinatorialy large. also I'm not quite sure whether it would be optimal in any mathematical sense for the goal to achieve.
There are two naive loss functions that I could think of :
a) Total variation loss for each time sample across all signals (to force to reach ideally zero variation). the problem is here the weight needs to be very large to yield zero varion. masking any other loss term that is going to be added and also there is no inherent order among the N signals, which doesnt make it suitable to TV loss to begin with.
b) minimizing the sum of variance at each time point among all signals. however, choice of the reference of variance (aka mean) could be crucial I believe as just using the sample mean might not really yield the desired result, not quite sure.
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.
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.
I have a server and a client.
I have 40 opengl cubes. There state is described by 3d vector for position and 3x3 rotation matrix(or a quaternion).
How can I send intermediate packets and predict the object state on the client between those packets(extrapolation)?
for object position I can use a linear predictor on velocity.
How to predict quaternion states?
The easiest thing, parallel to what you're doing with linear velocity, is to use a linear predictor on angular velocity.
If you have two quaternions, q_0 and q_t, representing global orientations that are t seconds apart, you can compute the finite difference between the two quaternions and use that to find an angular velocity that can be used for extrapolation.
Make sure that the inner-product between q_0 and q_t is non-negative. If it's negative, negate all the components of one of the quaternions. This makes sure that we're not trying to go the long way around. If your bodies are rotating really fast relative to your sampling, this is a problem and you'll need a more complicated model that accounts for the previous angular velocity and makes assumptions about maximum possible acceleration. We'll assume that's not the case.
Then we compute the relative difference quaternion. dq = q_t * q_0' (where q_0' is the quaternion rotational inverse/conjugate). If you have the luxury of having fixed-sized steps, you can stop here and predict then next orientation t seconds into the future: q_2t = dq*d_t.
If we can' step forward by integer multiples of t, we compute the angle of rotation from dq. Quaternions and angular velocities are both variations on "axis-angle" representations of changes in orientation. If you rotate by Θ around unit-length axis [x,y,z], then the quaternion representation of that is q = [cos(Θ/2), sin(Θ/2)x, sin(Θ/2)y, sin(Θ/2)z] (using the quaternion convention where the w component comes first). If you rotate by Θ/t around axis [x,y,z], then the angular velocity is v = [Θx,Θy,Θz]/t. So v = Θ[q.x,q.y,q.z]/(t||[q.x,q.y,q.z]||). We can compute the angle two ways: Θ = 2acos(q.w) = 2asin(||[q.x,q.y,q.z]||). These will always be the same because of step 1. Numerics make it nicer to use sine since we need to find m = ||[q.x,q.y,q.z]|| anyway for the next step.
If m is large enough, then we just find the angular velocity:
v = 2asin(m)[dq.x,dq.y,dq.z]/(m*t)
However, if m's not large enough, we'll face numeric issues trying to divide by near-zero. So programmers will use the Taylor expansion of the sinc() function around zero, which happens to be very accurate in this case. Remember that m = sin(Θ/2). With m<1e-4, we can accurately compute asin(m)/m = 6/(6-m*m). Then you just need to multiply the result by 2*[dq.x,dq.y,dq.z]/t and you have your angular velocity. Phew.
Extrapolating is then a matter of multiplying your angular velocity times the time that has passed. Then you go backwards, converting the angular change to a quaternion and multiplying it onto q_t.
It seems like there must be an easier way...
I've tried the typical physics equations for this but none of them really work because the equations deal with constant acceleration and mine will need to change to work correctly. Basically I have a car that can be going at a large range of speeds and needs to slow down and stop over a given distance and time as it reaches the end of its path.
So, I have:
V0, or the current speed
Vf, or the speed I want to reach (typically 0)
t, or the amount of time I want to take to reach the end of my path
d, or the distance I want to go as I change from V0 to Vf
I want to calculate
a, or the acceleration needed to go from V0 to Vf
The reason this becomes a programming-specific question is because a needs to be recalculated every single timestep as the car keeps stopping. So, V0 constantly is changed to be V0 from last timestep plus the a that was calculated last timestep. So essentially it will start stopping slowly then will eventually stop more abruptly, sort of like a car in real life.
EDITS:
All right, thanks for the great responses. A lot of what I needed was just some help thinking about this. Let me be more specific now that I've got some more ideas from you all:
I have a car c that is 64 pixels from its destination, so d=64. It is driving at 2 pixels per timestep, where a timestep is 1/60 of a second. I want to find the acceleration a that will bring it to a speed of 0.2 pixels per timestep by the time it has traveled d.
d = 64 //distance
V0 = 2 //initial velocity (in ppt)
Vf = 0.2 //final velocity (in ppt)
Also because this happens in a game loop, a variable delta is passed through to each action, which is the multiple of 1/60s that the last timestep took. In other words, if it took 1/60s, then delta is 1.0, if it took 1/30s, then delta is 0.5. Before acceleration is actually applied, it is multiplied by this delta value. Similarly, before the car moves again its velocity is multiplied by the delta value. This is pretty standard stuff, but it might be what is causing problems with my calculations.
Linear acceleration a for a distance d going from a starting speed Vi to a final speed Vf:
a = (Vf*Vf - Vi*Vi)/(2 * d)
EDIT:
After your edit, let me try and gauge what you need...
If you take this formula and insert your numbers, you get a constant acceleration of -0,0309375. Now, let's keep calling this result 'a'.
What you need between timestamps (frames?) is not actually the acceleration, but new location of the vehicle, right? So you use the following formula:
Sd = Vi * t + 0.5 * t * t * a
where Sd is the current distance from the start position at current frame/moment/sum_of_deltas, Vi is the starting speed, and t is the time since the start.
With this, your decceleration is constant, but even if it is linear, your speed will accomodate to your constraints.
If you want a non-linear decceleration, you could find some non-linear interpolation method, and interpolate not acceleration, but simply position between two points.
location = non_linear_function(time);
The four constraints you give are one too many for a linear system (one with constant acceleration), where any three of the variables would suffice to compute the acceleration and thereby determine the fourth variables. However, the system is way under-specified for a completely general nonlinear system -- there may be uncountably infinite ways to change acceleration over time while satisfying all the constraints as given. Can you perhaps specify better along what kind of curve acceleration should change over time?
Using 0 index to mean "at the start", 1 to mean "at the end", and D for Delta to mean "variation", given a linearly changing acceleration
a(t) = a0 + t * (a1-a0)/Dt
where a0 and a1 are the two parameters we want to compute to satisfy all the various constraints, I compute (if there's been no misstep, as I did it all by hand):
DV = Dt * (a0+a1)/2
Ds = Dt * (V0 + ((a1-a0)/6 + a0/2) * Dt)
Given DV, Dt and Ds are all given, this leaves 2 linear equations in the unknowns a0 and a1 so you can solve for these (but I'm leaving things in this form to make it easier to double check on my derivations!!!).
If you're applying the proper formulas at every step to compute changes in space and velocity, it should make no difference whether you compute a0 and a1 once and for all or recompute them at every step based on the remaining Dt, Ds and DV.
If you're trying to simulate a time-dependent acceleration in your equations, it just means that you should assume that. You have to integrate F = ma along with the acceleration equations, that's all. If acceleration isn't constant, you just have to solve a system of equations instead of just one.
So now it's really three vector equations that you have to integrate simultaneously: one for each component of displacement, velocity, and acceleration, or nine equations in total. The force as a function of time will be an input for your problem.
If you're assuming 1D motion you're down to three simultaneous equations. The ones for velocity and displacement are both pretty easy.
In real life, a car's stopping ability depends on the pressure on the brake pedal, any engine braking that's going on, surface conditions, and such: also, there's that "grab" at the end when the car really stops. Modeling that is complicated, and you're unlikely to find good answers on a programming website. Find some automotive engineers.
Aside from that, I don't know what you're asking for. Are you trying to determine a braking schedule? As in there's a certain amount of deceleration while coasting, and then applying the brake? In real driving, the time is not usually considered in these maneuvers, but rather the distance.
As far as I can tell, your problem is that you aren't asking for anything specific, which suggests that you really haven't figured out what you actually want. If you'd provide a sample use for this, we could probably help you. As it is, you've provided the bare bones of a problem that is either overdetermined or way underconstrained, and there's really nothing we can do with that.
if you need to go from 10m/s to 0m/s in 1m with linear acceleration you need 2 equations.
first find the time (t) it takes to stop.
v0 = initial velocity
vf = final velocity
x0 = initial displacement
xf = final displacement
a = constant linear acceleration
(xf-x0)=.5*(v0-vf)*t
t=2*(xf-x0)/(v0-vf)
t=2*(1m-0m)/(10m/s-0m/s)
t=.2seconds
next to calculate the linear acceleration between x0 & xf
(xf-x0)=(v0-vf)*t+.5*a*t^2
(1m-0m)=(10m/s-0m/s)*(.2s)+.5*a*((.2s)^2)
1m=(10m/s)*(.2s)+.5*a*(.04s^2)
1m=2m+a*(.02s^2)
-1m=a*(.02s^2)
a=-1m/(.02s^2)
a=-50m/s^2
in terms of gravity (g's)
a=(-50m/s^2)/(9.8m/s^2)
a=5.1g over the .2 seconds from 0m to 10m
Problem is either overconstrained or underconstrained (a is not constant? is there a maximum a?) or ambiguous.
Simplest formula would be a=(Vf-V0)/t
Edit: if time is not constrained, and distance s is constrained, and acceleration is constant, then the relevant formulae are s = (Vf+V0)/2 * t, t=(Vf-V0)/a which simplifies to a = (Vf2 - V02) / (2s).