I'm trying to figure out an algorithm for setting up a rendezvous between two spaceships.
There is no gravity or drag. Both spaceships have a position and a velocity at the start. Spaceship B continues on its course with no acceleration, so spaceship A needs to accelerate to close the distance between them and then match velocities when it arrives at the position of spaceship B.
The spaceship can instantly change its direction of thrust, but can only use maximum acceleration or no acceleration at all. I also want a limit on the velocity difference between the spaceships during the maneuver.
I would like the output to be in the form of a number of trajectory legs, i.e: leg1: accelerate direction x for t1 seconds, leg2: coast for t2 seconds, leg3: accelerate direction y for t3 seconds.
I don't need an optimal solution, but I would like it to "look right".
I tried to make an impulse to equalize the velocities and add it to an impulse for moving towards spaceship B, but even though spaceship A ends up with the correct velocity, it fails to reach the targets position. I've tried the impulses by themselves and they seem to perform as expected, so I'm guessing it's the way I'm adding them together that is the problem. I don't know if I am implementing it incorrectly or if this approach simply won't work. I'm hoping someone with stronger math and physics skills can enlighten me.
Here is the code I am using:
// var velocityAdjustmentTime = (int)Math.Sqrt(2 * velocityDelta.Length / tp.Acceleration);
var velocityAdjustmentTime = (int)(velocityDelta.Length / tp.Acceleration);
var velocityAdjustVector = velocityDelta;
velocityAdjustVector.Normalize();
velocityAdjustVector *= tp.Acceleration;
var targetAccelerationDisplacement = new Vector3D(0, 0, 0); // TODO: Replace this with proper values.
Vector3D newPosition;
Vector3D newVelocity;
Vector3D targetNewPosition;
// Check if the formation and the target already have a paralell course with the same velocity.
if (velocityAdjustmentTime > 0)
{
// If not, calculate the position and velocity after the velocity has been aligned.
newPosition = tp.StartPosition + (tp.StartVelocity * velocityAdjustmentTime) + ((velocityAdjustVector * velocityAdjustmentTime * velocityAdjustmentTime) / 2);
newVelocity = tp.StartVelocity + velocityAdjustVector * velocityAdjustmentTime;
targetNewPosition = tp.TargetStartPosition + (tp.TargetStartVelocity * velocityAdjustmentTime) + targetAccelerationDisplacement;
}
else
{
// Else, new and old is the same.
newPosition = tp.StartPosition;
newVelocity = tp.StartVelocity;
targetNewPosition = tp.TargetStartPosition;
}
// Get the new direction from the position after velocity change.
var newDirection = targetNewPosition - newPosition;
// Changing this value moves the end position closer to the target. Thought it would be newdirection length, but then it doesn't reach the target.
var length = newDirection.Length;
// I don't think this value matters.
var speed = (int)(cruiseSpeed);
var legTimes = CalculateAccIdleDecLegs(tp.Acceleration, length, speed);
// Sets how much of the velocity change happens on the acceleration or deceleration legs.
var velFactorAcc = 1;
var velFactorDec = 1 - velFactorAcc;
// Make the acceleration vector.
accelerationVector = newDirection;
accelerationVector.Normalize();
accelerationVector *= legTimes[0] * tp.Acceleration;
accelerationVector += velocityDelta * velFactorAcc;
accelerationTime = (int)(accelerationVector.Length / tp.Acceleration);
accelerationVector.Normalize();
accelerationVector *= tp.Acceleration;
// Make the acceleration order.
accelerationLeg.Acceleration = accelerationVector;
accelerationLeg.Duration = accelerationTime;
// Make the deceleration vector.
decelerationVector = newDirection;
decelerationVector.Negate();
decelerationVector.Normalize();
decelerationVector *= legTimes[2] * tp.Acceleration;
decelerationVector += velocityDelta * velFactorDec;
decelerationTime = (int)(decelerationVector.Length / tp.Acceleration);
decelerationVector.Normalize();
decelerationVector *= tp.Acceleration;
// And deceleration order.
decelerationLeg.Acceleration = decelerationVector;
decelerationLeg.Duration = decelerationTime;
// Add the orders to the list.
trajectory.Add(accelerationLeg);
// Check if there is an idle leg in the middle...
if (legTimes[1] > 0)
{
// ... if so, make the order and add it to the list.
idleLeg.Duration = legTimes[1];
trajectory.Add(idleLeg);
}
// Add the deceleration order.
trajectory.Add(decelerationLeg);
And the function for calculating the approach legs:
private static int[] CalculateAccIdleDecLegs(double acceleration, double distance, int cruiseSpeed)
{
int[] legDurations = new int[3];
int accelerationTime;
int idleTime;
int decelerationTime;
// Calculate the max speed it's possible to accelerate before deceleration needs to begin.
var topSpeed = Math.Sqrt(acceleration * distance);
// If the cruise speed is higher than or equal to the possible top speed, the formation should accelerate to top speed and then decelerate.
if (cruiseSpeed >= topSpeed)
{
// Get the time to accelerate to the max velocity.
accelerationTime = (int)((topSpeed) / acceleration);
// Idle time is zero.
idleTime = 0;
// Get the deceleration time.
decelerationTime = (int)(topSpeed / acceleration);
}
// Else, the formation should accelerate to max velocity and then coast until it starts decelerating.
else
{
// Find the acceleration time.
accelerationTime = (int)((cruiseSpeed) / acceleration);
// Get the deceleration time.
decelerationTime = (int)(cruiseSpeed / acceleration);
// Calculate the distance traveled while accelerating.
var accelerationDistance = 0.5 * acceleration * accelerationTime * accelerationTime;
// Calculate the distance traveled while decelerating.
var decelerationDistance = 0.5 * acceleration * decelerationTime * decelerationTime;
// Add them together.
var thrustDistance = accelerationDistance + decelerationDistance;
// Find the idle distance.
var idleDistance = distance - thrustDistance;
// And the time to idle.
idleTime = (int)(idleDistance / cruiseSpeed);
}
legDurations[0] = accelerationTime;
legDurations[1] = idleTime;
legDurations[2] = decelerationTime;
return legDurations;
}
NEW VERSION:
Assume you have the initial positions and velocities xA0, vA0 and xB0, vB0 of spaceships A and B respectively. As you said, B moves with no acceleration and with constant velocity vB0. Therefore, it travels uniformly along a straight line. Its motion is described as: xB = xB0 + t*vB0. Spaceship A can turn on and off an acceleration of constant magnitude a0 but can change its direction as it sees fit. The velocity of A should not exceed certain value v_max > 0.
Since spaceship B travels uniformly, along a straight line with constant velocity vB0, it actually defines an inertial coordinate system. In other words, if we translate the original coordinate system and attach it to B, the new system travels with constant velocity along a straight line and is therefore also inertial. The transformation is Galilean, so one can define the following change of coordinates (in both directions)
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
in particular, for B for any moment of time t we get
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
At time t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
We are aiming to design the control in this new coordinate system and them move it back into the original one. So let us switch coordinates:
y = x - xB
u = v - vB0
So in this new inertial coordinate system we are solving a problem of control theory and to engineer a good control, we would use as a Lyapunov function (a function that allows us to guarantee certain stable behavior and design the proper expression for the acceleration a) the magnitude of the velocity squared L = norm(u)^2. We want to design acceleration a so that the Lyapunov function in the initial phase of the motion monotonically and steadily decreases while the velocity reorients appropriately.
Define the unit vector
L_unit = cross(x0A - xB0, v0A - vB0) / norm(cross(x0A - xB0, v0A - vB0))
Let in the coordinate system attached to B the motion of A satisfy the system of ordinary differential equations (these equations in both systems are Newton's, because both systems are inertial):
dy/dt = u
du/dt = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
In other words, the acceleration is set to
a = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
Observe that by design norm(a) = a0. Because the vectors u and cross(L_unit, u) are orthogonal and of equal magnitude (simply cross(L_unit, u) is the ninety degree rotation of vector u), the denominator simplifies to
norm(u - cross(L_unit, u)) = sqrt( norm(u - cross(L_unit, u))^2 )
= sqrt( norm(u)^2 + norm(L_unit, u)^2 )
= sqrt( norm(u)^2 + norm(L_unit)^2*norm(u)^2 )
= sqrt( norm(u)^2 + norm(u)^2)
= sqrt(2) * norm(u)
So the system of differential equations simplifies to
dy/dt = u
du/dt = -(a0/sqrt(2)) * u/norm(u) + (a0/sqrt(2)) * cross(L_unit, u)) / norm(u)
The system is designed so that A always moves in the plane passing thorugh the origin B and perpendicular to the vector L_unit.
Becauseu and cross(L_unit, u) are perpendicular, their dot product is 0, which allows us to calculate the time-derivative of the lyapunov function along the solutions to the system above (u' means transpose of column-vector u):
d/dt( L ) = d/dt( norm(u)^2 ) = d/dt( u' * u ) = u' * du/dt
= u' * ( -(a0/sqrt(2)) * u/norm(u)
+ (a0/sqrt(2)) * cross(L_unit, u)) / norm(u) )
= -(a0/sqrt(2)) * u'*u / norm(u)
+ (a0/sqrt(2)) * u'*cross(L_unit, u)) / norm(u)
= -(a0/sqrt(2)) * norm(u)^2 / norm(u)
= -(a0/sqrt(2)) * norm(u)
= - (a0/sqrt(2)) * sqrt(L)
d/dt( L ) = -(a0/sqrt(2)) * sqrt(L) < 0
which means that norm(u) decreases with time to 0, as desired.
The system of differential equations, that governs the motion, looks initially non-linear but can be linearized and explicitly solvaed. However, for simplicity, I have decided to integrate it numerically.
The system of differential equations, that governs the motion, looks initially non-linear but can be linearized and explicitly solved. However, for simplicity, I have decided to integrate it numerically. To do that, I have chosen a geometric integrator method, where the system is split into two explicitly solvable systems, whose solutions are combined together to give (a very good approximation of) the solution to the original system. The systems are:
dy/dt = u / 2
du/dt = -(a0/sqrt(2)) u / norm(u)
and
dy/dt = u / 2
du/dt = (a0/sqrt(2)) cross(L_unit, u) / norm(u)
Initially, the second system is nonlinear, however after we calculate:
d/dt(norm(u)*2) = d/dt (dot(u, u)) = 2 * dot(u, du/dt)
= 2 * dot(u, (a0/sqrt(2)) * cross(L_unit , u))
= 2 * (a0/sqrt(2)) * dot(u, cross(L_unit , u))
= 0
we conclude that during the motion defined by this system, the magnitude of the
velocity is constant, i.e. norm(u) = norm(u0) where u0 = u(0). Thus, the systems, together with their solutions, now look like:
First system:
dy/dt = u / 2
du/dt = -(a0/sqrt(2)) u / norm(u)
Solution:
y(t) = y0 + h * u0/2 - t^2 * a0 * u0 / (4*sqrt(2)*norm(u0));
u(t) = u - t * a0 * u0 / (sqrt(2)*norm(u0));
and
Second system:
dy/dt = u / 2
du/dt = (a0/(sqrt(2)*norm(u0))) cross(L_unit, u)
Solution:
y(t) = y0 + (sqrt(2)*norm(u0)/a0) *( cross(L_unit, u0)
+ sin( t * a0/(sqrt(2)*norm(u0)) ) * u0
- cos( t *a0/(sqrt(2)*norm(u0)) ) * cross(L_unit, u0) )
u(t) = cos( t *a0/(sqrt(2)*norm(u0)) ) * u0
+ sin( t *a0/(sqrt(2)*norm(u0)) ) * cross(L_unit, u0)
The solution to the original system can be approximated as follows. Select a time step h. Then if at time t the spaceship's position and velocity have been calculated to be y, u, the updated spaceship's position and velocity at time t + h can be calculated by first letting the ship move along the solution of the second system starting from y, u for time h/2, then move along the solution of the first system for time h and then move along the solution of the second system for time h/2.
function main()
h = 0.3;
a0 = 0.1;
u_max = .8; % velocity threshold
xB0 = [0; 0; 0];
vB0 = [-1; 2; 0];
xA0 = [ 7; 12; 0] + xB0;
vA0 = [1; 5; 0]/7;
%vA0 = [2; -1; 0];
L_unit = cross(xA0 - xB0, vA0 - vB0);
L_unit = L_unit / norm(L_unit);
t = 0;
xB = xB0;
x = xA0;
v = vA0;
hold on
grid on
%axis([-200 20 -100 350])
plot(0, 0, 'bo')
% STEP 0 (the motion as described in the text above):
n = floor(sqrt(2)*norm(vA0 - vB0)/(a0*h));
for i=1:n
[t, x, v, xB] = E(t, x, v, xB, vB0, a0, L_unit, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
u = v - vB0;
norm_u = norm(u);
% short additional decceleration so that A attains velocity v = vB0
t0 = t + norm_u/a0;
n = floor((t0 - t)/h);
a = - a0 * u / norm_u;
for i=1:n
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, t0-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
% STEP 1 (uniform acceleration of magnitude a0):
v = vB0;
a = x-xB;
norm_y0 = norm(a);
a = - a0 * a / norm_y0;
%t2 = t1 + sqrt( norm_y/a0 );
accel_time = min( u_max/a0, sqrt( norm_y0/a0 ) );
t1 = t0 + accel_time;
n = floor((t1 - t0)/h);
for i=1:n
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
end
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, t1-t);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
% STEP 2 (uniform straight-line motion):
norm_y1 = norm(x-xB);
norm_y12 = max(0, norm_y0 - 2*(norm_y0 - norm_y1));
t12 = norm_y12 / norm(v-vB0)
t = t + t12
n12 = floor(t12/h)
for i=1:n12
x = x + h*v;
xB = xB + h*vB0;
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
x = x + (t12-n12*h)*v;
xB = xB + (t12-n12*h)*vB0;
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
% STEP 3 (uniform deceleration of magnitude a0, symmetric to STEP 1):
a = -a;
for i=1:n % t2 + (t2-t1)
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
end
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, t0+t12+2*accel_time-t);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
norm(x-xB)
norm(v-vB0)
end
Here are the additional functions that are used in the main code above:
% change of coordinates from world coordinates x, v
% to coordinates y, u from spaship B's point of view:
function [y, u] = change(x, v, xB, vB0)
y = x - xB;
u = v - vB0;
end
% inverse chage of coordinates from y, u to x, v
function [x, v] = inv_change(y, u, xB, vB0)
x = y + xB;
v = u + vB0;
end
% solution to the second system of differential equations for a step h:
function [y_out, u_out] = R(y, u, a0, L_unit, h)
omega = a0 / (sqrt(2) * norm(u));
L_x_u = cross(L_unit, u);
cos_omega_h = cos(omega*h);
sin_omega_h = sin(omega*h);
omega = 2*omega;
y_out = y + (L_x_u ...
+ sin_omega_h * u - cos_omega_h * L_x_u) / omega;
u_out = cos_omega_h * u + sin_omega_h * L_x_u;
end
% solution to the first system of differential equations for a step h:
function [y_out, u_out] = T(y, u, a0, h)
sqrt_2 = sqrt(2);
u_unit = u / norm(u);
y_out = y + h * u/2 - h^2 * a0 * u_unit/ (4*sqrt_2);
u_out = u - h * a0 * u_unit / sqrt_2;
end
% approximate solution of the original system of differential equations for step h
% i.e. the sum of furst and second systems of differential equations:
function [t_out, x_out, v_out, xB_out] = E(t, x, v, xB, vB0, a0, L_unit, h)
t_out = t + h;
[y, u] = change(x, v, xB, vB0);
[y, u] = R(y, u, a0, L_unit, h/2);
[y, u] = T(y, u, a0, h);
[y, u] = R(y, u, a0, L_unit, h/2);
xB_out = xB + h*vB0;
[x_out, v_out] = inv_change(y, u, xB_out, vB0);
end
% straight-line motion with constant acceleration:
function [t_out, x_out, v_out, xB_out] = ET(t, x, v, xB, vB0, a, h)
t_out = t + h;
[y, u] = change(x, v, xB, vB0);
y = y + h * u + h^2 * a / 2;
u = u + h * a;
xB_out = xB + h*vB0;
[x_out, v_out] = inv_change(y, u, xB_out, vB0);
end
OLDER VERSION:
I developed two models. Both models are initially described in the moving with B inertial frame of reference y, u (see my previous answers) and then the coordinates are transformed into the original ones x, v. I designed the control based on the function norm(u)^2 as a Lyapunov function, so that in the first step of the algorithm, the acceleration is designed so that the Lyapunov function norm(u)^2 decreases steadily. In the first version, the speed of decrease is quadratic, but the model is easier to integrate, while in the second version, the speed of decrease is exponential, but the model requires Runge-Kutta integration. And I haven't quite tuned it well. I think Version 1 should looks good.
Take L_unit = cross(y0, u0) / norm(cross(y0, u0)).
Version 1: The model is:
dy/dt = y
du/dt = - a0 * (u + cross(L_unit, u)) / norm(u + cross(L_unit, u))
= - a0 * (u + cross(L_unit, u)) / (norm(u)*sqrt(1 + norm(L_unit)^2))
= - a0 * (u + cross(L_unit, u)) / (sqrt(2) * norm(u))
To integrate it, split it into a pair of systems:
dy/dt = y
du/dt = - a0 * u / norm(u)
dy/dt = y
du/dt = - a0 * cross(L_unit, u) / norm(u0) (see previous answers)
and integrate them one after the other for small increments of h time intervals, and then go back and forth between these two systems consecutively. I experimented with some Matlab code:
function main()
h = 0.3;
a0 = 0.1;
xB0 = [0; 0; 0];
vB0 = [-1; 2; 0];
xA0 = [ 7; 12; 0] + xB0;
vA0 = [2; -1; 0];
L_unit = cross(xA0 - xB0, vA0 - vB0);
L_unit = L_unit / norm(L_unit);
t = 0;
xB = xB0;
x = xA0;
v = vA0;
hold on
grid on
%axis([-200 20 -100 350])
plot(0, 0, 'bo')
n = floor(2*norm(v - vB0)/(h*a0));
for i=1:n
[t, x, v, xB] = R(t, x, v, xB, vB0, a0, L_unit, h/2);
a = - a0 * (v - vB0) / norm(v - vB0);
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h/2);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
t1 = t + norm(v - vB0)/a0;
n = floor((t1 - t)/h);
a = - a0 * (v - vB0) / norm(v - vB0);
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t1-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
t2 = t1 + sqrt( norm(x - xB)/a0 );
n = floor((t2 - t1)/h);
a = - a0 * (x - xB) / norm(x - xB);
v = vB0;
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t2-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
for i=1:n % t2 + (t2-t1)
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, 2*t2 - t1 -t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
where the relevant functions are:
function [t_out, y_out, u_out] = R1(t, y, u, a0, L_unit, h)
t_out = t + h;
norm_u = norm(u);
R = norm_u^2 / a0;
cos_omega_h = cos(a0 * h / norm_u);
sin_omega_h = sin(a0 * h / norm_u);
u_unit = u / norm_u;
y_out = y + R * cross(L_unit, u_unit) ...
+ R * sin_omega_h * u_unit ...
- R * cos_omega_h * cross(L_unit, u_unit);
u_out = norm_u * sin_omega_h * cross(L_unit, u_unit) ...
+ norm_u * cos_omega_h * u_unit;
end
function [t_out, x_out, v_out, xB_out] = R(t, x, v, xB, vB0, a0, L_unit, h)
[t_out, y_out, u_out] = R1(t, x - xB, v - vB0, a0, L_unit, h);
xB_out = xB + h * vB0;
x_out = y_out + xB_out;
v_out = u_out + vB0;
end
function [t_out, y_out, u_out] = T1(t, y, u, a, h)
t_out = t + h;
u_out = u + h * a;
y_out = y + h * u + h^2 * a / 2;
end
function [t_out, x_out, v_out, xB_out] = T(t, x, v, xB, vB0, a, h)
[t_out, y_out, u_out] = T1(t, x - xB, v - vB0, a, h);
xB_out = xB + h * vB0;
x_out = y_out + xB_out;
v_out = u_out + vB0;
end
Version 2: The model is:
0 < k0 < 2 * a0 / norm(u0)
dy/dt = y
du/dt = - k0 * u / 2 + sqrt(a0^2 - k0^2 * norm_u^2 / 4) * cross(L_unit, u/norm_u);
Matlab code:
function main()
h = 0.3;
a0 = 0.1;
xB0 = [0; 0; 0];
vB0 = [-1; 2; 0];
xA0 = [ 7; 12; 0] + xB0;
vA0 = [2; -1; 0];
k0 = a0/norm(vA0-vB0);
L_unit = cross(xA0 - xB0, vA0 - vB0);
L_unit = L_unit / norm(L_unit);
t = 0;
xB = xB0;
x = xA0;
v = vA0;
hold on
grid on
%axis([-200 20 -100 350])
plot(0, 0, 'bo')
n = floor(2*norm(v - vB0)/(h*a0)); % this needs to be improved
for i=1:n
[t, x, v, xB] = F_step(t, x, v, xB, vB0, a0, L_unit, k0, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
t1 = t + norm(v - vB0)/a0;
n = floor((t1 - t)/h);
a = - a0 * (v - vB0) / norm(v - vB0);
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t1-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
t2 = t1 + sqrt( norm(x - xB)/a0 );
n = floor((t2 - t1)/h);
a = - a0 * (x - xB) / norm(x - xB);
v = vB0;
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t2-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
for i=1:n % t2 + (t2-t1)
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, 2*t2 - t1 -t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
where the relevant functions are:
function [dydt, dudt] = F1(u, a0, L_unit, k0)
norm_u = norm(u);
dydt = u;
dudt = - k0 * u / 2 + sqrt(a0^2 - k0^2 * norm_u^2/4) * cross(L_unit, u/norm_u);
end
function [t_out, y_out, u_out] = F1_step(t, y, u, a0, L_unit, k0, h)
t_out = t + h;
[z1, w1] = F1(u, a0, L_unit, k0);
[z2, w2] = F1(u + h * w1/2, a0, L_unit, k0);
[z3, w3] = F1(u + h * w2/2, a0, L_unit, k0);
[z4, w4] = F1(u + h * w3, a0, L_unit, k0);
y_out = y + h*(z1 + 2*z2 + 2*z3 + z4)/6;
u_out = u + h*(w1 + 2*w2 + 2*w3 + w4)/6;
end
function [t_out, x_out, v_out, xB_out] = F_step(t, x, v, xB, vB0, a0, L_unit, k0, h)
[t_out, x_out, v_out] = F1_step(t, x-xB, v-vB0, a0, L_unit, k0, h);
xB_out = xB + h * vB0;
x_out = x_out + xB_out;
v_out = v_out + vB0;
end
function [t_out, y_out, u_out] = T1(t, y, u, a, h)
t_out = t + h;
u_out = u + h * a;
y_out = y + h * u + h^2 * a / 2;
end
function [t_out, x_out, v_out, xB_out] = T(t, x, v, xB, vB0, a, h)
[t_out, y_out, u_out] = T1(t, x - xB, v - vB0, a, h);
xB_out = xB + h * vB0;
x_out = y_out + xB_out;
v_out = u_out + vB0;
end