[890] | 1 | function pmsm_lqg |
---|
| 2 | % rizeni pmsm motoru - jednoduchy lqg algoritmus |
---|
| 3 | |
---|
| 4 | %nastaveni algortimu |
---|
[893] | 5 | K = 10; %casy |
---|
| 6 | Kt = 100; %test casy |
---|
[890] | 7 | |
---|
[898] | 8 | N = 50; %vzorky |
---|
[893] | 9 | It = 1; %iterace |
---|
[890] | 10 | |
---|
| 11 | |
---|
| 12 | %konstanty modelu |
---|
| 13 | DELTAt = 0.000125; |
---|
| 14 | |
---|
| 15 | cRs = 0.28; |
---|
| 16 | cLs = 0.003465; |
---|
| 17 | cPSIpm = 0.1989; |
---|
| 18 | ckp = 1.5; |
---|
| 19 | cp = 4.0; |
---|
| 20 | cJ = 0.04; |
---|
| 21 | cB = 0; |
---|
| 22 | |
---|
| 23 | % a = 0.9898; |
---|
| 24 | % b = 0.0072; |
---|
| 25 | % c = 0.0361; |
---|
| 26 | % d = 1; |
---|
| 27 | % e = 0.0149; |
---|
| 28 | |
---|
| 29 | a = 1 - DELTAt*cRs/cLs; |
---|
| 30 | b = DELTAt*cPSIpm/cLs; |
---|
| 31 | c = DELTAt/cLs; |
---|
| 32 | d = 1 - DELTAt*cB/cJ; |
---|
| 33 | e = DELTAt*ckp*cp*cp*cPSIpm/cJ; |
---|
| 34 | |
---|
| 35 | OMEGAt = 1.0015; |
---|
| 36 | |
---|
| 37 | %penalizace vstupu a rizeni |
---|
[898] | 38 | v = 0.000001;%0.000001; |
---|
[890] | 39 | w = 1; |
---|
| 40 | |
---|
| 41 | %matice modelu |
---|
[898] | 42 | A = [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]; |
---|
[890] | 47 | |
---|
[893] | 48 | B = [c 0;... |
---|
[898] | 49 | 0 c;... |
---|
| 50 | 0 0;... |
---|
| 51 | 0 0;... |
---|
| 52 | 0 0]; |
---|
[890] | 53 | |
---|
| 54 | % C = [1 0 0 0;... |
---|
| 55 | % 0 1 0 0]; |
---|
| 56 | |
---|
[898] | 57 | X = [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]; |
---|
[890] | 62 | |
---|
| 63 | Y = [v 0;... |
---|
| 64 | 0 v]; |
---|
| 65 | |
---|
| 66 | %pocatecni nastaveni |
---|
| 67 | Q = diag([0.0013, 0.0013, 5e-6, 1e-10]); |
---|
| 68 | R = diag([0.0006, 0.0006]); |
---|
| 69 | |
---|
[898] | 70 | x0 = [0 0 1-OMEGAt pi/2 OMEGAt]; |
---|
| 71 | P = diag([0.01, 0.01, 0.01, 0.01, 0]); |
---|
[890] | 72 | |
---|
| 73 | %globalni promenne |
---|
[898] | 74 | u = zeros(2, Kt+K); |
---|
| 75 | xs = zeros(5, Kt+K); |
---|
| 76 | xn = zeros(5, Kt+K, N); |
---|
[890] | 77 | |
---|
[898] | 78 | S = zeros(5, 5, K); |
---|
| 79 | L = zeros(2, 5, Kt+K); |
---|
[890] | 80 | |
---|
| 81 | %zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s |
---|
| 82 | %rozptylem |
---|
[893] | 83 | sum = 1;%0.01; |
---|
| 84 | sumsim = 1;%0.01; |
---|
[890] | 85 | neznalost = 1; |
---|
| 86 | |
---|
| 87 | |
---|
| 88 | |
---|
[898] | 89 | % vycisti kreslici okno |
---|
[890] | 90 | clf |
---|
| 91 | subplot(2, 3, 3); |
---|
| 92 | hold all |
---|
[893] | 93 | plot(1:Kt, OMEGAt*ones(1,Kt)); |
---|
[898] | 94 | |
---|
| 95 | tic |
---|
| 96 | |
---|
| 97 | % vzorky stavu |
---|
| 98 | for n = 1:N, |
---|
| 99 | L = zeros(2, 5, Kt+K); |
---|
| 100 | %iterace |
---|
| 101 | % for iterace = 1:It, |
---|
| 102 | x00 = x0' + neznalost*sqrt(P)*randn(5,1); |
---|
| 103 | %testovaci casy |
---|
| 104 | for kt = 1:Kt, |
---|
| 105 | %generovani stavu - jen pro horizont |
---|
| 106 | xn(:, 1, n) = x00; |
---|
| 107 | for k = 1:kt+K-1, |
---|
| 108 | tu = L(:, :, k)*(xn(:, k, n)); |
---|
| 109 | 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(); |
---|
| 110 | 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(); |
---|
| 111 | 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(); |
---|
| 112 | xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); |
---|
| 113 | xn(5, k+1, n) = xn(5, k, n); |
---|
| 114 | end |
---|
| 115 | %prumerny stav |
---|
| 116 | xs = xn(:, :, n);%mean(xn, 3); |
---|
| 117 | |
---|
| 118 | %receding horizon |
---|
| 119 | S(:, :, K) = X; |
---|
| 120 | for k = K:-1:2, |
---|
| 121 | A(3, 1) = -e*sin(xs(4, k+kt-1)); |
---|
| 122 | A(3, 2) = e*cos(xs(4, k+kt-1)); |
---|
| 123 | A(1, 3) = b*sin(xs(4, k+kt-1)); |
---|
| 124 | A(2, 3) = -b*cos(xs(4, k+kt-1)); |
---|
| 125 | A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1)); |
---|
| 126 | A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1)); |
---|
| 127 | 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)))); |
---|
| 128 | A(1, 5) = b*sin(xs(4, k+kt-1)); |
---|
| 129 | A(2, 5) = -b*cos(xs(4, k+kt-1)); |
---|
| 130 | S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X; |
---|
| 131 | end |
---|
| 132 | L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; |
---|
| 133 | %spocital kt-te rizeni a vsechna dalsi nahradi jim |
---|
| 134 | for k = kt+1:kt+K-1, |
---|
| 135 | L(:, :, k) = L(:, :, kt); |
---|
| 136 | end |
---|
| 137 | end |
---|
[890] | 138 | |
---|
[898] | 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)*(xn(:, 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 | end |
---|
| 150 | |
---|
| 151 | |
---|
| 152 | %vykresleni |
---|
[890] | 153 | subplot(2, 3, 1); |
---|
| 154 | hold all |
---|
[898] | 155 | plot(1:Kt, xn(1, 1:Kt, n)); |
---|
[890] | 156 | title('i_{\alpha}'); |
---|
| 157 | subplot(2, 3, 2); |
---|
| 158 | hold all |
---|
[898] | 159 | plot(1:Kt, xn(2, 1:Kt, n)); |
---|
[890] | 160 | title('i_{\beta}'); |
---|
| 161 | subplot(2, 3, 3); |
---|
| 162 | hold all |
---|
[898] | 163 | plot(1:Kt, xn(3, 1:Kt, n) + xn(5, 1:Kt, n)); |
---|
[890] | 164 | title('\omega'); |
---|
| 165 | subplot(2, 3, 4); |
---|
| 166 | hold all |
---|
[898] | 167 | plot(1:Kt, xn(4, 1:Kt, n)); |
---|
[890] | 168 | title('\theta'); |
---|
| 169 | subplot(2, 3, 5); |
---|
| 170 | hold all |
---|
[898] | 171 | plot(1:Kt, u(1, 1:Kt)); |
---|
[890] | 172 | title('u_{\alpha}'); |
---|
| 173 | subplot(2, 3, 6); |
---|
| 174 | hold all |
---|
[898] | 175 | plot(1:Kt, u(2, 1:Kt)); |
---|
| 176 | title('u_{\beta}'); |
---|
| 177 | end |
---|
[890] | 178 | |
---|
[898] | 179 | toc |
---|
[890] | 180 | |
---|
[898] | 181 | % %hlavni iteracni smycka |
---|
| 182 | % for iterace = 1:It, |
---|
| 183 | % %generovani stavu |
---|
| 184 | % for n = 1:N, |
---|
| 185 | % xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); |
---|
| 186 | % for k = 1:Kt-1, |
---|
| 187 | % tu = L(:, :, k)*(xn(:, k, n) - [0 0 OMEGAt 0]'); |
---|
| 188 | % xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, k, n)*sin(xn(4, k, n)) + c*tu(1) + sumsim*sqrt(Q(1, 1))*randn(); |
---|
| 189 | % xn(2, k+1, n) = a*xn(2, k, n) - b*xn(3, k, n)*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn(); |
---|
| 190 | % xn(3, k+1, n) = d*xn(3, 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(); |
---|
| 191 | % xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); |
---|
| 192 | % % xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n)+OMEGAt)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn(); |
---|
| 193 | % % xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n)+OMEGAt)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn(); |
---|
| 194 | % % xn(3, k+1, n) = -OMEGAt + d*(xn(3, k, n)+OMEGAt) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sum*sqrt(Q(3, 3))*randn(); |
---|
| 195 | % % xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); |
---|
| 196 | % end |
---|
| 197 | % end |
---|
| 198 | % |
---|
| 199 | % %prumerny stav |
---|
| 200 | % xs = mean(xn, 3); |
---|
| 201 | % |
---|
| 202 | % %napocteni S a L |
---|
| 203 | % for kt = 1:Kt-1, |
---|
| 204 | % % receding horizon |
---|
| 205 | % if((K-1+kt)<Kt) |
---|
| 206 | % S(:, :, K) = X; |
---|
| 207 | % for k = K-1+kt-1:-1:1+kt-1, |
---|
| 208 | % A(3, 1) = -e*sin(xs(4, k)); |
---|
| 209 | % A(3, 2) = e*cos(xs(4, k)); |
---|
| 210 | % A(1, 3) = b*sin(xs(4, k)); |
---|
| 211 | % A(2, 3) = -b*cos(xs(4, k)); |
---|
| 212 | % A(1, 4) = b*(xs(3, k))*cos(xs(4, k)); |
---|
| 213 | % A(2, 4) = b*(xs(3, k))*sin(xs(4, k)); |
---|
| 214 | % % A(1, 4) = b*(xs(3, k)+OMEGAt)*cos(xs(4, k)); |
---|
| 215 | % % A(2, 4) = b*(xs(3, k)+OMEGAt)*sin(xs(4, k)); |
---|
| 216 | % A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k)))); |
---|
| 217 | % S(:, :, k-kt+1) = A'*(S(:, :, k-kt+2) - S(:, :, k-kt+2)*B*inv(B'*S(:, :, k-kt+2)*B + Y)*B'*S(:, :, k-kt+2))*A + X; |
---|
| 218 | % end |
---|
| 219 | % L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; |
---|
| 220 | % else |
---|
| 221 | % L(:, :, kt) = L(:, :, kt-1); %kopiruje poslednich K kroku z Kt kde to nejde na K predpocitat |
---|
| 222 | % end |
---|
| 223 | % |
---|
| 224 | % end |
---|
| 225 | % end |
---|
| 226 | % toc |
---|
| 227 | % |
---|
| 228 | % %vysledky |
---|
| 229 | % clf |
---|
| 230 | % subplot(2, 3, 3); |
---|
| 231 | % hold all |
---|
| 232 | % plot(1:Kt, OMEGAt*ones(1,Kt)); |
---|
| 233 | % % L(:,:,1) |
---|
| 234 | % for n = 1:N, |
---|
| 235 | % xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); |
---|
| 236 | % for k = 1:Kt-1, |
---|
| 237 | % tu = L(:, :, 1)*(xn(:, k, n) - [0 0 OMEGAt 0]'); |
---|
| 238 | % xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, k, n)*sin(xn(4, k, n)) + c*tu(1) + sumsim*sqrt(Q(1, 1))*randn(); |
---|
| 239 | % xn(2, k+1, n) = a*xn(2, k, n) - b*xn(3, k, n)*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn(); |
---|
| 240 | % xn(3, k+1, n) = d*xn(3, 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(); |
---|
| 241 | % xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); |
---|
| 242 | % % xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n)+OMEGAt)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn(); |
---|
| 243 | % % xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n)+OMEGAt)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn(); |
---|
| 244 | % % xn(3, k+1, n) = -OMEGAt + d*(xn(3, k, n)+OMEGAt) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sum*sqrt(Q(3, 3))*randn(); |
---|
| 245 | % % xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); |
---|
| 246 | % |
---|
| 247 | % u(:, k) = tu; |
---|
| 248 | % end |
---|
| 249 | % |
---|
| 250 | % % xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt); |
---|
| 251 | % |
---|
| 252 | % subplot(2, 3, 1); |
---|
| 253 | % hold all |
---|
| 254 | % plot(1:Kt, xn(1, :, n)); |
---|
| 255 | % title('i_{\alpha}'); |
---|
| 256 | % subplot(2, 3, 2); |
---|
| 257 | % hold all |
---|
| 258 | % plot(1:Kt, xn(2, :, n)); |
---|
| 259 | % title('i_{\beta}'); |
---|
| 260 | % subplot(2, 3, 3); |
---|
| 261 | % hold all |
---|
| 262 | % plot(1:Kt, xn(3, :, n)); |
---|
| 263 | % title('\omega'); |
---|
| 264 | % subplot(2, 3, 4); |
---|
| 265 | % hold all |
---|
| 266 | % plot(1:Kt, xn(4, :, n)); |
---|
| 267 | % title('\theta'); |
---|
| 268 | % subplot(2, 3, 5); |
---|
| 269 | % hold all |
---|
| 270 | % plot(1:Kt, u(1, :)); |
---|
| 271 | % title('u_{\alpha}'); |
---|
| 272 | % subplot(2, 3, 6); |
---|
| 273 | % hold all |
---|
| 274 | % plot(1:Kt, u(2, :)); |
---|
| 275 | % title('u_{\beta}'); |
---|
| 276 | % end |
---|
| 277 | |
---|
| 278 | |
---|
[890] | 279 | end |
---|