root/applications/dual/IterativeLocal/pmsm_lqg.m @ 913

Revision 913, 5.2 kB (checked in by smidl, 14 years ago)

upravy

Line 
1function pmsm_lqg
2% rizeni pmsm motoru - jednoduchy lqg algoritmus
3
4%nastaveni algortimu
5K = 10; %casy
6Kt = 100; %test casy
7
8N = 50; %vzorky
9It = 1; %iterace
10
11
12%konstanty modelu
13DELTAt = 0.000125;
14
15cRs = 0.28;
16cLs = 0.003465;
17cPSIpm = 0.1989;
18ckp = 1.5;
19cp = 4.0;
20cJ = 0.04;
21cB = 0;
22
23% a = 0.9898;
24% b = 0.0072;
25% c = 0.0361;
26% d = 1;
27% e = 0.0149;
28
29a = 1 - DELTAt*cRs/cLs;
30b = DELTAt*cPSIpm/cLs;
31c = DELTAt/cLs;
32d = 1 - DELTAt*cB/cJ;
33e = DELTAt*ckp*cp*cp*cPSIpm/cJ;
34
35OMEGAt = 2.15;%1.0015;
36
37%penalizace vstupu a rizeni
38v = 0.0001;%0.000001;
39w = 1;
40
41%matice modelu
42A = [a 0 0 0 0;...
43     0 a 0 0 0;...
44     0 0 d 0 (d-1);...
45     0 0 DELTAt 1 DELTAt;...
46     0 0 0 0 1];
47 
48B = [c 0;...
49     0 c;...
50     0 0;...
51     0 0;...
52     0 0];
53 
54% C = [1 0 0 0;...
55%      0 1 0 0];
56 
57X = [0 0 0 0 0;...
58     0 0 0 0 0;...
59     0 0 w 0 0;...
60     0 0 0 0 0;...
61     0 0 0 0 0];
62 
63Y = [v 0;...
64     0 v];
65 
66%pocatecni nastaveni
67Q = diag([0.0013, 0.0013, 5e-6, 1e-10]);
68R = diag([0.0006, 0.0006]);
69
70x0 = [0 0 1.0-OMEGAt pi/2 OMEGAt];
71P = diag([0.01, 0.01, 0.01, 0.5, 0]);
72
73%globalni promenne
74u = zeros(2, Kt+K);
75xs = zeros(5, Kt+K);
76xn = zeros(5, Kt+K, N);
77
78S = zeros(5, 5, K);
79L = zeros(2, 5, Kt+K);
80
81%zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s
82%rozptylem
83% sum = 1;%0.01;
84sumsim = 1;%0.01;
85neznalost = 1;
86
87% vycisti kreslici okno
88    clf
89    subplot(2, 3, 3);
90    hold all
91%    plot(1:Kt, OMEGAt*ones(1,Kt));
92   
93tic
94
95% vzorky stavu
96for n = 1:N,
97    L = zeros(2, 5, Kt+K);
98    %iterace
99    x00 = x0' + neznalost*sqrt(P)*randn(5,1);   
100        %testovaci casy
101        for kt = 1:Kt,
102            %generovani stavu - jen pro horizont
103            for iterace = 1:1,
104                xn(:, 1, n) = x00;
105                for k = 1:kt+K-1,
106                    tu = L(:, :, k)*(xn(:, k, n));
107                    xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n) + xn(5, k, n))*sin(xn(4, k, n)) + c*tu(1);% + sumsim*sqrt(Q(1, 1))*randn();
108                    xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n) + xn(5, k, n))*cos(xn(4, k, n)) + c*tu(2);% + sumsim*sqrt(Q(2, 2))*randn();
109                    xn(3, k+1, n) = d*xn(3, k, n) + (d-1)*xn(5, k, n) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n)));% + sumsim*sqrt(Q(3, 3))*randn();
110                    xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt;% + sumsim*sqrt(Q(4, 4))*randn();
111                    xn(5, k+1, n) = xn(5, k, n);
112                end
113                %prumerny stav
114                xs = xn(:, :, n);%mean(xn, 3);
115
116                %receding horizon
117                S(:, :, K) = X;
118                for k = K:-1:2,
119                    A(3, 1) = -e*sin(xs(4, k+kt-1));
120                    A(3, 2) = e*cos(xs(4, k+kt-1));
121                    A(1, 3) = b*sin(xs(4, k+kt-1));
122                    A(2, 3) = -b*cos(xs(4, k+kt-1));
123                    A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1));
124                    A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1));
125                    A(3, 4) = -e*(xs(2, k+kt-1)*sin(xs(4, k+kt-1) + xs(1,k+kt-1)*cos(xs(4, k+kt-1))));
126                    A(1, 5) = b*sin(xs(4, k+kt-1));
127                    A(2, 5) = -b*cos(xs(4, k+kt-1));
128                    S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X;
129                    L(:, :, kt+k-2) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
130                end
131%                L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
132                %spocital kt-te rizeni a vsechna dalsi nahradi jim
133%                 for k = kt+1:kt+K-1,
134%                     L(:, :, k) = L(:, :, kt);
135%                 end
136            end
137        end       
138       
139%     end
140        %napocte trajektorii pro vykresleni s kompletnim rizenim
141            xn(:, 1, n) = x00;
142            for k = 1:Kt+K-1,               
143               u(:, k) = L(:, :, k)*(x_kalman(:, k, n)); 
144               xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n) + xn(5, k, n))*sin(xn(4, k, n)) + c*u(1, k) + sumsim*sqrt(Q(1, 1))*randn(); 
145               xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n) + xn(5, k, n))*cos(xn(4, k, n)) + c*u(2, k) + sumsim*sqrt(Q(2, 2))*randn();
146               xn(3, k+1, n) = d*xn(3, k, n) + (d-1)*xn(5, k, n) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sumsim*sqrt(Q(3, 3))*randn();
147               xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn();
148               xn(5, k+1, n) = xn(5, k, n);
149               
150               yn()=...;
151               x_kalman = ...;
152            end
153           
154       
155    %vykresleni
156        subplot(2, 3, 1);
157        hold all
158        plot(1:Kt, xn(1, 1:Kt, n));
159        title('i_{\alpha}');
160        subplot(2, 3, 2);
161        hold all
162        plot(1:Kt, xn(2, 1:Kt, n));
163        title('i_{\beta}');
164        subplot(2, 3, 3);
165        hold all
166        plot(1:Kt, xn(3, 1:Kt, n) + xn(5, 1:Kt, n));
167        title('\omega');
168        subplot(2, 3, 4);
169        hold all
170        plot(1:Kt, xn(4, 1:Kt, n));
171        title('\theta');
172        subplot(2, 3, 5);
173        hold all
174        plot(1:Kt, u(1, 1:Kt));
175        title('u_{\alpha}');
176        subplot(2, 3, 6);
177        hold all
178        plot(1:Kt, u(2, 1:Kt));
179        title('u_{\beta}');   
180end
181
182toc
183
184end
Note: See TracBrowser for help on using the browser.