Index: /applications/dual/SIDP/smc29/my_sample.m
===================================================================
--- /applications/dual/SIDP/smc29/my_sample.m (revision 1255)
+++ /applications/dual/SIDP/smc29/my_sample.m (revision 1255)
@@ -0,0 +1,7 @@
+function x_0=my_sample(vahy,x_1)
+N=size(vahy,2);
+F= (vahy*triu(ones(N,N),0));
+u=rand(1,N)'; 
+index=min(N-sum((ones(1,N)'*F-u*ones(1,N))>0,2)+1,N); 
+x_0=x_1(:,index);
+end
Index: /applications/dual/SIDP/smc29/kalman_filter.m
===================================================================
--- /applications/dual/SIDP/smc29/kalman_filter.m (revision 1255)
+++ /applications/dual/SIDP/smc29/kalman_filter.m (revision 1255)
@@ -0,0 +1,15 @@
+function H=kalman_filter(H0,u,system)
+H=H0;
+A=[system.a 0 system.b*sin(H(4,1)) system.b*H(3,1)*cos(H(4,1)) 
+   0 system.a -system.b*cos(H(4,1)) system.b*H(3,1)*sin(H(4,1)) 
+   -system.e*sin(H(4,1)) system.e*cos(H(4,1)) system.d -system.e*(H(2,1)*sin(H(4,1)) +H(1,1)*cos(H(4,1)))
+   0 0 system.deltat 1];
+
+P=A*H(:,2:5)*A'+system.Q;
+R=system.C*P*system.C'+system.R;
+K=P*system.C'/(R);
+predikce=model(H(:,1),u,system);
+H(:,1)=predikce+K*(system.pozorovani-predikce([1 2]));
+H(:,2:5)=P-P*system.C'/R*system.C*P;
+end
+
Index: /applications/dual/SIDP/smc29/kresli.m
===================================================================
--- /applications/dual/SIDP/smc29/kresli.m (revision 1255)
+++ /applications/dual/SIDP/smc29/kresli.m (revision 1255)
@@ -0,0 +1,10 @@
+function kresli(stav,hranice)
+for i=1:size(stav,1)/3
+    figure
+    a=i;
+    plot(stav(a,:)); hold on; plot(stav(a+size(stav,1)/3,:),'g');plot(stav(a+8,:),'r');
+    plot(hranice(a,:),'g.-');   
+    plot(hranice(a+size(stav,1)/3,:),'g.-');    
+
+end
+end
Index: /applications/dual/SIDP/smc29/model_simple.m
===================================================================
--- /applications/dual/SIDP/smc29/model_simple.m (revision 1255)
+++ /applications/dual/SIDP/smc29/model_simple.m (revision 1255)
@@ -0,0 +1,4 @@
+function x_1=model_simple(x_0,system)
+eps=0.01;
+x_1=sin(x_0)./(x_0.^2+eps)+system.sigma*rand(size(x_0));
+end
Index: /applications/dual/SIDP/smc29/porovnani.m
===================================================================
--- /applications/dual/SIDP/smc29/porovnani.m (revision 1255)
+++ /applications/dual/SIDP/smc29/porovnani.m (revision 1255)
@@ -0,0 +1,10 @@
+function porovnani(x_1_all,vahy,x_all)
+for i=1:size(x_all,1)
+    figure
+    x_1=x_1_all(i,:);    
+    x=x_all(i);
+    u=[x_1' vahy'];
+    u=sortrows(u,1)';
+    plot(u(1,:),u(2,:),'.'); hold on; plot(x+0*(1:10),max(vahy)*(0:9)/9); hold off
+end
+end
Index: /applications/dual/SIDP/smc29/sis.asv
===================================================================
--- /applications/dual/SIDP/smc29/sis.asv (revision 1255)
+++ /applications/dual/SIDP/smc29/sis.asv (revision 1255)
@@ -0,0 +1,85 @@
+function sis(N,n)
+
+system.P=[1 3 3];
+system.I=[0.00375 0.5 0.5];
+system.S=zeros(1,3);
+
+system.a=0.9898;
+system.b=0.0072;
+system.c=0.0361;
+system.d=1;
+system.e=0.0149;
+system.deltat=0.000125;
+system.Q=diag([0.0013,0.0013,5*10^(-6),10^(-10)],0);
+system.R=(diag([0.006,0.006],0));
+system.B=[system.c 0
+          0 system.c
+          0 0
+          0 0];
+system.C=[1 0 0 0
+          0 1 0 0];  
+system.x=[-0.01; -0.05;0.1;pi/2];
+system.x_opt=[0; 0; 10.1; 0];
+
+v=0.1;
+system.gamma=[v 0
+              0 v];
+
+N_tresh=N/3;
+          
+apriori=[0 0.2 0 0 0
+         0 0 0.2 0 0
+         0 0 0 0.3 0
+           0 0 0 0 4];
+H=apriori;
+
+x_0=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
+vahy=1/N*ones(1,N);
+
+stav=[]; hranice=stav;
+u=ones(2,n);
+ztrata=0;
+
+N_tresh=N/2;
+
+SIGMA_inv=inv(system.Q)+system.C'/system.R*system.C;
+SIGMA=inv(SIGMA_inv);
+SIGMA_sqrt=sqrt(SIGMA);
+system.Q_inv=inv(system.Q);
+system.R_inv=inv(system.R);
+for i=1:n
+    if mod(i,10)==0
+        ;
+    end
+    u(:,i)=control(system,x_0*vahy',0);
+
+    system.x=model(system.x,u(:,i),system);
+    system.pozorovani=system.C*system.x+sqrt(system.R)*randn(2,1);
+    
+    realizace=model(x_0,u(:,i)*ones(1,N),system);
+    m=SIGMA*(system.Q_inv*realizace+system.C'*system.R_inv*system.pozorovani*ones(1,N));
+    
+    x_1=m+SIGMA_sqrt*randn(size(x_0));
+    
+    reziduum=diag((system.pozorovani*ones(1,N)-system.C*realizace)'/(system.R+system.C*system.Q*system.C')*(system.pozorovani*ones(1,N)-system.C*realizace));
+    p_yx=exp(-reziduum/2);
+    vahy=vahy.*p_yx';
+    vahy=vahy./sum(vahy);
+    
+   %porovnani(x_1,vahy,system.x);
+   
+    if (1/(vahy*vahy')<N_tresh)
+        [x_1 vahy]=my_resample(x_1,vahy);
+       % porovnani(x_1(1,:),vahy,system.x(1));
+    end
+
+    H=kalman_filter(H,u(:,i),system);
+    
+    stav=[stav [system.x; mean(x_1,2);H(:,1)]];
+    hranice=[hranice [min(x_1,[],2); max(x_1,[],2)]];
+    %ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
+    x_0=x_1;
+end
+%plot(abs(stav(1,:)-stav(2,:))); hold on
+kresli(stav,hranice)
+end
Index: /applications/dual/SIDP/smc29/smc_simple.m
===================================================================
--- /applications/dual/SIDP/smc29/smc_simple.m (revision 1255)
+++ /applications/dual/SIDP/smc29/smc_simple.m (revision 1255)
@@ -0,0 +1,41 @@
+function smc_simple(N,n)
+system.sigma=0.01;
+system.ny=0.02;
+
+system.x=0.5;
+apriori=[0 1];
+
+x_1=apriori(:,1)*ones(1,N)+apriori(:,2:end)*(0.5-rand(1,N)); vahy=1/N*ones(1,N);
+N_tresh=N/2;
+stav=[]; hranice=stav;
+
+for i=1:n
+    x_0=my_sample(vahy,x_1);
+    
+    x_1=model_simple(x_0,system);
+    
+    system.x=model_simple(system.x,system);
+    system.pozorovani=system.x+sqrt(system.ny)*randn;
+    
+    reziduum=system.pozorovani*ones(1,N)-x_1;
+    p_v=exp(-(reziduum.^2/system.ny)/2);
+    vahy=p_v/sum(p_v);
+    
+   %porovnani(x_1,vahy,system.x);
+   
+    if (1/(vahy*vahy')<N_tresh)
+        [x_1 vahy]=my_resample(x_1,vahy);
+       % porovnani(x_1(1,:),vahy,system.x(1));
+    end
+
+    %x_0=my_sample(vahy,x_1);    
+    %H=kalman_filter(H,u(:,i),system);
+    
+    stav=[stav [system.x; mean(x_1,2)]];
+    hranice=[hranice [min(x_1,[],2); max(x_1,[],2)]];
+    %ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
+
+end
+%plot(abs(stav(1,:)-stav(2,:))); hold on
+kresli(stav,hranice)
+end
Index: /applications/dual/SIDP/smc29/my_resample.m
===================================================================
--- /applications/dual/SIDP/smc29/my_resample.m (revision 1255)
+++ /applications/dual/SIDP/smc29/my_resample.m (revision 1255)
@@ -0,0 +1,6 @@
+function [x vahy]=my_resample(x,vahy)
+    N=size(x,2);
+    index=my_sample(vahy,1:N);
+    x=x(:,index);
+    vahy=1/N*ones(1,N);
+end
Index: /applications/dual/SIDP/smc29/sis.m
===================================================================
--- /applications/dual/SIDP/smc29/sis.m (revision 1255)
+++ /applications/dual/SIDP/smc29/sis.m (revision 1255)
@@ -0,0 +1,82 @@
+function sis(N,n)
+
+system.P=[1 3 3];
+system.I=[0.00375 0.5 0.5];
+system.S=zeros(1,3);
+
+system.a=0.9898;
+system.b=0.0072;
+system.c=0.0361;
+system.d=1;
+system.e=0.0149;
+system.deltat=0.000125;
+system.Q=diag([0.0013,0.0013,5*10^(-6),10^(-10)],0);
+system.R=(diag([0.0006,0.0006],0));
+system.B=[system.c 0
+          0 system.c
+          0 0
+          0 0];
+system.C=[1 0 0 0
+          0 1 0 0];  
+system.x=[-0.01; -0.1;-0.1;pi/2];
+system.x_opt=[0; 0; 10.1; 0];
+
+v=0.1;
+system.gamma=[v 0
+              0 v];
+        
+apriori=[0 0.2 0 0 0
+         0 0 0.2 0 0
+         0 0 0 0.3 0
+           0 0 0 0 4];
+H=apriori;
+
+x_0=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
+vahy=1/N*ones(1,N);
+
+stav=[]; hranice=stav;
+u=ones(2,n);
+ztrata=0;
+
+N_tresh=N/100;
+
+SIGMA_inv=inv(system.Q)+system.C'/system.R*system.C;
+SIGMA=inv(SIGMA_inv);
+SIGMA_sqrt=sqrt(SIGMA);
+system.Q_inv=inv(system.Q);
+system.R_inv=inv(system.R);
+kov_inv=inv(system.R+system.C*system.Q*system.C');
+for i=1:n
+    u(:,i)=control(system,x_0*vahy',0);
+
+    system.x=model(system.x,u(:,i),system);
+    system.pozorovani=system.C*system.x+sqrt(system.R)*randn(2,1);
+    
+    realizace=model(x_0,u(:,i)*ones(1,N),system);
+    m=SIGMA*(system.Q_inv*realizace+system.C'*system.R_inv*system.pozorovani*ones(1,N));
+    
+    x_1=m+SIGMA_sqrt*randn(size(x_0));
+    
+    reziduum=system.pozorovani*ones(1,N)-system.C*realizace;
+    reziduum=sum(kov_inv*reziduum.^2,1);
+    p_yx=exp(-reziduum/2);
+    vahy=vahy.*p_yx;
+    vahy=vahy./sum(vahy);
+    
+   %porovnani(x_1,vahy,system.x);
+   
+    if (1/(vahy*vahy')<N_tresh)
+        [x_1 vahy]=my_resample(x_1,vahy);
+       % porovnani(x_1(1,:),vahy,system.x(1));
+    end
+
+    H=kalman_filter(H,u(:,i),system);
+    
+    stav=[stav [system.x; mean(x_1,2);H(:,1)]];
+    hranice=[hranice [min(x_1,[],2); max(x_1,[],2)]];
+    %ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
+    x_0=x_1;
+end
+%plot(abs(stav(1,:)-stav(2,:))); hold on
+kresli(stav,hranice)
+end
Index: /applications/dual/SIDP/smc29/PI_controler.m
===================================================================
--- /applications/dual/SIDP/smc29/PI_controler.m (revision 1255)
+++ /applications/dual/SIDP/smc29/PI_controler.m (revision 1255)
@@ -0,0 +1,5 @@
+function [x system]=PI_controler(eps,system,co)
+S=system.S(co);
+x=system.P(co)*eps+system.I(co)*(S+eps);
+system.S(co)=S+eps;
+end
Index: /applications/dual/SIDP/smc29/smc_motor.asv
===================================================================
--- /applications/dual/SIDP/smc29/smc_motor.asv (revision 1255)
+++ /applications/dual/SIDP/smc29/smc_motor.asv (revision 1255)
@@ -0,0 +1,91 @@
+function smc_motor(N,n)
+system.P=[1 3 3];
+system.I=[0.00375 0.5 0.5];
+system.S=zeros(1,3);
+
+system.a=0.9898;
+system.b=0.0072;
+system.c=0.0361;
+system.d=1;
+system.e=0.0149;
+system.deltat=0.000125;
+system.Q=diag([0.0013,0.0013,5*10^(-3),10^(-5)],0);
+system.R=(diag([0.006,0.006],0));
+system.B=[system.c 0
+          0 system.c
+          0 0
+          0 0];
+system.C=[1 0 0 0
+          0 1 0 0];  
+system.sigma_v=0.1;
+system.x=[-0.05; -0.05;0.01;pi/4];
+system.x_opt=[0; 0; 10.1; 0];
+
+system.ksi=[0 0 0 0
+            0 0 0 0
+            0 0 1 0
+            0 0 0 0];
+          v=0.1;
+system.gamma=[v 0
+              0 v];
+
+N_tresh=N/2;
+          
+apriori=[0 0.2 0 0 0
+         0 0 0.2 0 0
+         0 0 0 0.3 0
+           0 0 0 0 2];
+H=apriori;
+
+x_1=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
+vahy=1/N*ones(1,N);
+
+stav=[];
+hranice=stav;
+u=ones(2,n);
+ztrata=0;
+
+A=diag(sqrt(system.C'*system.R*system.R*system.C+system.Q*system.Q));
+
+for i=1:n
+    x_0=my_sample(vahy,x_1);
+
+    u(:,i)=control(system,x_0*vahy',0);
+    u(:,1:10)=[ones(1,10);-ones(1,10)];
+    
+    x_1=model(x_0,u(:,i)*ones(1,N),system);
+    
+    system.x=model(system.x,u(:,i),system);
+    system.pozorovani=system.x([1 2])+sqrt(system.R)*randn(2,1);
+    
+    reziduum=system.pozorovani*ones(1,N)-x_1([1 2],:);
+    p_v=exp(-sum(reziduum.^2/system.R(1,1))/2);
+    S=sum(p_v);
+    %if (S<10^-10)
+        %apriori=[mean(x_1,2) diag(max(x_1,[],2)-min(x_1,[],2))];
+     %   x_1=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
+     %   vahy=1/N*ones(1,N);
+    %else
+        vahy=p_v./S;
+   % end
+        
+    %porovnani(x_1,vahy,system.x);
+
+    if (1/(vahy*vahy')<N_tresh)
+   %     [x_1 vahy]=my_resample(x_1,vahy);
+       % porovnani(x_1(1,:),vahy,system.x(1));
+
+    end
+
+    %x_0=my_sample(vahy,x_1);    
+    H=kalman_filter(H,u(:,i),system);
+    
+    stav=[stav [system.x; x_1*vahy';H(:,1)]];
+    hranice=[hranice [min(x_1,[],2); max(x_1,[],2)]];
+    
+    %ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
+
+end
+%plot(abs(stav(1,:)-stav(2,:))); hold on
+kresli(stav,hranice)
+end
Index: /applications/dual/SIDP/smc29/control.m
===================================================================
--- /applications/dual/SIDP/smc29/control.m (revision 1255)
+++ /applications/dual/SIDP/smc29/control.m (revision 1255)
@@ -0,0 +1,30 @@
+function [best_control system]=control(system,x_hat,dual)
+omegar=system.x_opt(3);
+
+i_d=x_hat(1)*cos(x_hat(4))+x_hat(2)*sin(x_hat(4));
+i_q=x_hat(2)*cos(x_hat(4))-x_hat(1)*sin(x_hat(4));
+
+[i_q_bar system]=PI_controler(omegar-x_hat(3),system,1);
+[u_d system]=PI_controler(-i_d,system,2);
+[u_q system]=PI_controler(i_q_bar-i_q,system,3);
+
+u_d=u_d-0.003465*x_hat(3)*i_q_bar;
+u_q=u_q+0.1989*x_hat(3);
+
+U=max(sqrt(u_d^2+u_q^2)+dual,0);
+fi=atan(u_q/u_d)+x_hat(4);
+if u_d<0
+    fi=fi+pi;
+end
+
+best_control(1,1) = U*cos(fi);
+best_control(2,1) = U*sin(fi);
+
+%best_control(1,1) = 1;
+%best_control(2,1) = 1;
+
+n=best_control'*best_control;
+if n>100
+    best_control=best_control*sqrt(100/n);
+end
+end
Index: /applications/dual/SIDP/smc29/model.m
===================================================================
--- /applications/dual/SIDP/smc29/model.m (revision 1255)
+++ /applications/dual/SIDP/smc29/model.m (revision 1255)
@@ -0,0 +1,6 @@
+function x1=model(x,u,system)
+x1=[system.a*x(1,:)+ system.b*x(3,:).*sin(x(4,:))+system.c*u(1,:);
+    system.a*x(2,:)- system.b*x(3,:).*cos(x(4,:))+system.c*u(2,:);
+    system.d*x(3,:)+ system.e*x(2,:).*cos(x(4,:))-system.e*x(1,:).*sin(x(4,:));
+    x(4,:)+x(3,:)*system.deltat]+system.Q*rand(size(x));
+end
Index: /applications/dual/SIDP/smc29/smc_motor.m
===================================================================
--- /applications/dual/SIDP/smc29/smc_motor.m (revision 1255)
+++ /applications/dual/SIDP/smc29/smc_motor.m (revision 1255)
@@ -0,0 +1,91 @@
+function smc_motor(N,n)
+system.P=[1 3 3];
+system.I=[0.00375 0.5 0.5];
+system.S=zeros(1,3);
+
+system.a=0.9898;
+system.b=0.0072;
+system.c=0.0361;
+system.d=1;
+system.e=0.0149;
+system.deltat=0.000125;
+system.Q=diag([0.0013,0.0013,5*10^(-3),10^(-5)],0);
+system.R=(diag([0.006,0.006],0));
+system.B=[system.c 0
+          0 system.c
+          0 0
+          0 0];
+system.C=[1 0 0 0
+          0 1 0 0];  
+system.sigma_v=0.1;
+system.x=[-0.05; -0.05;0.01;pi/4];
+system.x_opt=[0; 0; 10.1; 0];
+
+system.ksi=[0 0 0 0
+            0 0 0 0
+            0 0 1 0
+            0 0 0 0];
+          v=0.1;
+system.gamma=[v 0
+              0 v];
+
+N_tresh=N/2;
+          
+apriori=[0 0.2 0 0 0
+         0 0 0.2 0 0
+         0 0 0 0.3 0
+           0 0 0 0 2];
+H=apriori;
+
+x_1=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
+vahy=1/N*ones(1,N);
+
+stav=[];
+hranice=stav;
+u=ones(2,n);
+ztrata=0;
+
+A=diag(sqrt(system.C'*system.R*system.R*system.C+system.Q*system.Q));
+
+for i=1:n
+    x_0=my_sample(vahy,x_1);
+
+    u(:,i)=control(system,x_0*vahy',0);
+    u(:,1:10)=[ones(1,10);-ones(1,10)];
+    
+    x_1=model(x_0,u(:,i)*ones(1,N),system);
+    
+    system.x=model(system.x,u(:,i),system);
+    system.pozorovani=system.x([1 2])+sqrt(system.R)*randn(2,1);
+    
+    reziduum=system.pozorovani*ones(1,N)-x_1([1 2],:);
+    p_v=exp(-sum(reziduum.^2/system.R(1,1))/2);
+    S=sum(p_v);
+    %if (S<10^-10)
+        %apriori=[mean(x_1,2) diag(max(x_1,[],2)-min(x_1,[],2))];
+     %   x_1=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
+     %   vahy=1/N*ones(1,N);
+    %else
+        vahy=p_v./S;
+   % end
+        
+    %porovnani(x_1,vahy,system.x);
+
+    if (1/(vahy*vahy')<N_tresh)
+   %     [x_1 vahy]=my_resample(x_1,vahy);
+       % porovnani(x_1(1,:),vahy,system.x(1));
+
+    end
+
+    %x_0=my_sample(vahy,x_1);    
+    H=kalman_filter(H,u(:,i),system);
+    
+    stav=[stav [system.x; x_1*vahy';H(:,1)]];
+    hranice=[hranice [min(x_1,[],2); max(x_1,[],2)]];
+    
+    %ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
+
+end
+%plot(abs(stav(1,:)-stav(2,:))); hold on
+kresli(stav,hranice)
+end
