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