Математический маятник и дифф.-алгебр. система уравнений

[tudexaon]

Здравствуйте!
Помогите сформулировать систему нелинейных уравнений для численного решений задачи математического маятника.

Суть:
Есть система уравнений

$\noindent \xi'(t) = u(t)\\ \eta'(t) = v(t)\\ u'(t) = -\frac{2}{m}\lambda (t)\xi (t)\\ v'(t) = -\frac{2}{m}\lambda (t)\eta (t) - g\\ 0 = \xi^2(t) + \eta^2(t) - l^2 $

Если решить численно как систему обыкновенных дифференциальных уравнений, ошибка будет дрейфовать и нить маятника со временем "удлиняться". Поэтому нужно решить численно как систему дифференциально-алгебраических уравнений. Но я не могу понять как преобразовать систему уравнений для этого.

То что я сделал, так это нашел $\lambda' = -\frac{3mg}{2l^2}v(t)$ и с помощью неявного метода трапеций составил систему нелинейных уравнений и "скормил" это fsolve() Матлаба.
Вот участок кода:

Используется синтаксис Matlab M

    % x1 = v, x2 = u, x3 = xi, x4 = eta, x5 = lambda
    fsystem = @(x) [
        (v(i) + h/2*(fv(lambda(i), eta(i)) + fv(x(5), x(4)))) - x(1);
        (u(i) + h/2*(fu(lambda(i), xi(i)) + fu(x(5), x(3)))) - x(2);
        (eta(i) + h/2*(v(i) + x(1))) - x(4);
        (xi(i) + h/2*(u(i) + x(2))) - x(3);
        (lambda(i) + h/2*(fl(v(i)) + fl(x(1)))) - x(5);
        (x(3)^2 + x(4)^2) - l^2;
        x(3)*x(2) + x(4)*x(1)
        ];
    
    x = [v(i), u(i), xi(i), eta(i), lambda(i)];
    options = optimset('Display','off', 'Algorithm', 'levenberg-marquardt');
    x = fsolve(fsystem, x, options);
    
    v(i+1) = x(1);
    u(i+1) = x(2);
    xi(i+1) = x(3);
    eta(i+1) = x(4);
    lambda(i+1) = x(5);
 

Получились следующие графики:

Алгебраическое ограничение не дрейфует, но вот скрытое ограничение ведет себя странно.

[DeBill]

tudexaon
Система решается (почти) явно.
Домножая третье на Эту, а четвертое - на Кси, и вычитая, избавимся от лямбды.
Решение будем искать в виде $\xi = l\cdot \sin \varphi, \eta =l \cdot \cos \varphi$. Подставляя все это дело, получим (надо проверить, делал на коленке) для $\varphi$ уравнение "физического" маятника $\ddot{\varphi} =-\frac{g}{l} \sin \varphi$.
У него есть первый интеграл (энергии)....

[pogulyat_vyshel]

как искать множители Лагранжа написано тут https://dxdy.ru/topic128178.html