[1436] | 1 | function [loss] = ukf_comp(T, ref_profile, theta0, simulator, graf) |
---|
| 2 | % clear all; |
---|
| 3 | |
---|
| 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 5 | %%%%%pouziti SIMULATORU |
---|
| 6 | % simulator = 1; |
---|
| 7 | % simulator = 0; |
---|
| 8 | |
---|
| 9 | if(simulator == 1) |
---|
| 10 | sim_param = pmsm_sim; |
---|
| 11 | % sim_param(9) = 0; %vypne dead-time |
---|
| 12 | pmsm_sim(sim_param); |
---|
| 13 | end |
---|
| 14 | %%%%% |
---|
| 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 16 | |
---|
| 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 18 | % parametry stroje |
---|
| 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 20 | % Rs = 0.28; |
---|
| 21 | Ls = 0.003465; |
---|
| 22 | psipm = 0.1989; |
---|
| 23 | %B = 0; |
---|
| 24 | %TL = 0; |
---|
| 25 | % kp = 1.5; |
---|
| 26 | % pp = 4.0; |
---|
| 27 | % J = 0.04; |
---|
| 28 | dt = 0.000125; |
---|
| 29 | |
---|
| 30 | a = 0.9898; |
---|
| 31 | b = 0.0072; |
---|
| 32 | c = 0.0361; |
---|
| 33 | d = 1.0; |
---|
| 34 | e = 0.0149; |
---|
| 35 | |
---|
| 36 | M = diag([0.0013 0.0013 5.0e-6 1.0e-10]); |
---|
| 37 | N = diag([0.0006 0.0006]); |
---|
| 38 | |
---|
| 39 | |
---|
| 40 | iP = eye(4); |
---|
| 41 | % iQ = diag([0.001 0.001 0.00001 0.00001]); |
---|
| 42 | % iR = diag([0.0005 0.0005]); |
---|
| 43 | % iQ = diag([0.4 0.4 0.2 0.2]); |
---|
| 44 | % iR = diag([0.5 0.5]); |
---|
| 45 | iQ = M; |
---|
| 46 | iR = N; |
---|
| 47 | |
---|
| 48 | |
---|
| 49 | alpha = 0.001; |
---|
| 50 | beta = 2.0; |
---|
| 51 | kappa = 0.0; |
---|
| 52 | |
---|
| 53 | L = 4; |
---|
| 54 | % x0 = zeros(4,1); |
---|
| 55 | P0 = iP; |
---|
| 56 | |
---|
| 57 | lambda = alpha*alpha*(L + kappa) - L; |
---|
| 58 | Wm0 = lambda / (L + lambda); |
---|
| 59 | Wc0 = Wm0 + 1 - alpha*alpha + beta; |
---|
| 60 | Wmc = 1.0 / (2.0*(L + lambda)); |
---|
| 61 | |
---|
| 62 | WM = [Wm0; Wmc*ones(2*L,1)]; |
---|
| 63 | WC = [Wc0; Wmc*ones(2*L,1)]; |
---|
| 64 | |
---|
| 65 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 66 | % nasteveni experimentu |
---|
| 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 68 | |
---|
| 69 | |
---|
| 70 | % T = 120000; %horizont |
---|
| 71 | |
---|
| 72 | % theta0 = pi;%0.0; |
---|
| 73 | |
---|
| 74 | ref_ome = zeros(1, T); %referencni signal |
---|
| 75 | % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0]; |
---|
| 76 | % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; |
---|
| 77 | % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; |
---|
| 78 | |
---|
| 79 | for k = 1:T, |
---|
| 80 | index = floor(k*dt); |
---|
| 81 | if(index>0) |
---|
| 82 | lower = ref_profile(index); |
---|
| 83 | else |
---|
| 84 | lower = 0; |
---|
| 85 | end |
---|
| 86 | if(index<T*dt) |
---|
| 87 | upper = ref_profile(index+1); |
---|
| 88 | else |
---|
| 89 | upper = 0; |
---|
| 90 | end |
---|
| 91 | ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); |
---|
| 92 | end |
---|
| 93 | |
---|
| 94 | |
---|
| 95 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 96 | % nastaveni rizeni |
---|
| 97 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 98 | |
---|
| 99 | MAXu = 100; %maximalni povolene napeti |
---|
| 100 | MAXufl = 1; %flag pro volbu omezeni |
---|
| 101 | |
---|
| 102 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 103 | % promenne stavu systemu |
---|
| 104 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 105 | |
---|
| 106 | x_sys = zeros(4, T); %vnitrni stav systemu |
---|
| 107 | y_sys = zeros(2, T); %vystup systemu |
---|
| 108 | u_ab = zeros(2, T); %rizeni systemu v alfa-beta |
---|
| 109 | % sign_om = zeros(2, T); % signum omega: real, estimated |
---|
| 110 | |
---|
| 111 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 112 | % promenne estimatoru EKF |
---|
| 113 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 114 | |
---|
| 115 | x_est = zeros(4, T); %odhad stavu |
---|
| 116 | % P = zeros(4, 4); %kovariancni matice EKF |
---|
| 117 | |
---|
| 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 119 | % promenne rizeni |
---|
| 120 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 121 | |
---|
| 122 | % u_dq = zeros(2, T); |
---|
| 123 | |
---|
| 124 | %PI vektorove |
---|
| 125 | kon_pi = 3.0; |
---|
| 126 | kon_ii = 0.00375; |
---|
| 127 | kon_pu = 20.0; |
---|
| 128 | kon_iu = 0.05; |
---|
| 129 | sum_iq = 0; |
---|
| 130 | sum_ud = 0; |
---|
| 131 | sum_uq = 0; |
---|
| 132 | |
---|
| 133 | % koef_bic = 6.0; |
---|
| 134 | |
---|
| 135 | |
---|
| 136 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 137 | % inicializace |
---|
| 138 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 139 | |
---|
| 140 | x_est(4, 1) = theta0; %pocatecni natoceni theta |
---|
| 141 | % x_est(:,1) = x0; %state estimate |
---|
| 142 | x = x_est(:, 1); |
---|
| 143 | P = P0; %covariance |
---|
| 144 | |
---|
| 145 | % figure; |
---|
| 146 | |
---|
| 147 | lambda2 = lambda; |
---|
| 148 | % lambda2 = lambda + 0.001; %+100pro randi vyber pro prumer trebe daleko min cca 0.001 |
---|
| 149 | % lambda2 = lambda + 100; %+100pro randi vyber pro prumer trebe daleko min cca 0.001 |
---|
| 150 | %%%%%% oboji celkem jede, jeste to zkusit |
---|
| 151 | |
---|
| 152 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 153 | % hlavni smycka |
---|
| 154 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 155 | |
---|
| 156 | for t = 1:T-1, |
---|
| 157 | %%%% estimace %%% (yt -> estxt) |
---|
| 158 | %UKF |
---|
| 159 | |
---|
| 160 | if(t == 1) |
---|
| 161 | u_aold = 0; |
---|
| 162 | u_bold = 0; |
---|
| 163 | % ome = 0; |
---|
| 164 | % the = 0; |
---|
| 165 | else |
---|
| 166 | u_aold = u_ab(1, t-1); |
---|
| 167 | u_bold = u_ab(2, t-1); |
---|
| 168 | end |
---|
| 169 | |
---|
| 170 | %sigma points |
---|
| 171 | sigma = [x, x*ones(1,L)+chol((L+lambda)*P), x*ones(1,L)-chol((L+lambda)*P)]; |
---|
| 172 | |
---|
| 173 | %time update |
---|
| 174 | Fsigma = [a*sigma(1,:) + b*sigma(3,:).*sin(sigma(4,:)) + c*u_aold*ones(1,2*L+1);... |
---|
| 175 | a*sigma(2,:) - b*sigma(3,:).*cos(sigma(4,:)) + c*u_bold*ones(1,2*L+1);... |
---|
| 176 | d*sigma(3,:) + e*(sigma(2,:).*cos(sigma(4,:)) - sigma(1,:).*sin(sigma(4,:)));... |
---|
| 177 | sigma(4,:) + dt*sigma(3,:)]; |
---|
| 178 | |
---|
| 179 | x_m = Fsigma*WM; |
---|
| 180 | |
---|
| 181 | P_m = zeros(4); |
---|
| 182 | for i = 1:2*L+1 |
---|
| 183 | P_m = P_m + WC(i)*(Fsigma(:,i)-x_m)*(Fsigma(:,i)-x_m)'; |
---|
| 184 | end |
---|
| 185 | P_m = P_m + iQ; |
---|
| 186 | |
---|
| 187 | Hsigma = [Fsigma(1,:); Fsigma(2,:)]; |
---|
| 188 | |
---|
| 189 | y_m = Hsigma*WM; |
---|
| 190 | |
---|
| 191 | %measurement update |
---|
| 192 | Pyy = zeros(2); |
---|
| 193 | Pxy = zeros(4,2); |
---|
| 194 | for i = 1:2*L+1 |
---|
| 195 | Pyy = Pyy + WC(i)*(Hsigma(:,i)-y_m)*(Hsigma(:,i)-y_m)'; |
---|
| 196 | Pxy = Pxy + WC(i)*(Fsigma(:,i)-x_m)*(Hsigma(:,i)-y_m)'; |
---|
| 197 | end |
---|
| 198 | Pyy = Pyy + iR; |
---|
| 199 | |
---|
| 200 | Kk = Pxy/(Pyy); |
---|
| 201 | |
---|
| 202 | x = x_m + Kk*(y_sys(:,t) - y_m); |
---|
| 203 | |
---|
| 204 | P = P_m - Kk*Pyy*Kk'; |
---|
| 205 | |
---|
| 206 | x_est(:,t) = x; |
---|
| 207 | % p33(t) = P(3,3); |
---|
| 208 | |
---|
| 209 | sigma_est = [x, x*ones(1,L)+chol((L+lambda2)*P), x*ones(1,L)-chol((L+lambda2)*P)]; |
---|
| 210 | |
---|
| 211 | %%%%% rizeni %%%% (estxt -> ut) |
---|
| 212 | ial = x_est(1, t); |
---|
| 213 | ibe = x_est(2, t); |
---|
| 214 | ome = x_est(3, t); |
---|
| 215 | the = x_est(4, t); |
---|
| 216 | |
---|
| 217 | % sign_om(2, t) = sign(x_est(3, t)); |
---|
| 218 | |
---|
| 219 | |
---|
| 220 | % id = ial*cos(the) + ibe*sin(the); |
---|
| 221 | % iq = ibe*cos(the) - ial*sin(the); |
---|
| 222 | % |
---|
| 223 | % sum_iq = sum_iq + ref_ome(t) - ome; |
---|
| 224 | % ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq; |
---|
| 225 | % sum_ud = sum_ud - id; |
---|
| 226 | % u_dq(1, t) = kon_pu*(-id) + kon_iu*sum_ud; |
---|
| 227 | % sum_uq = sum_uq + ref_iq - iq; |
---|
| 228 | % u_dq(2, t) = kon_pu*(ref_iq - iq) + kon_iu*sum_uq; |
---|
| 229 | % u_dq(1, t) = u_dq(1, t) - Ls*ome*ref_iq; |
---|
| 230 | % u_dq(2, t) = u_dq(2, t) + psipm*ome; |
---|
| 231 | % |
---|
| 232 | % u_dq(1, t) = u_dq(1, t) + sign(-ome)*koef_bic; |
---|
| 233 | % u_dq(2, t) = u_dq(2, t) + sign(ome)*koef_bic; |
---|
| 234 | % |
---|
| 235 | % u_ab(1, t) = u_dq(1, t)*cos(the) - u_dq(2, t)*sin(the); |
---|
| 236 | % u_ab(2, t) = u_dq(2, t)*cos(the) + u_dq(1, t)*sin(the); |
---|
| 237 | |
---|
| 238 | %%%%%%%%%%%%%%%%%% |
---|
| 239 | %predpoctu spolecne integracni cleny pro vsechny sigma body dle prumeru (at |
---|
| 240 | %neni moc uchovavanych dat) |
---|
| 241 | id = ial*cos(the) + ibe*sin(the); |
---|
| 242 | iq = ibe*cos(the) - ial*sin(the); |
---|
| 243 | sum_iq = sum_iq + ref_ome(t) - ome; |
---|
| 244 | ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq; |
---|
| 245 | sum_ud = sum_ud - id; |
---|
| 246 | sum_uq = sum_uq + ref_iq - iq; |
---|
| 247 | |
---|
| 248 | %vypoctu rizeni pro kazdy sigma bod se spolecnym integracnim clenem |
---|
| 249 | jedna = ones(size(sigma_est(3,:))); |
---|
| 250 | ref_iq = kon_pi*(ref_ome(t)*jedna - sigma_est(3,:)) + kon_ii*sum_iq*jedna; |
---|
| 251 | id = sigma_est(1,:).*cos(sigma_est(4,:)) + sigma_est(2,:).*sin(sigma_est(4,:)); |
---|
| 252 | iq = sigma_est(2,:).*cos(sigma_est(4,:)) - sigma_est(1,:).*sin(sigma_est(4,:)); |
---|
| 253 | u_dq = [kon_pu*(-id) + kon_iu*sum_ud*jedna - Ls*sigma_est(3,:).*ref_iq;... |
---|
| 254 | kon_pu*(ref_iq - iq) + kon_iu*sum_uq*jedna + psipm*sigma_est(3,:)]; |
---|
| 255 | |
---|
| 256 | %vyber vhodneho rizeni z distribuce na zaklade sigma bodu u_dq |
---|
| 257 | %puvodni (bez sigma bodu) |
---|
| 258 | % u_ab(1, t) = u_dq(1, 1)*cos(the) - u_dq(2, 1)*sin(the); |
---|
| 259 | % u_ab(2, t) = u_dq(2, 1)*cos(the) + u_dq(1, 1)*sin(the); |
---|
| 260 | %vazeny prumer (melo by vyjit podobne) |
---|
| 261 | % sg_u_ab = [u_dq(1,:).*cos(sigma_est(4,:)) - u_dq(2,:).*sin(sigma_est(4,:));... |
---|
| 262 | % u_dq(2,:).*cos(sigma_est(4,:)) + u_dq(1,:).*sin(sigma_est(4,:))]; |
---|
| 263 | % u_ab(:,t) = sg_u_ab*WM; |
---|
| 264 | %randomizace - nahodna volba jednoho ze sigma bodu |
---|
| 265 | sg_u_ab = [u_dq(1,:).*cos(sigma_est(4,:)) - u_dq(2,:).*sin(sigma_est(4,:));... |
---|
| 266 | u_dq(2,:).*cos(sigma_est(4,:)) + u_dq(1,:).*sin(sigma_est(4,:))]; |
---|
| 267 | u_ab(:,t) = sg_u_ab(:,randi(9)); |
---|
| 268 | |
---|
| 269 | if(MAXufl == 1) |
---|
| 270 | if(u_ab(1, t) > MAXu) |
---|
| 271 | u_ab(1, t) = MAXu; |
---|
| 272 | elseif(u_ab(1, t) < -MAXu) |
---|
| 273 | u_ab(1, t) = -MAXu; |
---|
| 274 | end |
---|
| 275 | if(u_ab(2, t) > MAXu) |
---|
| 276 | u_ab(2, t) = MAXu; |
---|
| 277 | elseif(u_ab(2, t) < -MAXu) |
---|
| 278 | u_ab(2, t) = -MAXu; |
---|
| 279 | end |
---|
| 280 | elseif(MAXufl == 2) |
---|
| 281 | uampl = sqrt(u_ab(:, t)'*u_ab(:, t)); |
---|
| 282 | uangl = atan2(u_ab(2, t), u_ab(1, t)); |
---|
| 283 | if(uampl > MAXu) |
---|
| 284 | uampl = MAXu; |
---|
| 285 | end |
---|
| 286 | u_ab(1, t) = uampl*cos(uangl); |
---|
| 287 | u_ab(2, t) = uampl*sin(uangl); |
---|
| 288 | end |
---|
| 289 | |
---|
| 290 | %%%% simulace %%% (xt + ut -> xt+1; xt+1 -> yt+1) |
---|
| 291 | if (simulator == 1), |
---|
| 292 | ua = 3*u_ab(1, t); |
---|
| 293 | ub = 3*u_ab(2, t); |
---|
| 294 | |
---|
| 295 | [tx, ty] = pmsm_sim(ua, ub, 0); |
---|
| 296 | |
---|
| 297 | x_sys(:, t+1) = tx(1:4); %isa, isb, omega, theta |
---|
| 298 | y_sys(:, t+1) = ty(3:4); %isa, isb |
---|
| 299 | else |
---|
| 300 | x_sys(1, t+1) = a*x_sys(1, t) + b*x_sys(3, t)*sin(x_sys(4, t)) + c*u_ab(1, t) + sqrt(M(1, 1))*randn(); |
---|
| 301 | x_sys(2, t+1) = a*x_sys(2, t) - b*x_sys(3, t)*cos(x_sys(4, t)) + c*u_ab(2, t) + sqrt(M(2, 2))*randn(); |
---|
| 302 | x_sys(3, t+1) = d*x_sys(3, t) + e*(x_sys(2, t)*cos(x_sys(4, t)) - x_sys(1, t)*sin(x_sys(4, t))) + sqrt(M(3, 3))*randn(); |
---|
| 303 | x_sys(4, t+1) = x_sys(4, t) + dt*x_sys(3, t) + sqrt(M(4, 4))*randn(); |
---|
| 304 | |
---|
| 305 | % sign_om(1, t+1) = sign(x_sys(3, t+1)); |
---|
| 306 | |
---|
| 307 | y_sys(1, t+1) = x_sys(1, t+1) + sqrt(N(1, 1))*randn(); |
---|
| 308 | y_sys(2, t+1) = x_sys(2, t+1) + sqrt(N(2, 2))*randn(); |
---|
| 309 | end |
---|
| 310 | end |
---|
| 311 | |
---|
| 312 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 313 | % zaznam vysledku |
---|
| 314 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 315 | |
---|
| 316 | % xax = 1:T-1; |
---|
| 317 | % timex = (xax)*dt; |
---|
| 318 | % subplot(2, 2, 1); |
---|
| 319 | % plot(timex, x_sys(1, xax), timex, x_est(1, xax)); |
---|
| 320 | % subplot(2, 2, 2); |
---|
| 321 | % plot(timex, x_sys(2, xax), timex, x_est(2, xax)); |
---|
| 322 | % subplot(2, 2, 3); |
---|
| 323 | % plot(timex, x_sys(3, xax), timex, x_est(3, xax), timex, ref_ome(xax)); |
---|
| 324 | % subplot(2, 2, 4); |
---|
| 325 | % plot(timex, atan2(sin(x_sys(4, xax)),cos(x_sys(4, xax))), timex, atan2(sin(x_est(4, xax)),cos(x_est(4, xax)))); |
---|
| 326 | |
---|
| 327 | % figure; |
---|
| 328 | % plot(timex, sign_om(1, xax), timex, sign_om(2, xax)); |
---|
| 329 | |
---|
| 330 | % figure; |
---|
| 331 | % plot(timex, x_sys(3, :)-ref_ome); |
---|
| 332 | |
---|
| 333 | if(graf == 1) |
---|
| 334 | %vykresleni |
---|
| 335 | cas = (1:T)*dt; |
---|
| 336 | figure; |
---|
| 337 | subplot(2,1,1); |
---|
| 338 | plot(cas,x_est(3,:),cas,x_sys(3,:),cas,ref_ome); |
---|
| 339 | title('Prubeh otacek v case'); |
---|
| 340 | xlabel('cas [s]'); |
---|
| 341 | ylabel('otacky [rad/s]'); |
---|
| 342 | legend('odhad','skutecna hodnota','referencni hodnota'); |
---|
| 343 | subplot(2,1,2); |
---|
| 344 | plot(cas,atan2(sin(x_est(4,:)),cos(x_est(4,:))),cas,atan2(sin(x_sys(4,:)),cos(x_sys(4,:)))); |
---|
| 345 | title('Prubeh polohy v case'); |
---|
| 346 | xlabel('cas [s]'); |
---|
| 347 | ylabel('poloha [rad]'); |
---|
| 348 | |
---|
| 349 | figure; |
---|
| 350 | plot(cas,x_sys(3,:)-ref_ome); |
---|
| 351 | title('Prubeh chyby (skutecne - pozadovane otacky v case)'); |
---|
| 352 | xlabel('cas [s]'); |
---|
| 353 | ylabel('chyba [rad/s]'); |
---|
| 354 | end |
---|
| 355 | |
---|
| 356 | loss = sum((x_sys(3,:)-ref_ome).^2); |
---|
| 357 | |
---|
| 358 | end |
---|