@Thomas Richard
Hi Thomas, the wohle program:
Hi Preben, the whole program:
restart;with(LinearAlgebra):with(ArrayTools):with(plots):with(DEtools):
A:=Matrix(12, 12, {(1, 1) = 0, (1, 2) = 0, (1, 3) = 0, (1, 4) = (1.*sin(psi)*sin(phi)+cos(psi)*sin(theta)*cos(phi))*v+(sin(psi)*cos(phi)-cos(psi)*sin(theta)*sin(phi))*w, (1, 5) = -cos(psi)*sin(theta)*u+cos(psi)*cos(theta)*sin(phi)*v+cos(theta)*cos(phi)*cos(psi)*w, (1, 6) = -sin(psi)*cos(theta)*u+(-1.*cos(psi)*cos(phi)-sin(psi)*sin(theta)*sin(phi))*v+(-sin(psi)*sin(theta)*cos(phi)+cos(psi)*sin(phi))*w, (1, 7) = cos(psi)*cos(theta), (1, 8) = -1.*sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi), (1, 9) = sin(psi)*sin(phi)+cos(psi)*sin(theta)*cos(phi), (1, 10) = 0, (1, 11) = 0, (1, 12) = 0, (2, 1) = 0, (2, 2) = 0, (2, 3) = 0, (2, 4) = (-cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi))*v+(-1.*cos(psi)*cos(phi)-sin(psi)*sin(theta)*sin(phi))*w, (2, 5) = -sin(psi)*sin(theta)*u+sin(psi)*cos(theta)*sin(phi)*v+sin(psi)*cos(theta)*cos(phi)*w, (2, 6) = cos(psi)*cos(theta)*u+(-sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi))*v+(1.*sin(psi)*sin(phi)+cos(psi)*sin(theta)*cos(phi))*w, (2, 7) = sin(psi)*cos(theta), (2, 8) = cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi), (2, 9) = -1.*cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi), (2, 10) = 0, (2, 11) = 0, (2, 12) = 0, (3, 1) = 0, (3, 2) = 0, (3, 3) = 0, (3, 4) = cos(theta)*cos(phi)*v-cos(theta)*sin(phi)*w, (3, 5) = -1.*cos(theta)*u-sin(theta)*sin(phi)*v-sin(theta)*cos(phi)*w, (3, 6) = 0, (3, 7) = -1.*sin(theta), (3, 8) = cos(theta)*sin(phi), (3, 9) = cos(theta)*cos(phi), (3, 10) = 0, (3, 11) = 0, (3, 12) = 0, (4, 1) = 0, (4, 2) = 0, (4, 3) = 0, (4, 4) = cos(phi)*sin(theta)*q/cos(theta)-sin(phi)*sin(theta)*r/cos(theta), (4, 5) = sin(phi)*q+sin(phi)*sin(theta)^2*q/cos(theta)^2+cos(phi)*r+cos(phi)*sin(theta)^2*r/cos(theta)^2, (4, 6) = 0, (4, 7) = 0, (4, 8) = 0, (4, 9) = 0, (4, 10) = 1., (4, 11) = sin(phi)*sin(theta)/cos(theta), (4, 12) = cos(phi)*sin(theta)/cos(theta), (5, 1) = 0, (5, 2) = 0, (5, 3) = 0, (5, 4) = -sin(phi)*q-1.*cos(phi)*r, (5, 5) = 0, (5, 6) = 0, (5, 7) = 0, (5, 8) = 0, (5, 9) = 0, (5, 10) = 0, (5, 11) = cos(phi), (5, 12) = -1.*sin(phi), (6, 1) = 0, (6, 2) = 0, (6, 3) = 0, (6, 4) = cos(phi)*q/cos(theta)-sin(phi)*r/cos(theta), (6, 5) = sin(phi)*q*sin(theta)/cos(theta)^2+cos(phi)*r*sin(theta)/cos(theta)^2, (6, 6) = 0, (6, 7) = 0, (6, 8) = 0, (6, 9) = 0, (6, 10) = 0, (6, 11) = sin(phi)/cos(theta), (6, 12) = cos(phi)/cos(theta), (7, 1) = 0., (7, 2) = 0., (7, 3) = 0., (7, 4) = 0., (7, 5) = 0.1315067375e-1*cos(theta), (7, 6) = 0., (7, 7) = -0.1371850317e-2-.3972207569*abs(u), (7, 8) = 2.023246691*r, (7, 9) = -2.023246691*q, (7, 10) = 0., (7, 11) = -2.023246691*w, (7, 12) = 2.023246691*v, (8, 1) = 0., (8, 2) = 0., (8, 3) = 0., (8, 4) = -0.6499787598e-2*cos(theta)*cos(phi), (8, 5) = 0.6499787598e-2*sin(theta)*sin(phi), (8, 6) = 0., (8, 7) = -.4942551022*r, (8, 8) = -0.3390220094e-3-2.804575846*abs(v), (8, 9) = .9999999999*p, (8, 10) = .9999999999*w, (8, 11) = 0., (8, 12) = -.4942551022*u, (9, 1) = 0., (9, 2) = 0., (9, 3) = 0., (9, 4) = 0.6499787598e-2*cos(theta)*sin(phi), (9, 5) = 0.6499787598e-2*sin(theta)*cos(phi), (9, 6) = 0., (9, 7) = .4942551022*q, (9, 8) = -.9999999999*p, (9, 9) = -0.1017066028e-2-2.804575846*abs(w), (9, 10) = -.9999999999*v, (9, 11) = .4942551022*u, (9, 12) = 0., (10, 1) = 0., (10, 2) = 0., (10, 3) = 0., (10, 4) = -43.66429218*cos(theta)*cos(phi), (10, 5) = 43.66429218*sin(theta)*sin(phi), (10, 6) = 0., (10, 7) = 0., (10, 8) = 0., (10, 9) = 0., (10, 10) = -0.7377506151e-2-38.83936290*abs(p), (10, 11) = 0., (10, 12) = 0., (11, 1) = 0., (11, 2) = 0., (11, 3) = 0., (11, 4) = 0., (11, 5) = -.4978289591*cos(theta), (11, 6) = 0., (11, 7) = 1.140707359*w, (11, 8) = 0., (11, 9) = 1.140707359*u, (11, 10) = .9885987172*r, (11, 11) = -0.1070529557e-3-75.90863270*abs(q), (11, 12) = .9885987172*p, (12, 1) = 0., (12, 2) = 0., (12, 3) = 0., (12, 4) = 0., (12, 5) = 0., (12, 6) = 0., (12, 7) = -1.140707359*v, (12, 8) = -1.140707359*u, (12, 9) = 0., (12, 10) = -.9885987172*q, (12, 11) = -.9885987172*p, (12, 12) = -0.1223462351e-3-19.86275889*abs(r)}):
B_t:=<0 ,0 ,0 ,0 ,0 ,0 ,b1 ,0 ,0 ,0 ,0 ,0 |
0 ,0 ,0 ,0 ,0 ,0 ,0 ,b2 ,0 ,0 ,0 ,0 |
0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,b3 ,0 ,0 ,0 |
0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,b4 ,0 ,0 |
0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,b5 ,0 |
0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,b6 >:
C_t:=<<IdentityMatrix(6,6)|ZeroMatrix(6,6)>>:
alpha1:=1:
alpha2:=1:
alpha3:=1:
alpha4:=1:
alpha5:=1:
alpha6:=1:
beta1:=1:
beta2:=1:
beta3:=1:
beta4:=1:
beta5:=1:
beta6:=1:
alpha:=<<alpha1,0,0,0,0,0|0,alpha2,0,0,0,0|0,0,alpha3,0,0,0|0,0,0,alpha4,0,0|0,0,0,0,alpha5,0|0,0,0,0,0,alpha6>>:
beta:=<<beta1,0,0,0,0,0|0,beta2,0,0,0,0|0,0,beta3,0,0,0|0,0,0,beta4,0,0|0,0,0,0,beta5,0|0,0,0,0,0,beta6>>:
x_t:=<<x(t),y(t),z(t),phi(t),theta(t),psi(t),u(t),v(t),w(t),p(t),q(t),r(t)>>:
y_t:=C_t.x_t:
y_p:=map(diff,y_t,t):
nu:=-alpha.y_p-beta.y_t:
##################
operatorM:=proc(A,c,k)#Operator-M-Definition: c: Zeilenvektor
local MA ,i:
MA[0]:=c;
for i from 1 to k do
MA[i]:= map(diff,MA[i-1],t)+MA[i-1].A:
end do
end proc:
#################
x_r:=10*sin(t):
y_r:=10*cos(t):
z_r:=20*t:
psi_r:=arctan(diff(y_r,t)/diff(x_r,t)):
theta_r:=-arctan(diff(z_r,t)*cos(psi_r)/diff(x_r,t)):
phi_r:=arccot(cot(theta_r)/sin(psi_r)+tan(psi_r)/sin(theta_r)):
eta_r:=<<x_r,y_r,z_r,phi_r,theta_r,psi_r>>:
dot_eta_r:=<<diff(x_r,t),diff(y_r,t),diff(z_r,t),diff(phi_r,t),diff(theta_r,t),diff(psi_r,t)>>:
u_r:= (cos(psi_r)*cos(theta_r)*diff(x_r,t)+sin(psi_r)*cos(theta_r)*diff(y_r,t)-sin(theta_r)*diff(z_r,t)):
v_r:= ((cos(psi_r)*sin(theta_r)*sin(phi_r)-sin(psi_r)*cos(phi_r))*diff(x_r,t)+(cos(psi_r)*cos(phi_r)+sin(psi_r)*sin(theta_r)*sin(phi_r))*diff(y_r,t)+cos(theta_r)*sin(phi_r)*diff(z_r,t)):
w_r:= ((cos(psi_r)*sin(theta_r)*cos(phi_r)+sin(psi_r)*sin(phi_r))*diff(x_r,t)+(sin(psi_r)*sin(theta_r)*cos(phi_r)-cos(psi_r)*sin(phi_r))*diff(y_r,t)+cos(theta_r)*cos(phi_r)*diff(z_r,t)):
p_r:=(phi_r-sin(theta_r)*psi_r):
q_r:= (cos(phi_r)*theta_r+cos(theta_r)*sin(phi_r)*psi_r):
r_r:= (-sin(phi_r)*theta_r+cos(theta_r)*cos(phi_r)*psi_r):
nu_r:=<<u_r,v_r,w_r,p_r,q_r,r_r>>:
A_r:=eval(A,[x=x_r,y=y_r,z=z_r,phi=phi_r,theta=theta_r,psi=psi_r,u=u_r,v=v_r,w=w_r,p=p_r,q=q_r,r=r_r]):
B_r:=eval(B_t,[b1=0.00685925158705934,b2=0.00339022009418016,b3=0.00339022009418016,b4=0.670682377401357,b5=0.00764663969487991,b6=0.00764663969487991]):
A_r_bar:=<<operatorM(A_r,(Row(C_t,1)),2),
operatorM(A_r,(Row(C_t,2)),2),
operatorM(A_r,(Row(C_t,3)),2),
operatorM(A_r,(Row(C_t,4)),2),
operatorM(A_r,(Row(C_t,5)),2),
operatorM(A_r,(Row(C_t,6)),2)>>:
#Koppel-Matrix
K_r:=<<operatorM(A_r,(Row(C_t,1)),1).Column(B_r,1),operatorM(A_r,(Row(C_t,2)),1).Column(B_r,1),operatorM(A_r,(Row(C_t,3)),1).Column(B_r,1),
operatorM(A_r,(Row(C_t,4)),1).Column(B_r,1),operatorM(A_r,(Row(C_t,5)),1).Column(B_r,1),operatorM(A_r,(Row(C_t,6)),1).Column(B_r,1)|
operatorM(A_r,(Row(C_t,1)),1).Column(B_r,2),operatorM(A_r,(Row(C_t,2)),1).Column(B_r,2),operatorM(A_r,(Row(C_t,3)),1).Column(B_r,2),
operatorM(A_r,(Row(C_t,4)),1).Column(B_r,2),operatorM(A_r,(Row(C_t,5)),1).Column(B_r,2),operatorM(A_r,(Row(C_t,6)),1).Column(B_r,2)|
operatorM(A_r,(Row(C_t,1)),1).Column(B_r,3),operatorM(A_r,(Row(C_t,2)),1).Column(B_r,3),operatorM(A_r,(Row(C_t,3)),1).Column(B_r,3),
operatorM(A_r,(Row(C_t,4)),1).Column(B_r,3),operatorM(A_r,(Row(C_t,5)),1).Column(B_r,3),operatorM(A_r,(Row(C_t,6)),1).Column(B_r,3)|
operatorM(A_r,(Row(C_t,1)),1).Column(B_r,4),operatorM(A_r,(Row(C_t,2)),1).Column(B_r,4),operatorM(A_r,(Row(C_t,3)),1).Column(B_r,4),
operatorM(A_r,(Row(C_t,4)),1).Column(B_r,4),operatorM(A_r,(Row(C_t,5)),1).Column(B_r,4),operatorM(A_r,(Row(C_t,6)),1).Column(B_r,4)|
operatorM(A_r,(Row(C_t,1)),1).Column(B_r,5),operatorM(A_r,(Row(C_t,2)),1).Column(B_r,5),operatorM(A_r,(Row(C_t,3)),1).Column(B_r,5),
operatorM(A_r,(Row(C_t,4)),1).Column(B_r,5),operatorM(A_r,(Row(C_t,5)),1).Column(B_r,5),operatorM(A_r,(Row(C_t,6)),1).Column(B_r,5)|
operatorM(A_r,(Row(C_t,1)),1).Column(B_r,6),operatorM(A_r,(Row(C_t,2)),1).Column(B_r,6),operatorM(A_r,(Row(C_t,3)),1).Column(B_r,6),
operatorM(A_r,(Row(C_t,4)),1).Column(B_r,6),operatorM(A_r,(Row(C_t,5)),1).Column(B_r,6),operatorM(A_r,(Row(C_t,6)),1).Column(B_r,6) >>:
K_r_inv:=MatrixInverse(K_r):
u_t:=K_r_inv.(nu-A_r_bar.x_t):
#System of differential equations
sys:=A_r.x_t+B_r.u_t:
#intial values
init:=x(0)=0, y(0)=0, z(0)=0, phi(0)=0, theta(0)=0, psi(0)=0, u(0)=0, v(0)=0, w(0)=0, p(0)=0, q(0)=0, r(0)=0:
sys1:=diff(x(t),t)=simplify(sys[1],trig):
sys2:=diff(y(t),t)=simplify(sys[2],trig):
sys3:=diff(z(t),t)=simplify(sys[3],trig):
sys4:=diff(phi(t),t)=simplify(sys[4],trig):
sys5:=diff(theta(t),t)=simplify(sys[5],trig):
sys6:=diff(psi(t),t)=simplify(sys[6],trig):
sys7:=diff(u(t),t)=sys[7]:
sys8:=diff(v(t),t)=sys[8]:
sys9:=diff(w(t),t)=sys[9]:
sys10:=diff(p(t),t)=sys[10]:
sys11:=diff(q(t),t)=sys[11]:
sys12:=diff(r(t),t)=sys[12]:
#Numerical solve of the System of differential equations
FF:=dsolve([sys1,sys2,sys3,sys4,sys5,sys6,sys7,sys8,sys9,sys10,sys11,sys12,init],[x(t),y(t),z(t),phi(t),theta(t),psi(t),u(t),v(t),w(t),p(t),q(t),r(t)],numeric);
#The Error message
Error, (in rtable/Sum) invalid arguments
###############
Thanks a lot
