root/applications/dual/SIDP/smc29/smc_motor.m @ 1344

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