function [loss] = basic_main_lq4(T, ref_profile, theta0, simulator, graf, inddq) % main - hlavni skript >>>>>>>PLNY STAV<<<<<<< % clear all; % oznaceni: s ... system % k ... kalman (EKF) % l ... rizeni (LQR) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%pouziti SIMULATORU % simulator = 1; % simulator = 0; if((simulator == 1)||(simulator == 10)) sim_param = pmsm_sim; % sim_param(9) = 0; %vypne dead-time pmsm_sim(sim_param); end %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % KONSTANTY % T = 120000; %horizont % T = 40000; dt = 0.000125; %casovy krok % Rs = 0.28; % Ls = 0.003465; % psipm = 0.1989; % B = 0; % kp = 1.5; % pp = 4.0; % J = 0.04; % Lq = 1.05*Ls; % Ld = 0.95*Ls; a = 0.9898; b = 0.0072; c = 0.0361; d = 1.0; e = 0.0149; Rs = 0.28; Ls = 0.003465; psi = 0.1989; B = 0; kp = 1.5; pp = 4.0; J = 0.04; Lq = 1.0*Ls; Ld = 0.9*Ls; kpp = kp*pp*pp; kppj = kpp/J; % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; % ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0]; % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0]; % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20]; %kovariance EKF na stavu, ktery vytvari hyperstav % Q_k = diag([0.01, 0.01, 0.001, 0.00001]); % R_k = diag([0.005, 0.005]); Q_k = diag([0.1, 0.1, 0.01, 0.0001]); R_k = diag([0.05, 0.05]); %hodnoty sumu v systemu nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); nR = diag([0.0006, 0.0006]); iter_l = 10;% pocet iteraci ve vypoctu rizeni B_l = zeros(5,2); B_l(1,1) = c; B_l(2,2) = c; Q_l = zeros(5); Q_l(3,3) = 1; %ome % Q_l(10,10) = 1; %Var ome % Q_l(14,14) = 1; %Var the % o t Po Pot Pt % Q_l = diag([1 0 1 0 0 0]); % asi spravne z teoretickeho hlediska % Q_l = diag([1 0 1 0 1 0]); % Q_l = diag([1 0 0 0 0 0]); r = 0.0001; R_l = diag([r, r]); % PROMENNE x_s = zeros(4,T); %stav y_s = zeros(2,T); %mereni x_k = zeros(4,T); %odhad hyperstavu % P_k = zeros(14); %kovariance hyperstavu u_l = zeros(2,T); %rizeni % S_l = zeros(15); %jadro ztraty % pre_k = zeros(10,1); %predikce stavu % POCATECNI HODNOTY noise = 1; %prepinac sumu % noise = 0; % theta0 = 0;%1.5;%1.7; %pocatecni poloha % Ps0 = eye(4); %odhad pocatecni kovariance stavu (apriorni) Pk0 = eye(4); %pocatecni kovariance stavu ST = eye(5); % INICIALIZACE x_k(4,1) = theta0; P_k = Pk0; ref_ome = zeros(1, T); for k = 1:T, index = floor(k*dt); if(index>0) lower = ref_profile(index); else lower = 0; end if(index 100 u_l(1,t+1) = 100; elseif u_l(1,t+1) < -100 u_l(1,t+1) = -100; end if u_l(2,t+1) > 100 u_l(2,t+1) = 100; elseif u_l(2,t+1) < -100 u_l(2,t+1) = -100; end % Vyvoj systemu [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator); end if(graf == 1) %vykresleni cas = (1:T)*dt; figure; subplot(2,1,1); plot(cas,x_k(3,:),cas,x_s(3,:),cas,ref_ome); title('Prubeh otacek v case'); xlabel('cas [s]'); ylabel('otacky [rad/s]'); legend('odhad','skutecne','pozadovane'); subplot(2,1,2); plot(cas,atan2(sin(x_k(4,:)),cos(x_k(4,:))),cas,atan2(sin(x_s(4,:)),cos(x_s(4,:)))); title('Prubeh polohy v case'); xlabel('cas [s]'); ylabel('poloha [rad]'); % figure; % subplot(2,1,1); % plot(cas,x_k(1,:),cas,x_s(1,:)); % subplot(2,1,2); % plot(cas,x_k(2,:),cas,x_s(2,:)); figure; plot(cas,x_s(3,:)-ref_ome); title('Prubeh chyby (skutecne - pozadovane otacky v case)'); xlabel('cas [s]'); ylabel('chyba [rad/s]'); end loss = sum((x_s(3,:)-ref_ome).^2); end