Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- %%%%%%%%%%%%% LGBIO2060: Projet %%%%%%%%%%%%%
- clear all;
- close all;
- %% Conditions %%
- Position_Obj = [ 1 1 ]';
- Vitesse_Obj = [ 0 0 ]';
- Temps_Max = 10;
- %% Parametres du modele %%
- l = 10; % Nombre de variables (!cf A, B)
- dt = 0.01; % Pas de temps de la dynamique
- Dt = 0.1; % Temps de reaction
- kv = 0.1; %
- T = 0.05; %
- lambda = 0.1; %
- sigma_mvt = 1e-6; % Ecart-type de l'erreur de mouvement
- sigma_est = 1e-2; % Ecart-type de l'erreur d'observation
- Omega_e = sigma_mvt*eye(10); %
- Omega_e = [ Omega_e zeros(l,l*(Dt/dt-1)); %
- eye(l*(Dt/dt-1)) zeros(l*(Dt/dt-1),l) ];
- Omega_w = sigma_est*eye(10); %
- Sigma = zeros(l*(Dt/dt)); Sigma(1:l,1:l) = 1e-6*eye(l); %
- w = [ 2 0.5 2 0.5 ]'; % Poids (end-point error)
- %% Dynamique du systeme %%
- A = [ 1 dt 0 0 0 0 0 0 0 0 %
- 0 1-dt*kv 0 0 dt 0 0 0 0 0
- 0 0 1 dt 0 0 0 0 0 0
- 0 0 0 1-dt*kv 0 dt 0 0 0 0
- 0 0 0 0 1-dt/T 0 0 0 0 0
- 0 0 0 0 0 1-dt/T 0 0 0 0
- 0 0 0 0 0 0 1 dt 0 0
- 0 0 0 0 0 0 0 1 0 0
- 0 0 0 0 0 0 0 0 1 dt
- 0 0 0 0 0 0 0 0 0 1 ];
- B = [ 0 0 %
- 0 0
- 0 0
- 0 0
- dt/T lambda*dt/T
- lambda*dt/T dt/T
- 0 0
- 0 0
- 0 0
- 0 0 ];
- W = diag(w); %
- H_h = [ 1 0 0 0 0 0 0 0 0 0 %
- 0 0 0 0 0 0 0 0 0 0
- 0 0 1 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 1 0 0 0
- 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 1 0
- 0 0 0 0 0 0 0 0 0 0 ];
- H = [ zeros(l,l*(Dt/dt-1)) H_h ]; %
- R = [ 0.001 0 %
- 0 0.001 ];
- Q = [W, zeros(4,2), -W; %
- zeros(2,10);
- -W, zeros(4,2), W];
- %% Ajout du temps de reaction dans la dynamique %%
- A = [ A zeros(l,l*(Dt/dt-1));
- eye(l*(Dt/dt-1)) zeros(l*(Dt/dt-1),l) ];
- B = [ B;
- zeros(l*(Dt/dt-1),2) ];
- Q = [ Q zeros(l,l*(Dt/dt-1));
- zeros(l*(Dt/dt-1)) zeros(l*(Dt/dt-1),l) ];
- %% Projection de la trajectoire (boucle backward) %%
- L = zeros(2,l*(Dt/dt),Temps_Max/dt); %
- S = Q; %
- for i = 1:Temps_Max/dt
- L(:,:,i) = (R + B.'*S*B)\(B.'*S*A);
- S = A.'*S*(A-B*L(:,:,i));
- end
- %% Mouvement proprement dit (boucle forward) %%
- X = zeros(l*(Dt/dt),Temps_Max/dt); %
- X(1:l,1) = [ 0 0 0 0 0 0 Position_Obj(1) Vitesse_Obj(1) Position_Obj(2) Vitesse_Obj(2) ]';
- X_est = zeros(l*(Dt/dt),Temps_Max/dt); %
- 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))';
- %X_est(5:6,1) = [ 0 0 ]';
- Wa = [0.3;0.25;0.2;0.15;0.1];
- X_pre = zeros(l*(Dt/dt),Temps_Max/dt);
- X_pre(1:l,1) = X_est(1:l,1);
- Test1 = zeros(1,2000);
- Test2 = zeros(1,2000);
- T_jump = 150;
- for i = 1:(Temps_Max/dt -1)
- if(i == T_jump)
- X(8,i) = -0.2;
- X(10,i) = -0.04;
- %X(7,i) = -1.0654;
- %X(9,i) = 0.9324;
- end
- noise_mvt = normrnd(zeros(1,10), sigma_mvt*ones(1,10))'; %
- noise_est = normrnd(zeros(1,10), sigma_est*ones(1,10))'; %
- u = -L(:,:,Temps_Max/dt-i+1)*X_est(:,i); %
- X(:,i+1) = A*X(:,i) + B*u + [noise_mvt; zeros(l*(Dt/dt-1),1)]; %
- K = A*Sigma*H.'*((H*Sigma*H.' + Omega_w)\eye(10)); %
- Y = H*X(:,i) + noise_est; %
- X_est(:,i+1) = A*X_est(:,i) + B*u + K*(Y - H*X_est(:,i)); %
- if(i>=T_jump)
- 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
- 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);
- X_est(7,i+1) = X_pre(7,i+1);
- X_est(9,i+1) = X_pre(9,i+1);
- if(std(X_pre(7,i-50:i)) < 0.005 && std(X_pre(9,i-50:i)) < 0.005)
- disp(i);
- X_est(8,i+1) = 0;
- X_est(10,i+1) = 0;
- end
- end
- Sigma = Omega_e + (A - K*H)*Sigma*A'; %
- % if(abs(X(1,i)-X(7,i))<0.003 && abs(X(3,i)-X(9,i))<0.003)
- % G = i*dt;
- % fprintf('The hand reach the target after %f seconds \n', G);
- % break;
- % end
- end
- %% Resultats %%
- time = linspace(0,Temps_Max,(Temps_Max/dt));
- % Positions
- x2 = X(1,:);
- y2 = X(3,:);
- xt = X(7,:);
- yt = X(9,:);
- Fx = X(5,:);
- Fy = X(6,:);
- xx2 = nonzeros(x2).';
- yy2 = nonzeros(y2).';
- xxt = nonzeros(xt).';
- yyt = nonzeros(yt).';
- figure;
- plot(x2,y2,'.g');hold on
- %plot(x2(1155),y2(1155),'og');hold on
- %plot(xt,yt,'.r');hold on
- plot(X_est(7,:),X_est(9,:),'.r');hold on
- %plot(X_est(7,700),X_est(9,700),'oc');hold on
- %plot(X_est(1,:),X_est(3,:),'.c');hold on
- plot(xx2(end),yy2(end),'xb');hold on
- plot(x2(T_jump),y2(T_jump),'xr');hold on
- plot(Position_Obj(1),Position_Obj(2),'or');hold on
- plot(xxt(end),yyt(end), 'ob');hold on
- % figure
- % plot(time,Fx,'b');hold on
- % plot(time,Fy,'r');
- % figure
- % plot(time,x2,'g');hold on; plot(time(T_jump),x2(T_jump),'or');
- % hold on
- % plot(time,y2,'b');hold on; plot(time(T_jump),y2(T_jump),'or');
- % hold on
- % plot(time,xt,'r');
- % hold on
- % plot(time,yt,'c');
- % Vitesses
- u2 = X(2,:);
- v2 = X(4,:);
- ut = X(8,:);
- vt = X(10,:);
- %figure
- %plot(u2(end),v2(end),'xg'); hold on
- %plot(Vitesse_Obj(1), Vitesse_Obj(2),'ob');
- % figure
- % plot(time,u2,'r');hold on
- % plot(time,v2,'c');hold on
- % plot(time,X_est(8,:),'g');hold on
- % plot(time,X_est(10,:),'b');hold on
- % figure
- % plot(time,ut);
- % figure
- % plot(time,vt);
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
