root/applications/dual/SIDP/smc29/smc_motor.asv @ 1300

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