# -*- coding: utf-8 -*- """ Created on Wed Jan 1 14:30:33 2015 ok @author: joseorellana """ # import basic modules import numpy as np import matplotlib.pyplot as plt from numpy import linalg as la import math def cm0(alpha1): return -.3*alpha1+.001 def cmg(alpha1): return -.1*alpha1 def cxg(beta0): return .2*beta0 def cz0(alpha1): return 10.*alpha1-.05*alpha1**2 def czg(alpha1): return 1.*alpha1-.01*alpha1**2 def dcm0(alpha1): return -.3 def dcmg(alpha1): return -.1 def dcz0(alpha1): return 10.-.1*alpha1**2 def dczg(alpha1): return 1.-.02*alpha1 def soluedo(RS,NPTs,dt,phi0s): zs=np.linspace(1,2,NPT)*0. alphas=np.linspace(1,2,NPT)*0. zs[0]=phi0s[0] zs[1]=zs[0]+dt*phi0s[1] alphas[0]=phi0s[2] alphas[1]=alphas[0]+dt*phi0s[3] X1=np.transpose([zs[0],alphas[0]]) X2=np.transpose([zs[1],alphas[1]]) for i in range(2,NPTs): X3=2*X2-X1-dt**2*np.dot(RS,X2) zs[i]=X3[0] alphas[i]=X3[1] X1=X2 X2=X3 return zs,alphas def calculgramp(NPTg,Tg,dtg,z0Mg,dz0Mg,alpha0Mg,dalpha0Mg,xeqMg,Radg,B0g,C0g,F0g,a0g,b0g): GRAMM=np.zeros((4,4),float) phi0=0.*np.linspace(1,4,4) deltaphi0=0.*np.linspace(1,4,4) Lsec=0.*np.linspace(1,2,4) P1dot=np.linspace(1,2,NPTg) P2dot=np.linspace(1,2,NPTg) Q1dot=np.linspace(1,2,NPTg) Q2dot=np.linspace(1,2,NPTg) betag=np.linspace(1,2,NPTg) r=math.sqrt(a0g/b0g) cc=math.sinh(r*Tg) for k1 in range(4): for i1 in range(4): if i1 == k1: phi0[i1]=1 else: phi0[i1]=0 P1,P2=soluedo(Radg,NPTg,dtg,phi0) P1dot[0]=phi0[1] P2dot[0]=phi0[3] for i in range(1,NPTg): P1dot[i]=(P1[i]-P1[i-1])/dtg P2dot[i]=(P2[i]-P2[i-1])/dtg intj=0. for iii in range(NPTg): intj=intj+math.sinh(r*(NPTg-iii)*dtg)*(C0g[0]*P1dot[iii]+C0g[1]*P2dot[iii]-B0g[0]*P1[iii]-B0g[1]*P2[iii]) intj=intj*dtg for kkk in range(NPTg): resi=0. for jjkk in range(kkk+1): resi=resi+(C0g[0]*P1dot[jjkk]+C0g[1]*P2dot[jjkk]-B0g[0]*P1[jjkk]-B0g[1]*P2[jjkk])*math.sinh(r*(kkk-jjkk+1)*dtg) betag[kkk]=(1/(r*b0g))*((np.sinh(r*dtg*kkk)/cc)*intj-resi*dtg) betag[0]=0. Lsec[k1]=dz0Mg*phi0[0]+dalpha0Mg*phi0[2]-z0Mg*phi0[1]-alpha0Mg*phi0[3]+xeqMg[0]*P1dot[NPTg-1]+xeqMg[1]*P2dot[NPTg-1] squatro=0. for jkl in range(NPTg): squatro=squatro+F0g[0]*P1[jkl]+F0g[1]*P2[jkl] Lsec[k1]=Lsec[k1]+squatro*dtg for k2 in range(k1,4): for i2 in range(4): if i2 == k2 : deltaphi0[i2]=1 else: deltaphi0[i2]=0 Q1,Q2=soluedo(Radg,NPTg,dtg,deltaphi0) Q1dot[0]=phi0[1] Q2dot[0]=phi0[3] for i in range(1,NPTg): Q1dot[i]=(Q1[i]-Q1[i-1])/dtg Q2dot[i]=(Q2[i]-Q2[i-1])/dtg sbis=0 for jj in range(NPTg): sbis=sbis+(C0g[0]*Q1dot[jj]+C0g[1]*Q2dot[jj]-B0g[0]*Q1[jj]-B0g[1]*Q2[jj])*betag[jj] GRAMM[k1,k2]=sbis*dtg GRAMM[k2,k1]=GRAMM[k1,k2] PHISOL=la.solve(GRAMM,np.transpose(Lsec)) return PHISOL def calculetatadj(NPT,dt,PHIHUMc,Rad): zz=np.linspace(1,2,NPT)*0. alphaa=np.linspace(1,2,NPT)*0. zz[0]=PHIHUMc[0] zz[1]=zz[0]+dt*PHIHUMc[1] alphaa[0]=PHIHUMc[2] alphaa[1]=alphaa[0]+dt*PHIHUMc[3] X1=np.transpose([zz[0],alphaa[0]]) X2=np.transpose([zz[1],alphaa[1]]) for i in range(2,NPT): X3=2*X2-X1-dt**2*np.dot(Rad,X2) zz[i]=X3[0] alphaa[i]=X3[1] X1=X2 X2=X3 return zz,alphaa m=1. J0=1. k=10. c=2. a=-.02 V=1. z0=.0 dz0=.0 alpha0=.1 dalpha0=1. beta0=.05 ro=1.2 S=1. L=1. a0=1. b0=10. r=math.sqrt(a0/b0) NPT=6000 T=3. dt=T/NPT bL=1. temp=np.linspace(1,T+1,NPT)-1 temp2=np.linspace(1,2*T+1,2*NPT)-1 z=np.linspace(1,2,NPT)*0. alpha=np.linspace(1,2,NPT)*0. z2=np.linspace(1,2,2*NPT)*0. alpha2=np.linspace(1,2,2*NPT)*0. M=[[m, a*math.cos(alpha0)],[a*math.cos(alpha0),J0]] K=[[k ,-ro*S*V**2*dcz0(alpha0)/2],[0., c-ro*S*V**2*L*dcm0(alpha0)/2]] R=la.solve(M,K) Rad=la.solve(M,np.transpose(K)) F0=[ro*S*V**2*cz0(alpha0)/2,ro*S*L*V**2*cm0(alpha0)/2] B0=[ro*S*V**2*dczg(beta0)/2,ro*S*L*V**2*dcmg(beta0)/2] C0=[-bL*ro*S*V*(dczg(beta0)+cxg(beta0))/2,-bL*ro*S*L*V*(2*cmg(beta0)+dcmg(beta0))/2] xeq=la.solve(K,np.transpose(F0)) F=la.solve(M,np.transpose(F0)) C=la.solve(M,np.transpose(C0)) B=la.solve(M,np.transpose(B0)) beta=np.linspace(1,2,NPT)*0 betadot=np.linspace(1,2,NPT)*0 P1dot=np.linspace(1,2,NPT)*0. P2dot=np.linspace(1,2,NPT)*0. bipB=la.norm(B0)**2 bipC=la.norm(C0)**2 bipBC=np.inner(B0,np.transpose(C0)) delta=bipB*bipC-bipBC matin=[[bipC,-bipBC],[-bipBC,bipC]]/delta semB=[3 , 1] semC=[1 ,1] Bstar=np.dot(matin,np.transpose(semB)) Cstar=np.dot(matin,np.transpose(semC)) f1=np.dot(B0,np.dot(Rad,Bstar)) f2=np.dot(B0,np.dot(Rad,Cstar)) f3=np.dot(C0,np.dot(Rad,Bstar)) f4=np.dot(C0,np.dot(Rad,Cstar)) root1=(-f3+math.sqrt(f3**2-4*f4))/2 root2=(-f3-math.sqrt(f3**2-4*f4))/2 rav1=root1**3+f1*root1+f2 rav2=root2**3+f1*root2+f2 Avis='si le nombre est nul il n y a pas controlabilite' test=abs(rav1*rav2/f2) print Avis , test condinit= np.dot(M,[z0,alpha0]) vitesseinit=np.dot(M,[dz0,dalpha0]) xeqM=np.dot(M,xeq) z0M=condinit[0] alpha0M=condinit[1] dz0M=vitesseinit[0] dalpha0M=vitesseinit[1] PHIHUM=calculgramp(NPT,T,dt,z0M,dz0M,alpha0M,dalpha0M,xeqM,Rad,B0,C0,F0,a0,b0) P1,P2=calculetatadj(NPT,dt,PHIHUM,Rad) P1dot[0]=PHIHUM[1] P2dot[0]=PHIHUM[3] for ijk in range(1,NPT): P1dot[ijk]=(P1[ijk]-P1[ijk-1])/dt P2dot[ijk]=(P2[ijk]-P2[ijk-1])/dt cc=math.sinh(r*T) intj=0 for jj in range(NPT): intj=intj+(C0[0]*P1dot[jj]+C0[1]*P2dot[jj]-B0[0]*P1[jj]-B0[1]*P2[jj])*math.sinh(r*(NPT-jj+1)*dt) intj=intj*dt for kl in range(NPT): rich=0. for jj in range(kl): rich=rich+(C0[0]*P1dot[jj]+C0[1]*P2dot[jj]-B0[0]*P1[jj]-B0[1]*P2[jj])*math.sinh(r*(kl-jj+1)*dt)*dt beta[kl]=(1/(b0*r))*((math.sinh(r*dt*(kl-1))/cc)*intj-rich) beta[0]=0 for jj in range(1,NPT-1): betadot[jj]=(beta[jj]-beta[jj-1])/dt betadot[0]=betadot[1] betadot[NPT-1]=betadot[NPT-2] plt.figure(1) plt.subplot(1,3,1) plt.plot(temp,beta) plt.xlabel('Temps') plt.ylabel('Loi de controle exacte'); plt.title('controle exact') z[0]=z0; z[1]=z0+dt*dz0 alpha[0]=alpha0 alpha[1]=alpha[0]+dt*dalpha0 X1=np.transpose([z[0],alpha[0]]) X2=np.transpose([z[1],alpha[1]]) for i in range(2,NPT): X3=2*X2-X1+(dt**2)*(F+B*beta[i-1]+C*betadot[i-1]-np.dot(R,X2)) z[i]=X3[0] alpha[i]=X3[1] X1=X2 X2=X3 plt.subplot(1,3,2) plt.plot(temp,z,temp,alpha) plt.xlabel('Temps'); plt.ylabel('Solution en z et alpha ') plt.title('Solution controlee ') for i in range(NPT): z2[i]=z[i] alpha2[i]=alpha[i] X3=2*X2-X1+dt**2*(F-np.dot(R,X2)) z2[i+NPT]=X3[0] alpha2[i+NPT]=X3[1] X1=X2 X2=X3 plt.subplot(1,3,3) plt.plot(temp2,z2,temp2,alpha2) plt.xlabel('Temps') plt.ylabel('Solution avec coupure du controle a T') plt.title('Solution prolongee apres que le controle soit coupe') plt.show()