I'm trying to program an arduino to generate a Trapezoidal Motion Profile to control a DC motor with a quadrature encoder.
Essentially, the user will input the desired Target Position, Max Velocity and Acceleration (decel = -accel) and the code will calculate the target position versus time which will then be compared with the actual position. The result will then be subject to a PID calculation
My initial assumption was that I could use basic Newtonian physics to determine position (i.e. PT = P0 + V0T + 1/2AT2, VT = V0 + AT). However, after reading through documentation for pre-existing motion controllers, I discovered that the prevalent method was to use a discrete time method, which is as follows:
VK = VK-1 + A (A = Acceleration)
PK = PK-1 + VK-1 + A/2
I'm having a hard time understanding quite how this equation would generate the target position versus time. In the case of Velocity, it seems to just add the acceleration to the current velocity. But what about everything in between?
Could anybody take a shot at explaining to me how this method is used? I've spent ages searching for answers online but have had no such luck.
Related
I am currently working on a project (for fun) for simulating Newtonian gravity and want to be able to visualize the future path of an object orbiting around a single attractor. At any given simulation step I know
The object's current position relative to the center of the attractor as a 2D Vector
The object's current velocity as a 2D Vector
The mass of the attractor
The mass of the object
Using these parameters I would like to be able to predict up to N steps ahead in the simulation so that I can draw a line between each point along the orbit.
The Position and Velocity in my simulation are measured in m and m/s while the mass is unitless. The Gravitational Constants is set to 1 and the masses for each object is 1000, for the attractor, and 10 for the object.
The simulation itself produces the result that I want, now I just want to be able to predict the future path.
During my own research, I have found that I need to use the Keplerian Elements in some way. The various examples, questions etc... I have found on stackexchange and elsewhere do not provide sufficient explanation for me to be able to work it into my simulation or they are specific for 3-dimensional geometry and provide incorrect outputs when I attempt to calculate them (NaN and Infinities).
For example, in a few places I have read that the semi-major axis and the total orbital energy should remain constant and from these I can get the eccentricity and various other properties, but when calculating them at any given step using the above-mentioned properties I have access to, they vary wildly or produce numbers that are so ridiculously high or low that they are essentially useless. I resolved the last problem and have a semi-major axis value that makes sense in the context of my simulation, but again it varies from step to step.
As an example:
Initial conditions
Attractor
Mass = 1000
Position = 0,0
Object
Mass = 10
Position = 5, 0
Velocity = 0, 5
This produces an elliptical orbit. The semi-major axis should remain constant as no other forces are acting on the object, however, the value of the semi-major axis varies from 2-4 meters.
I have figured it out and to any future visitors to this question I am (hopefully) going to make your life much, much easier.
If you are like me, fairly lay in terms of astrodynamics then the sheer number of equations, their very technical explanations, and usage is a little daunting. With a little bit of effort, I now know how to iterate equations to be able to calculate future orbital positions.
What we will need are the velocity and positional coordinates of the object you want to predict as well as the mass of the orbital body and the mass of the object itself.
These need to be in m and m/s for no other reason than for this implementation they are assumed to be m and m/s but the mass can be unitless (at first).
Terms:
Attractor - The nearest massive body that is attracting the Object.
Object - The orbital body that is being attracted to the Attractor.
Step 1: Calculate the True Mass of your objects
True mass is defined as the KG mass of your Attractor and Object. To find this value you need to convert your unitless mass, using the gravitational constant, into kilograms.
If you don't know how to do this, first calculate the ratio of unitless mass to the gravitational constant in your simulation and use the value of that ratio to find true mass using the real gravitational constant.
(I cannot post images or else I would include the equations)
After this point when I refer to the gravitational constant, I will be using our real universe's value.
Step 2: Calculate μ
μ is a ratio of masses and the gravitational constant. If the mass of the attractor is large enough μ = GM, where G is the gravitational constant and M is the mass of the attractor. For my own implementation I found that μ can be calculated as:
μ = G ( M * M * M) / ((M+m)*(M+m))
Where:
M is the mass of the Attractor
m is the mass of the Object
G is the gravitational constant.
It is very very important that this value is correct. If what you are rendering looks weird, it is probably because this value was not calculated correctly and it would be the first thing I check. In my initial attempts, this was off by an order of magnitude, it should have been around 100 but it was outputting as 1000 because the mass unit conversion value I calculated was 10x larger than it should have been.
Step 3: Calculate the Orbital Elements
First up is the Semi-Major Axis. This is the radius from the furthest point in the orbit to the center (Aprox). It is represented with the symbol a and calculated as:
a = -(μ * len(pos)) / (len(pos) * len(vel) * len(vel) - (2 * μ))
Man I wish I could post images so these equations looked better, sorry!
Next up is the period of the orbit. This is used later so that we can step through the orbit incrementally. It is represented by the symbol T and its formula is:
T = 2 * π * sqrt((a * a * a) / μ)
Next, we need angular momentum. Now, I know that the post says 2D but in my own code, I am using Vector3's to store my information in case I decide to make it 3D later. This is important because normally to get the angular momentum you would take the cross product of the velocity and position. Since we are in 2D there is no true cross product for vectors. Instead, you can calculate the momentum as:
p = pos.x * vel.y - pos.y * vel.x
Now onto orbital energy, represented by the symbol ε. We use this to find the eccentricity of the orbit.
ε = (len(vel) * len(vel)) / 2 - (μ / len(pos))
After that we can get eccentricity, represented with the symbol e:
e = sqrt(1 + ((2 * ε * p * p) / (μ * μ)))
The second to last thing we need to calculate is the eccentricity vector, represented with ev:
ev = ((((len(vel) * len(vel)) / μ) - (1 / len(pos))) * pos) - ((dot(pos, vel) / μ) * vel)
I know, there are a lot of brackets but I want to make it clear what goes where. Blame StackOverflow for not letting me post equation images.
The final value is the argument of periapsis. Because we are in 2D we need to use this formula to calculate it:
w = atan2(ev.y, ev.x)
we will use this to orient our orbital path correctly.
Step 4: Keplers Equation
Now is where we get to predict the orbital positions using all the information we calculated above.
First, we need n. n is the radians / second that the Object is traveling around the Attractor. It is fairly straightforward to calculate, it is 2π/T where T is the period. This value is used when we calculate the mean anomaly in the predictive step.
Now we get into the loop. You can set the number of steps you want to integrate over to whatever you want, the higher the number the closer each point along the orbit will be. In my case, I have a step of 100, so in my for loop declaration, I will loop 100 times before exiting. In addition, you will want to define a max tries value because we need to do an integration (don't worry, not as scary as it sounds) to be able to get the correct Eccentric Anomaly.
Now that we are in the loop, we need to calculate the current mean anomaly:
ma = n * (T * (i / steps))
Where:
T is the period of the orbit in seconds
i is the current iterative step
n is speed from earlier (radians/second)
Now we do the integration to solve Kepler's Equation. This can be done using Newton's method. Basically, we take Kepler's Equation and its derivative and iterate over them until the delta value between the two of them is arbitrarily low.
This is why we need max tries, it is possible that this while loop iterates forever and never converges on a value in which case we need to bail out so we don't get stuck.
//Eccentric Anomaly
var ea = ma;
while(tries < maxTries)
{
tries++;
//Keplers equation
var dx = ea - e * sin(ea) - ma;
//Derivative of Keplers equation
var dy = 1 - e * cos(ea);
var dt = dx / dy;
ea -= dt;
if(abs(dt) < 10^-8) break;
}
See, the integration wasn't so hard. This loop is an implementation of Newton's method and will oscillate toward to correct value. It should be noted that his method is quick and dirty and produces okay results. But this is the only method I understand so this is why I used it.
Step 5: Calculating Orbital Position
Next up is where we calculate the orbital position. This is simple and I stole it directly from this post.
p = a * (cos(ea) - e)
q = a * sin(ea) * sqrt(1 - (e * e))
You can plot these two values as X and Y and you will get an orbit that looks correct in some cases, but we need to rotate it using the argument of periapsis we calculated earlier. Again, stolen from this post.
x = cos(w) * p - sin(w) * q
y = sin(w) * p - cos(w) * q
And that's it. You're done. You have the X and Y coordinate at the current step and you can plot it on the screen however you like. As the for loop continues to iterate, you will get the full orbital path from start to finish.
I hope this essay helps someone out there!
All the equations I used can be found on Wikipedia just by going to this page and navigating to the corresponding element.
I have the time evolution (unevenly spaced) of around 1000 quaternions which provides the transformation from an inertial coordinate system to a body fixed. My goal is to obtain the angular velocity of the body, so for that I need the derivative of the quaternion (or at least I think so), and I am having trouble to obtain it.
So far I have tried first to use the gradient function
time_grad = gradient(time);
dquat(1,:) = gradient(quat(1,:)) ./ (time_grad);
dquat(2,:) = gradient(quat(2,:)) ./ (time_grad);
dquat(3,:) = gradient(quat(3,:)) ./ (time_grad);
dquat(4,:) = gradient(quat(4,:)) ./ (time_grad);
I have also tried using a Fornberg algorithm I found online, but I think I am not obtaining good results based on the final outcome (some computations using the angular velocity which should result in something very similar to some information I already have).
These quaternions could be easily converted to Direction Cosine Matrices if you know a better method to calculate angular velocity based on DCMs.
What is the fitness function used to solve an inverted pendulum ?
I am evolving neural networks with genetic algorithm. And I don't know how to evaluate each individual.
I tried minimize the angle of pendulum and maximize distance traveled at the end of evaluation time (10 s), but this won't work.
inputs for neural network are: cart velocity, cart position, pendulum angular velocity and pendulum angle at time (t). The output is the force applied at time (t+1)
thanks in advance.
I found this paper which lists their objective function as being:
Defined as:
where "Xmax = 1.0, thetaMax = pi/6, _X'max = 1.0, theta'Max =
3.0, N is the number of iteration steps, T = 0.02 * TS and Wk are selected positive weights." (Using specific values for angles, velocities, and positions from the paper, however, you will want to use your own values depending on the boundary conditions of your pendulum).
The paper also states "The first and second terms determine the accumulated sum of
normalised absolute deviations of X1 and X3 from zero and the third term when minimised, maximises the survival time."
That should be more than enough to get started with, but i HIGHLY recommend you read the whole paper. Its a great read and i found it quite educational.
You can make your own fitness function, but i think the idea of using a position, velocity, angle, and the rate of change of the angle the pendulum is a good idea for the fitness function. You can, however, choose to use those variables in very different ways than the way the author of the paper chose to model their function.
It wouldn't hurt to read up on harmonic oscillators either. They take the general form:
mx" + Bx' -kx = Acos(w*t)
(where B, or A may be 0 depending on whether or not the oscillator is damped/undamped or driven/undriven respectively).
I am trying to program a 3 body problem using matlab. I was given the formula for the moon's trajectory in its rotational frame in space. It's basically the ydotdot, xdotdot=GM/(x^2+y^2)^3/2 formula. What the formula is, is not that important.
THe problem I am facing is that, I am supposed to code up a program that will numerically solve the moon's trajectory equation. I'm using ODE45 to compare with since my goal is to get the same results as ODE45. My ultimate problem is that, I want to iterate through time in terms of days so tspan= [0 365]. The thins is when I convert Gravitational constant to seconds and then do tspace= [0 365] I get a completely different result then If I were to do [0 365*3600*34] representing the seconds in a year and G= 6.67e-11. It seems that my units are very weird.
I was wondering if anyone can explain why this is happening when I use ODE 45. Why can't I convert seconds to days clearly using ODE45? Is there an extra step I have to do? The only other variables in my problem is radius, distance, and the mass of the 3 bodies.
Thank you so much. I've been working on this for a very very long time. Any help would be much appreciated.
That formula for gravitational acceleration along each axis isn't correct.
Put the earth, with mass M_e, at the origin, with the moon (mass M_m) at (x,y). Then
the earth-moon distance is given by:
R_em = sqrt(x^2 + y^2)
The total earth-moon force is given by:
F_em = G*M_e*M_m/R_em^2
The total acceleration due to Earth's gravity is given by:
a_em = F_em/M_m = G*M_e/R_em^2
and is directed toward the origin. The acceleration along
each axis is then:
xdotdot = -F_em*cos(theta) = -F_em*x/R_em = -G*M_e*x/R_em^(3/2)
ydotdot = -F_em*sin(theta) = -F_em*y/R_em = -G*M_e*y/R_em^(3/2)
Note the x and y factors, which are missing from the formula you stated.
I'm not sure what you mean by "converting the gravitational constant to seconds".
The value you're using for G has units of newton-meter^2/kg^2. So it's already
expressed in the MKS (meter-kilogram-second) system, and the accelerations calculated
using this value will have units of meters/sec^2.
With a third body (say, the sun) at (x_s, y_s), you compute a new R_s representing
the moon-sun distance, and compute new acceleration vectors as above, using the
sun's mass M_s (except the acceleration is now in the direction of (x_s, y_s), rather than (0,0)). The accelerations of the moon from the gravity of the earth and the sun just add component-wise, once everything is put into a common coordinate system (here,
geocentric coordinates -- although heliocentric might be a more convenient choice, if you're simulating the sun-earth-moon system). That, plus the initial positions and velocities, should be all you need to compute the positions and velocities at the next time step.
I am rewriting a Monte Carlo simulation model in MATLAB with an emphasis on readability. The model involves many particles, represented as (x,y,z), following a random walk over a small set of states with certain termination probabilities. The information relevant for output is the number of particles that terminate in a given state.
The simulation requires enough particles that running it for each particle individually is cost prohibitive. Vectorization seems to be the way to get performance out of MATLAB, but is there any idiomatic way of creating a vectorized version of this simulation in MATLAB?
I'm beating my head against the wall to accomplish this - I've even tried creating a (nStates x nParticles) matrix representing each particle-state combination, but this approach quickly spirals out of control in terms of readability since particles bounce from state to state independently of one another. Should I just bite the bullet and switch to a language more suitable for this?
Just write the code as you normally would. Almost all matlab functions can accept and return vectorized input. For instance, to simulate a brownian motion of N particles in 1 dimension
position = zeros([N 1]); %start at origin
sigma = sqrt(D * dt); %D is diffusion coefficient, dt is time step
for j = 1:numSteps
position = position + sigma*randn(size(position));
end
if you wanted to have a different sigma for each position, you would make sigma a vector the same size as position and use "dot times" notation to indicate element by element operation
position = position + sigma.*randn(size(position));
if the scattering was an arbitrary function of position and some random element, you would just have to write a vectorized function, e.g.
function newstep = step(position)
%diffusion in a overdamped harmonic potential
newstep = -dt*k*position + D*randn(size(position));
for j = 1:numsteps; position = position + step(position);
and so on