SHARE
TWEET

Untitled

a guest Nov 19th, 2019 87 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. %%%%%%%%%%%%% LGBIO2060: Projet %%%%%%%%%%%%%
  2. clear all;
  3. close all;
  4. %% Conditions %%
  5. Position_Obj = [    1    1   ]';
  6. Vitesse_Obj = [ 0   0   ]';
  7. Temps_Max = 10;
  8.  
  9. %% Parametres du modele %%
  10. l = 10;                                                                     % Nombre de variables (!cf A, B)
  11. dt = 0.01;                                                                  % Pas de temps de la dynamique
  12. Dt = 0.1;                                                                   % Temps de reaction
  13. kv = 0.1;                                                                   %
  14. T = 0.05;                                                                   %
  15. lambda = 0.1;                                                                 %
  16. sigma_mvt = 1e-6;                                                           % Ecart-type de l'erreur de mouvement
  17. sigma_est = 1e-2;                                                           % Ecart-type de l'erreur d'observation
  18. Omega_e = sigma_mvt*eye(10);                                                %
  19. Omega_e =   [   Omega_e             zeros(l,l*(Dt/dt-1));                   %
  20.                 eye(l*(Dt/dt-1))    zeros(l*(Dt/dt-1),l)    ];
  21. Omega_w = sigma_est*eye(10);                                                %
  22. Sigma = zeros(l*(Dt/dt)); Sigma(1:l,1:l) = 1e-6*eye(l);                    %                
  23. w = [  2 0.5 2 0.5 ]';                                                 % Poids (end-point error)
  24.  
  25. %% Dynamique du systeme %%
  26. A =     [   1   dt          0   0       0       0       0   0   0   0       %
  27.             0   1-dt*kv     0   0       dt      0       0   0   0   0
  28.             0   0           1   dt      0       0       0   0   0   0
  29.             0   0           0   1-dt*kv 0       dt      0   0   0   0
  30.             0   0           0   0       1-dt/T   0       0   0   0   0
  31.             0   0           0   0       0       1-dt/T   0   0   0   0  
  32.             0   0           0   0       0       0       1   dt  0   0
  33.             0   0           0   0       0       0       0   1   0   0  
  34.             0   0           0   0       0       0       0   0   1   dt
  35.             0   0           0   0       0       0       0   0   0   1  ];
  36. B =     [   0           0                                                   %
  37.             0           0
  38.             0           0
  39.             0           0
  40.             dt/T        lambda*dt/T
  41.             lambda*dt/T dt/T        
  42.             0           0
  43.             0           0
  44.             0           0
  45.             0           0           ];
  46. W = diag(w);                                                                %
  47. H_h =   [   1   0   0   0   0   0   0   0   0   0                           %
  48.             0   0   0   0   0   0   0   0   0   0
  49.             0   0   1   0   0   0   0   0   0   0
  50.             0   0   0   0   0   0   0   0   0   0
  51.             0   0   0   0   0   0   0   0   0   0
  52.             0   0   0   0   0   0   0   0   0   0
  53.             0   0   0   0   0   0   1   0   0   0
  54.             0   0   0   0   0   0   0   0   0   0
  55.             0   0   0   0   0   0   0   0   1   0
  56.             0   0   0   0   0   0   0   0   0   0   ];            
  57. H = [   zeros(l,l*(Dt/dt-1))    H_h  ];                                     %
  58. R = [   0.001  0                                                          %
  59.         0       0.001  ];
  60. Q = [W, zeros(4,2), -W;                                                     %
  61.      zeros(2,10);
  62.      -W, zeros(4,2), W];
  63.  
  64. %% Ajout du temps de reaction dans la dynamique %%
  65. A =     [   A                   zeros(l,l*(Dt/dt-1));
  66.             eye(l*(Dt/dt-1))    zeros(l*(Dt/dt-1),l)    ];
  67. B =     [   B;
  68.             zeros(l*(Dt/dt-1),2)  ];
  69. Q =     [   Q                   zeros(l,l*(Dt/dt-1));
  70.             zeros(l*(Dt/dt-1))    zeros(l*(Dt/dt-1),l)    ];
  71.  
  72. %% Projection de la trajectoire (boucle backward) %%
  73. L = zeros(2,l*(Dt/dt),Temps_Max/dt);                                        %
  74. S = Q;                                                                      %
  75. for i = 1:Temps_Max/dt
  76.     L(:,:,i) = (R + B.'*S*B)\(B.'*S*A);
  77.     S =  A.'*S*(A-B*L(:,:,i));
  78. end
  79.  
  80. %% Mouvement proprement dit (boucle forward) %%
  81.  
  82. X = zeros(l*(Dt/dt),Temps_Max/dt);                                          %
  83. X(1:l,1) = [ 0   0    0   0   0   0   Position_Obj(1)   Vitesse_Obj(1)  Position_Obj(2)   Vitesse_Obj(2)  ]';
  84. X_est = zeros(l*(Dt/dt),Temps_Max/dt);                                      %
  85. X_est(1:l,1) = [ 0   0    0   0   0   0   Position_Obj(1)   Vitesse_Obj(1)   Position_Obj(2)   Vitesse_Obj(2)  ]'; % + normrnd(zeros(1,10), sigma_est*ones(1,10))';
  86. %X_est(5:6,1) = [    0   0   ]';
  87. Wa = [0.3;0.25;0.2;0.15;0.1];
  88. X_pre = zeros(l*(Dt/dt),Temps_Max/dt);
  89. X_pre(1:l,1) = X_est(1:l,1);
  90. Test1 = zeros(1,2000);
  91. Test2 = zeros(1,2000);
  92. T_jump = 150;
  93. for i = 1:(Temps_Max/dt -1)
  94.     if(i == T_jump)
  95.         X(8,i) = -0.2;
  96.         X(10,i) = -0.04;
  97.         %X(7,i) = -1.0654;
  98.         %X(9,i) = 0.9324;
  99.     end
  100.     noise_mvt = normrnd(zeros(1,10), sigma_mvt*ones(1,10))';                %
  101.     noise_est = normrnd(zeros(1,10), sigma_est*ones(1,10))';                %
  102.     u = -L(:,:,Temps_Max/dt-i+1)*X_est(:,i);                                %
  103.     X(:,i+1) = A*X(:,i) + B*u + [noise_mvt; zeros(l*(Dt/dt-1),1)];          %
  104.     K = A*Sigma*H.'*((H*Sigma*H.' + Omega_w)\eye(10));                      %
  105.     Y = H*X(:,i) + noise_est;                                               %
  106.     X_est(:,i+1) = A*X_est(:,i) + B*u + K*(Y - H*X_est(:,i));               %
  107.  
  108.     if(i>=T_jump)
  109.      
  110.     X_pre(7,i+1) = X(7,i) + (([X(7,i) X(7,i-10) X(7,i-20) X(7,i-30) X(7,i-40) ]-[X(7,i-10) X(7,i-20) X(7,i-30) X(7,i-40) X(7,i-50)])* (1/(10*dt)) * Wa ) * (Temps_Max - i*dt);                                                      % DELETE SANS ANTICIPATION
  111.     X_pre(9,i+1) = X(9,i) + (([X(9,i) X(9,i-10) X(9,i-20) X(9,i-30) X(9,i-40) ]-[X(9,i-10) X(9,i-20) X(9,i-30) X(9,i-40) X(9,i-50)])* (1/(10*dt)) * Wa ) * (Temps_Max - i*dt);  
  112.     X_est(7,i+1) = X_pre(7,i+1);
  113.     X_est(9,i+1) = X_pre(9,i+1);
  114.    
  115.         if(std(X_pre(7,i-50:i)) < 0.005 && std(X_pre(9,i-50:i)) < 0.005)
  116.             disp(i);
  117.             X_est(8,i+1) = 0;
  118.             X_est(10,i+1) = 0;
  119.         end
  120.        
  121.     end
  122.     Sigma = Omega_e + (A - K*H)*Sigma*A'; %
  123.    
  124. %     if(abs(X(1,i)-X(7,i))<0.003 && abs(X(3,i)-X(9,i))<0.003)
  125. %         G = i*dt;
  126. %         fprintf('The hand reach the target after %f seconds \n', G);
  127. %         break;
  128. %     end
  129. end
  130.  
  131. %% Resultats %%
  132.  
  133. time = linspace(0,Temps_Max,(Temps_Max/dt));
  134.  
  135. % Positions
  136. x2 = X(1,:);
  137. y2 = X(3,:);
  138. xt = X(7,:);
  139. yt = X(9,:);
  140.  
  141. Fx = X(5,:);
  142. Fy = X(6,:);
  143.  
  144.  
  145. xx2 = nonzeros(x2).';
  146. yy2 = nonzeros(y2).';
  147. xxt = nonzeros(xt).';
  148. yyt = nonzeros(yt).';
  149.  
  150. figure;
  151. plot(x2,y2,'.g');hold on
  152. %plot(x2(1155),y2(1155),'og');hold on
  153. %plot(xt,yt,'.r');hold on
  154. plot(X_est(7,:),X_est(9,:),'.r');hold on
  155. %plot(X_est(7,700),X_est(9,700),'oc');hold on
  156. %plot(X_est(1,:),X_est(3,:),'.c');hold on
  157. plot(xx2(end),yy2(end),'xb');hold on
  158. plot(x2(T_jump),y2(T_jump),'xr');hold on
  159. plot(Position_Obj(1),Position_Obj(2),'or');hold on
  160. plot(xxt(end),yyt(end), 'ob');hold on
  161.  
  162. % figure
  163. % plot(time,Fx,'b');hold on
  164. % plot(time,Fy,'r');
  165.  
  166.  
  167. % figure
  168. % plot(time,x2,'g');hold on; plot(time(T_jump),x2(T_jump),'or');
  169. % hold on
  170. % plot(time,y2,'b');hold on; plot(time(T_jump),y2(T_jump),'or');
  171. % hold on
  172. % plot(time,xt,'r');
  173. % hold on
  174. % plot(time,yt,'c');
  175.  
  176. % Vitesses
  177. u2 = X(2,:);
  178. v2 = X(4,:);
  179. ut = X(8,:);
  180. vt = X(10,:);
  181. %figure
  182. %plot(u2(end),v2(end),'xg'); hold on
  183. %plot(Vitesse_Obj(1), Vitesse_Obj(2),'ob');
  184. % figure
  185. % plot(time,u2,'r');hold on
  186. % plot(time,v2,'c');hold on
  187. % plot(time,X_est(8,:),'g');hold on
  188. % plot(time,X_est(10,:),'b');hold on
  189. %  figure
  190. %  plot(time,ut);
  191. %  figure
  192. %  plot(time,vt);
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top