Как не изменять значения в функции?

Question

[Игорь]

Составил код для интегрирования функции:

%% Исходные данные
omega = deg2rad([ 1;  1;  1]);                  % Угловые скорости,   рад/с
ang   = deg2rad([ 0;  0;  0]);                  % Значение углов,     рад

%% Интегрирование
ITime = 700;                                    % Конечное время,     с     
IStep = 1;                                      % Шаг интегрирования, с
MTime = 0:IStep:ITime;                          % Массив времени,     с

[T,Y] = ode45(@Model, MTime, [omega; ang]);

Код функции:

function dy = Model(t, x)
    %% Начальные параметры и условия
    omega  = x(1:3);             % Угловая скорость,                 рад/с
    ang    = x(4:6);             % Угол,                             рад

    pomega = deg2rad([0; 0; 0]); % Программные угловые скорости,     рад/с
    pang   = deg2rad([0; 0; 0]); % Программные углы,                 рад
    PSIG   = deg2rad(1);         % Заданное значение действия углов, рад
    
    %  Управляющий момент
    MU     = [0; 0; 0];          % Изначально, далее меняется
    %  Исходный кватернион
    p      = [1; 1; 1; 1];       % Изначально, далее меняется
        
    %% Параметры аппарата
    P = 0.6;            % Тяга двигателя, кг
    L = 1;              % Плечо,          м
    
    %  Массив моментов для каждого ДУ
    MT = [ cross((P * [ 1  0  0]), (L * [ 0  1  0])); ...
           cross((P * [ 1  0  0]), (L * [ 0  0  1])); ...
           cross((P * [ 1  0  0]), (L * [ 0 -1  0])); ...
           cross((P * [ 1  0  0]), (L * [ 0  0 -1])); ...
           cross((P * [ 0  0 -1]), (L * [ 0  1  0])); ...
           cross((P * [ 0 -1  0]), (L * [ 0  0  1])); ...
           cross((P * [ 0  0  1]), (L * [ 0 -1  0])); ...
           cross((P * [ 0  1  0]), (L * [ 0  0 -1]))];

    %  Возмущающий момент
    MV = [0; 0; 0];   % Пока считаем нулевым
   
    %  Моменты инерции
    J = [1400; 830; 730];    
    
    %% Параметры закона управления
    K  = [ 3; 3; 3];
    Ki = [ 1; 1; 1];
    Ks = [ 1; 1; 1];
        
    %% Расчет программного кватерниона ориентации         
    pp        = [ cos(pang(3)/2)*cos(pang(2)/2)*cos(pang(1)/1) +   ...
                    sin(pang(3)/2)*sin(pang(2)/2)*sin(pang(1)/2);  ...
                  cos(pang(3)/2)*cos(pang(2)/2)*sin(pang(1)/1) -   ...
                    sin(pang(3)/2)*sin(pang(2)/2)*cos(pang(1)/2);  ...
                  cos(pang(3)/2)*sin(pang(2)/2)*cos(pang(1)/1) +   ...
                    sin(pang(3)/2)*cos(pang(2)/2)*sin(pang(1)/2);  ...
                 -cos(pang(3)/2)*sin(pang(2)/2)*sin(pang(1)/1) +   ...
                    sin(pang(3)/2)*cos(pang(2)/2)*cos(pang(1)/2)];
    
    %% Расчет текущего кватерниона ориентации         
    dphi_dt   = omega * 0.1;         
    
    dp        = 1/2 * [-dphi_dt(1)*p(2) - dphi_dt(2)*p(3) - dphi_dt(3)*p(4);...
                        dphi_dt(1)*p(1) + dphi_dt(3)*p(3) - dphi_dt(2)*p(4);...
                        dphi_dt(2)*p(1) - dphi_dt(3)*p(2) + dphi_dt(1)*p(4);...
                        dphi_dt(3)*p(1) + dphi_dt(1)*p(2) - dphi_dt(1)*p(2)];
    
    p         = dp + p;
    
    %% Расчет отклонений КА от программной ориентации
    %  Сопряженный кватернион
    if p(1) > 0
        pc    = [ p(1); -p(2); -p(3); -p(4)];
    else
        pc    = [-p(1);  p(2);  p(3);  p(4)];
    end
    %  Расчет кватерниона рассогласования
    R         = [ pc(1) * pp(1) - pc(2) * pp(2) -  ...
                    pc(3) * pp(3) - pc(4) * pp(4); ...
                  pc(1) * pp(2) + pc(2) * pp(1) +  ...
                    pc(3) * pp(4) - pc(4) * pp(3); ...
                  pc(1) * pp(3) + pc(3) * pp(1) -  ...
                    pc(2) * pp(4) + pc(4) * pp(2); ...
                  pc(1) * pp(4) + pc(4) * pp(1) +  ...
                    pc(2) * pp(3) - pc(3) * pp(2)];   
    
    %% Формирование закона управления          
    ddphi_dt  = dphi_dt * t; 
    
    SIG       = [ K(1)*(-2*R(1)*R(2)) + Ki(1)*(omega(1)-pomega(1)) + Ks(1)*ddphi_dt(1);...
                  K(2)*(-2*R(1)*R(3)) + Ki(2)*(omega(2)-pomega(2)) + Ks(2)*ddphi_dt(2);...
                  K(3)*(-2*R(1)*R(4)) + Ki(3)*(omega(3)-pomega(3)) + Ks(3)*ddphi_dt(3)];  
    
    %% Логика включения двигателя
    %  Сделано через перебор моментов тяги
    for i = 1 : 3  
        MU(i) = 0;
        if abs(SIG(i)) >= PSIG
            for j = 1 : 8
                if sign(MT(j, i)) == -sign(SIG(i))
                    MU(i) = MU(i) + MT(j, i);
                end
            end
        end
    end
    
    %% Сумма моментов
    M = MU + MV;
    
    %% Интегрирование углового движения и кинематических уравнений КА 
    domega_dt = [(M(1) - (J(3) - J(2))*omega(2)*omega(3))/J(1);...
                 (M(2) - (J(1) - J(3))*omega(1)*omega(3))/J(2);...
                 (M(3) - (J(2) - J(1))*omega(1)*omega(2))/J(3)];
    
    dang_dt   = [ omega(1) - tan(ang(2)) * sin(ang(1)) * omega(2) + tan(ang(1)) * cos(ang(2)) * omega(3);...
                  cos(ang(1)) * omega(2) + sin(ang(1)) * omega(3);...
                 -(sin(ang(1)) / cos(ang(2))) * omega(2) + (cos(1) / cos(ang(2))) * omega(3)];       
    
    %% Вывод результатов интегрирования
    dy = [ domega_dt;... % Угловая скорость
           dang_dt];     % Угол  

end

Проблема в том, что при интегрировании должны меняться константы (p и MU), но на новом шаге они принимают исходные значения.
Как это исправить?

Comments

[d'Ivan]

Константы на то и константы, что их значение не меняется. В коде же их значение меняется, а значит, называются переменными.

[d'Ivan]

Приятно смотреть на код: выглядит опрятно и присутствуют комментарии.

[Игорь]

Роман Мирр: Спасибо, приятно читать)

Answer 1

[d'Ivan]

Предлагаю вынести инициализацию p и MU за пределы функции Model.
Скорее всего, каждый вызов Model приводит к инициализации этих переменных.

%  Управляющий момент
    MU     = [0; 0; 0];          % Изначально, далее меняется
    %  Исходный кватернион
    p      = [1; 1; 1; 1];       % Изначально, далее меняется

Да и, вообще, любую инициализацию лучше выносить, раз она повторяется.

Comments

[Игорь]

С MU я уже разобрался (поставил логику управления в начало model), но вот с p так не пройдет. Если ее поставить в код для интегрирования функции и считать как входную переменную, надо переделывать уравнения для ее изменения (ведь теперь он будет его интегрировать)?