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

Revision 1255, 2.1 kB (checked in by zimamiro, 14 years ago)
Line 
1function sis(N,n)
2
3system.P=[1 3 3];
4system.I=[0.00375 0.5 0.5];
5system.S=zeros(1,3);
6
7system.a=0.9898;
8system.b=0.0072;
9system.c=0.0361;
10system.d=1;
11system.e=0.0149;
12system.deltat=0.000125;
13system.Q=diag([0.0013,0.0013,5*10^(-6),10^(-10)],0);
14system.R=(diag([0.006,0.006],0));
15system.B=[system.c 0
16          0 system.c
17          0 0
18          0 0];
19system.C=[1 0 0 0
20          0 1 0 0]; 
21system.x=[-0.01; -0.05;0.1;pi/2];
22system.x_opt=[0; 0; 10.1; 0];
23
24v=0.1;
25system.gamma=[v 0
26              0 v];
27
28N_tresh=N/3;
29         
30apriori=[0 0.2 0 0 0
31         0 0 0.2 0 0
32         0 0 0 0.3 0
33           0 0 0 0 4];
34H=apriori;
35
36x_0=apriori(:,1)*ones(1,N)+apriori(:,2:5)*(0.5-rand(4,N));
37vahy=1/N*ones(1,N);
38
39stav=[]; hranice=stav;
40u=ones(2,n);
41ztrata=0;
42
43N_tresh=N/2;
44
45SIGMA_inv=inv(system.Q)+system.C'/system.R*system.C;
46SIGMA=inv(SIGMA_inv);
47SIGMA_sqrt=sqrt(SIGMA);
48system.Q_inv=inv(system.Q);
49system.R_inv=inv(system.R);
50for i=1:n
51    if mod(i,10)==0
52        ;
53    end
54    u(:,i)=control(system,x_0*vahy',0);
55
56    system.x=model(system.x,u(:,i),system);
57    system.pozorovani=system.C*system.x+sqrt(system.R)*randn(2,1);
58   
59    realizace=model(x_0,u(:,i)*ones(1,N),system);
60    m=SIGMA*(system.Q_inv*realizace+system.C'*system.R_inv*system.pozorovani*ones(1,N));
61   
62    x_1=m+SIGMA_sqrt*randn(size(x_0));
63   
64    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));
65    p_yx=exp(-reziduum/2);
66    vahy=vahy.*p_yx';
67    vahy=vahy./sum(vahy);
68   
69   %porovnani(x_1,vahy,system.x);
70   
71    if (1/(vahy*vahy')<N_tresh)
72        [x_1 vahy]=my_resample(x_1,vahy);
73       % porovnani(x_1(1,:),vahy,system.x(1));
74    end
75
76    H=kalman_filter(H,u(:,i),system);
77   
78    stav=[stav [system.x; mean(x_1,2);H(:,1)]];
79    hranice=[hranice [min(x_1,[],2); max(x_1,[],2)]];
80    %ztrata= ztrata+u'*system.gamma*u+(system.x-system.x_opt)'*system.ksi*(system.x-system.x_opt);
81    x_0=x_1;
82end
83%plot(abs(stav(1,:)-stav(2,:))); hold on
84kresli(stav,hranice)
85end
Note: See TracBrowser for help on using the browser.