rhs_adj.m

Control optimisation. It is example of use BFGS algorithm to control satellite form Earth atmosphere

% funkcja zwracajaca prawe strony rownan stanu i rownan sprzezonych
function [out] = rhs(x,t,u1,u2)
global RzRm bm c az am gz gm gs alfa02;

% rownania ruchu Ziemi:
xz = cos(az*t);
yz = sin(az*t);

% rownania ruchu Marsa:
xm = RzRm*cos((am*t)+bm);
ym = RzRm*sin((am*t)+bm);

% prawe strony rownan:
pz = sqrt(((xz-x(1))^2)+((yz-x(3))^2));
pm = sqrt(((xm-x(1))^2)+((ym-x(3))^2));
ps = sqrt((x(1)^2)+(x(3)^2));
r_stanu(1) = x(2);
r_stanu(2) = (gz*(xz-x(1))/(pz^3))+(gm*(xm-x(1))/(pm^3))-(gs*x(1)/(ps^3))+((alfa02*u1*cos(u2))/(x(5)));
r_stanu(3) = x(4);
r_stanu(4) = (gz*(yz-x(3))/(pz^3))+(gm*(ym-x(3))/(pm^3))-(gs*x(3)/(ps^3))+((alfa02*u1*sin(u2))/(x(5)));
r_stanu(5) = c*u1;
r_stanu=r_stanu';

% prawe strony rownan sprzezonych:
A12= gz*((2*(xz-x(1))^2-(yz-x(3))^2)/pz^5)+gm*((2*(xm-x(1))^2-(ym-x(3))^2)/pm^5)+gs*((2*(x(1)^2)-x(3)^2)/ps^5);
A14= ((3*gz*(xz-x(1))*(yz-x(3)))/pz^5)+((3*gm*(xm-x(1))*(ym-x(3)))/pm^5)+((3*gs*x(1)*x(3))/ps^5);
A32= A14;
A34= gz*((2*(yz-x(3))^2-(xz-x(1))^2)/pz^5)+gm*((2*(ym-x(3))^2-(xm-x(1))^2)/pm^5)+gs*((2*(x(3)^2)-x(1)^2)/ps^5);
A52= -(alfa02*u1*cos(u2))/(x(5)^2);
A54= -(alfa02*u1*sin(u2))/(x(5)^2);
A = [ 0 A12 0 A14 0 ; 1 0 0 0 0 ; 0 A32 0 A34 0 ; 0 0 1 0 0 ; 0 A52 0 A54 0 ];
psi = x(6:10);
r_sprzezone = -A*psi';

out = [r_stanu' r_sprzezone'];
end