root/applications/dual/SIDP/smc29/sis.asv @ 1296

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