Составил код для интегрирования функции:
%% Исходные данные
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), но на новом шаге они принимают исходные значения.
Как это исправить?
Простой
