FindU

function res = FindU(A, y, c1, c2, c3, c4, m1, m2, m3)

u = zeros(6, 3);
res = zeros (6, 3, 251);
t = 0.2;

for i = 1:251
    B = [0 0 0;
         -y(1,i) / m1 0 0;
         0 0 0;
         0 0 -(c2 * y(1,i) -(c2 + c3)*y(3, i) + c3 * y(5, i)) / (m2^2);
         0 0 0;
         0 (- y(5, i)) / m3 0;];
    k1 = t * (A * u + B);
    k2 = t * (A * (u + k1 / 2.0) + B);
    k3 = t * (A * (u + k2 / 2.0) + B);
    k4 = t * (A * (u + k3) + B);
    u1 = u + (1.0 / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
    res(:,:,i + 1) = u1;
    u = u1;
end;

end