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