[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 | |
---|
[893] | 8 | N = 100; %vzorky |
---|
| 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 |
---|
[893] | 38 | v = 0.00001; |
---|
[890] | 39 | w = 1; |
---|
| 40 | |
---|
| 41 | %matice modelu |
---|
| 42 | A = [a 0 0 0;... |
---|
| 43 | 0 a 0 0;... |
---|
| 44 | 0 0 d 0;... |
---|
| 45 | 0 0 DELTAt 1]; |
---|
| 46 | |
---|
[893] | 47 | B = [c 0;... |
---|
[890] | 48 | 0 c;... |
---|
| 49 | 0 0;... |
---|
| 50 | 0 0]; |
---|
| 51 | |
---|
| 52 | % C = [1 0 0 0;... |
---|
| 53 | % 0 1 0 0]; |
---|
| 54 | |
---|
| 55 | X = [0 0 0 0;... |
---|
| 56 | 0 0 0 0;... |
---|
| 57 | 0 0 w 0;... |
---|
| 58 | 0 0 0 0]; |
---|
| 59 | |
---|
| 60 | Y = [v 0;... |
---|
| 61 | 0 v]; |
---|
| 62 | |
---|
| 63 | %pocatecni nastaveni |
---|
| 64 | Q = diag([0.0013, 0.0013, 5e-6, 1e-10]); |
---|
| 65 | R = diag([0.0006, 0.0006]); |
---|
| 66 | |
---|
| 67 | x0 = [0 0 1 pi/2]; |
---|
| 68 | P = diag([0.01, 0.01, 0.01, 0.01]); |
---|
| 69 | |
---|
| 70 | %globalni promenne |
---|
[893] | 71 | u = zeros(2, Kt); |
---|
| 72 | xs = zeros(4, Kt); |
---|
| 73 | xn = zeros(4, Kt, N); |
---|
[890] | 74 | |
---|
| 75 | S = zeros(4, 4, K); |
---|
[893] | 76 | L = zeros(2, 4, Kt); |
---|
[890] | 77 | |
---|
| 78 | %zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s |
---|
| 79 | %rozptylem |
---|
[893] | 80 | sum = 1;%0.01; |
---|
| 81 | sumsim = 1;%0.01; |
---|
[890] | 82 | neznalost = 1; |
---|
| 83 | |
---|
| 84 | tic |
---|
| 85 | |
---|
| 86 | %hlavni iteracni smycka |
---|
| 87 | for iterace = 1:It, |
---|
| 88 | %generovani stavu |
---|
| 89 | for n = 1:N, |
---|
[893] | 90 | xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); |
---|
| 91 | for k = 1:Kt-1, |
---|
| 92 | tu = L(:, :, k)*(xn(:, k, n) - [0 0 OMEGAt 0]'); |
---|
| 93 | 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(); |
---|
| 94 | 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(); |
---|
| 95 | 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(); |
---|
| 96 | xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); |
---|
| 97 | % 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(); |
---|
| 98 | % 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(); |
---|
| 99 | % 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(); |
---|
| 100 | % xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); |
---|
[890] | 101 | end |
---|
| 102 | end |
---|
| 103 | |
---|
| 104 | %prumerny stav |
---|
| 105 | xs = mean(xn, 3); |
---|
| 106 | |
---|
| 107 | %napocteni S a L |
---|
[893] | 108 | for kt = 1:Kt-1, |
---|
| 109 | % receding horizon |
---|
| 110 | if((K-1+kt)<Kt) |
---|
| 111 | S(:, :, K) = X; |
---|
| 112 | for k = K-1+kt-1:-1:1+kt-1, |
---|
| 113 | A(3, 1) = -e*sin(xs(4, k)); |
---|
| 114 | A(3, 2) = e*cos(xs(4, k)); |
---|
| 115 | A(1, 3) = b*sin(xs(4, k)); |
---|
| 116 | A(2, 3) = -b*cos(xs(4, k)); |
---|
| 117 | A(1, 4) = b*(xs(3, k))*cos(xs(4, k)); |
---|
| 118 | A(2, 4) = b*(xs(3, k))*sin(xs(4, k)); |
---|
| 119 | % A(1, 4) = b*(xs(3, k)+OMEGAt)*cos(xs(4, k)); |
---|
| 120 | % A(2, 4) = b*(xs(3, k)+OMEGAt)*sin(xs(4, k)); |
---|
| 121 | A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k)))); |
---|
| 122 | 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; |
---|
| 123 | end |
---|
| 124 | L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; |
---|
| 125 | else |
---|
| 126 | L(:, :, kt) = L(:, :, kt-1); %kopiruje poslednich K kroku z Kt kde to nejde na K predpocitat |
---|
| 127 | end |
---|
| 128 | |
---|
| 129 | end |
---|
[890] | 130 | end |
---|
| 131 | toc |
---|
| 132 | |
---|
| 133 | %vysledky |
---|
| 134 | clf |
---|
| 135 | subplot(2, 3, 3); |
---|
| 136 | hold all |
---|
[893] | 137 | plot(1:Kt, OMEGAt*ones(1,Kt)); |
---|
| 138 | % L(:,:,1) |
---|
[890] | 139 | for n = 1:N, |
---|
[893] | 140 | xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); |
---|
| 141 | for k = 1:Kt-1, |
---|
| 142 | tu = L(:, :, 1)*(xn(:, k, n) - [0 0 OMEGAt 0]'); |
---|
[890] | 143 | 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(); |
---|
| 144 | 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(); |
---|
| 145 | 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(); |
---|
| 146 | xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); |
---|
[893] | 147 | % 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(); |
---|
| 148 | % 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(); |
---|
| 149 | % 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(); |
---|
| 150 | % xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); |
---|
[890] | 151 | |
---|
[893] | 152 | u(:, k) = tu; |
---|
[890] | 153 | end |
---|
| 154 | |
---|
[893] | 155 | % xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt); |
---|
[890] | 156 | |
---|
| 157 | subplot(2, 3, 1); |
---|
| 158 | hold all |
---|
[893] | 159 | plot(1:Kt, xn(1, :, n)); |
---|
[890] | 160 | title('i_{\alpha}'); |
---|
| 161 | subplot(2, 3, 2); |
---|
| 162 | hold all |
---|
[893] | 163 | plot(1:Kt, xn(2, :, n)); |
---|
[890] | 164 | title('i_{\beta}'); |
---|
| 165 | subplot(2, 3, 3); |
---|
| 166 | hold all |
---|
[893] | 167 | plot(1:Kt, xn(3, :, n)); |
---|
[890] | 168 | title('\omega'); |
---|
| 169 | subplot(2, 3, 4); |
---|
| 170 | hold all |
---|
[893] | 171 | plot(1:Kt, xn(4, :, n)); |
---|
[890] | 172 | title('\theta'); |
---|
| 173 | subplot(2, 3, 5); |
---|
| 174 | hold all |
---|
[893] | 175 | plot(1:Kt, u(1, :)); |
---|
[890] | 176 | title('u_{\alpha}'); |
---|
| 177 | subplot(2, 3, 6); |
---|
| 178 | hold all |
---|
[893] | 179 | plot(1:Kt, u(2, :)); |
---|
[890] | 180 | title('u_{\beta}'); |
---|
| 181 | end |
---|
| 182 | |
---|
| 183 | |
---|
| 184 | end |
---|