1 | | % main - hlavni skript |
2 | | clear all; |
3 | | % oznaceni: s ... system |
4 | | % k ... kalman (EKF) |
5 | | % l ... rizeni (LQR) |
6 | | |
7 | | % KONSTANTY |
8 | | T = 40000; %horizont |
9 | | dt = 0.000125; %casovy krok |
10 | | |
11 | | % Rs = 0.28; |
12 | | % Ls = 0.003465; |
13 | | % psipm = 0.1989; |
14 | | % B = 0; |
15 | | % kp = 1.5; |
16 | | % pp = 4.0; |
17 | | % J = 0.04; |
18 | | |
19 | | % Lq = 1.05*Ls; |
20 | | % Ld = 0.95*Ls; |
21 | | |
22 | | a = 0.9898; |
23 | | b = 0.0072; |
24 | | c = 0.0361; |
25 | | d = 1.0; |
26 | | e = 0.0149; |
27 | | |
28 | | % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; |
29 | | ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; |
30 | | |
31 | | %kovariance EKF na stavu, ktery vytvari hyperstav |
32 | | % Q_k = diag([0.001, 0.00001]); |
33 | | % R_k = diag([0.015, 0.015]); |
34 | | Q_k = diag([0.01, 0.0001]); |
35 | | R_k = diag([0.15, 0.15]); |
36 | | |
37 | | %kovariance EKF na hyperstavu |
38 | | % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]); |
39 | | % Rh_k = diag([0.015, 0.015]); |
40 | | Qh_k = diag([0.01, 0.0001, 0.1, 0.1, 0.1]); |
41 | | Rh_k = diag([0.15, 0.15]); |
42 | | |
43 | | %hodnoty sumu v systemu |
44 | | nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); |
45 | | nR = diag([0.0006, 0.0006]); |
46 | | |
47 | | iter_l = 10;% pocet iteraci ve vypoctu rizeni |
48 | | |
49 | | B_l = zeros(6,2); |
50 | | % B_l(1,1) = c; |
51 | | % B_l(2,2) = c; |
52 | | |
53 | | % o t Po Pot Pt |
54 | | Q_l = diag([1 0 300 0.0 300 0]); |
55 | | % Q_l = diag([1 0 0 0 0 0]); |
56 | | r = 0.0001; |
57 | | R_l = diag([r, r]); |
58 | | |
59 | | |
60 | | |
61 | | % PROMENNE |
62 | | x_s = zeros(4,T); %stav |
63 | | y_s = zeros(2,T); %mereni |
64 | | x_k = zeros(5,T); %odhad hyperstavu |
65 | | P_k = zeros(5); %kovariance hyperstavu |
66 | | u_l = zeros(2,T); %rizeni |
67 | | S_l = zeros(6); %jadro ztraty |
68 | | pre_k = zeros(3,1); %predikce stavu |
69 | | |
70 | | |
71 | | % POCATECNI HODNOTY |
72 | | noise = 1; %prepinac sumu |
73 | | % noise = 0; |
74 | | |
75 | | theta0 = 1.5;%1.7; %pocatecni poloha |
76 | | Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) |
77 | | Pk0 = eye(5); %pocatecni kovariance hyperstavu |
78 | | ST = zeros(6); %koncova ztrata |
79 | | |
80 | | |
81 | | % INICIALIZACE |
82 | | x_s(4,1) = theta0; |
83 | | x_k(3,1) = Ps0(1,1); |
84 | | x_k(4,1) = Ps0(1,2); |
85 | | x_k(5,1) = Ps0(2,2); |
86 | | P_k = Pk0; |
87 | | S_l = ST; |
88 | | |
89 | | ref_ome = zeros(1, T); |
90 | | for k = 1:T, |
91 | | index = floor(k*dt); |
92 | | if(index>0) |
93 | | lower = ref_profile(index); |
94 | | else |
95 | | lower = 0; |
96 | | end |
97 | | if(index<T*dt) |
98 | | upper = ref_profile(index+1); |
99 | | else |
100 | | upper = 0; |
101 | | end |
102 | | ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); |
| 1 | function [loss] = main(T, ref_profile, theta0, simulator, graf, inddq) |
| 2 | % main - hlavni skript |
| 3 | % clear all; |
| 4 | % oznaceni: s ... system |
| 5 | % k ... kalman (EKF) |
| 6 | % l ... rizeni (LQR) |
| 7 | |
| 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
| 9 | %%%%%pouziti SIMULATORU |
| 10 | % simulator = 1; |
| 11 | % simulator = 0; |
| 12 | |
| 13 | if((simulator == 1)||(simulator == 10)) |
| 14 | sim_param = pmsm_sim; |
| 15 | % sim_param(9) = 0; %vypne dead-time |
| 16 | pmsm_sim(sim_param); |
104 | | |
105 | | % Derivace pro prvni EKF |
106 | | [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0); |
107 | | |
108 | | ri = 0.0001; |
109 | | ai = (1-a*a)/c/c; |
110 | | Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; |
111 | | Li = a*c*Si/(c*c*Si+ri); |
112 | | |
113 | | Pia = 1; |
114 | | Pib = 1; |
115 | | qi = 0.1; |
116 | | ri = 0.05; |
117 | | y = [0;0]; |
118 | | |
119 | | % HLAVNI SMYCKA |
120 | | for t = 1:T-1, |
121 | | % EKF |
122 | | Pp = Pia*(a*a+b*b+b*b*cos(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; |
123 | | Pia = Pp-Pp*Pp/(Pp+ri); |
124 | | y(1) = (1-Pp/(Pp+ri))*(a*y(1)+b*x_k(1,t)*sin(x_k(2,t))+c*u_l(1,t)) + Pp/(Pp+ri)*y_s(1,t); |
125 | | Pp = Pib*(a*a+b*b+b*b*sin(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; |
126 | | Pib = Pp-Pp*Pp/(Pp+ri); |
127 | | y(2) = (1-Pp/(Pp+ri))*(a*y(2)-b*x_k(1,t)*cos(x_k(2,t))+c*u_l(2,t)) + Pp/(Pp+ri)*y_s(2,t); |
128 | | [x_k(:,t+1), P_k] = extKF(x_k(:,t), y, u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k); |
129 | | % [x_k(:,t+1), P_k] = extKF(x_k(:,t), y_s(:,t), u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k); |
130 | | |
131 | | % Q_l(1,1) = 1/(1+exp(-2*x_k(1,t+1)+6))+0.1; |
132 | | Q_l(3,3) = x_k(5,t+1)^5*50; |
133 | | Q_l(5,5) = Q_l(3,3); |
134 | | |
135 | | % Derivace |
136 | | [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,t+1), y_s(:,t), x_k(:,t+1), Q_k, R_k, ref_ome(t)); |
137 | | |
138 | | % LQ |
139 | | B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))]; |
140 | | % [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, S_l, Q_l, R_l, iter_l); |
141 | | [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, ST, Q_l, R_l, iter_l); |
142 | | u_l(:,t+1) = b/c*x_k(1,t+1)*[-sin(x_k(2,t+1));cos(x_k(2,t+1))] + u_l(:,t+1) - Li*y_s(:,t); |
143 | | if u_l(1,t+1) > 100 |
144 | | u_l(1,t+1) = 100; |
145 | | elseif u_l(1,t+1) < -100 |
146 | | u_l(1,t+1) = -100; |
| 18 | %%%%% |
| 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
| 20 | |
| 21 | % KONSTANTY |
| 22 | % T = 120000; %horizont |
| 23 | dt = 0.000125; %casovy krok |
| 24 | |
| 25 | % Rs = 0.28; |
| 26 | % Ls = 0.003465; |
| 27 | % psipm = 0.1989; |
| 28 | % B = 0; |
| 29 | % kp = 1.5; |
| 30 | % pp = 4.0; |
| 31 | % J = 0.04; |
| 32 | |
| 33 | % Lq = 1.05*Ls; |
| 34 | % Ld = 0.95*Ls; |
| 35 | |
| 36 | a = 0.9898; |
| 37 | b = 0.0072; |
| 38 | c = 0.0361; |
| 39 | % d = 1.0; |
| 40 | e = 0.0149; |
| 41 | |
| 42 | % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; |
| 43 | % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; |
| 44 | % ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0]; |
| 45 | % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0]; |
| 46 | % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20]; |
| 47 | |
| 48 | %kovariance EKF na stavu, ktery vytvari hyperstav |
| 49 | % Q_k = diag([0.001, 0.00001]); |
| 50 | % R_k = diag([0.015, 0.015]); |
| 51 | Q_k = diag([0.01, 0.0001]); |
| 52 | R_k = diag([0.15, 0.15]); |
| 53 | |
| 54 | %kovariance EKF na hyperstavu |
| 55 | % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]); |
| 56 | % Rh_k = diag([0.015, 0.015]); |
| 57 | Qh_k = diag([0.01, 0.0001, 0.1, 0.1, 0.1]); |
| 58 | Rh_k = diag([0.15, 0.15]); |
| 59 | |
| 60 | %hodnoty sumu v systemu |
| 61 | nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); |
| 62 | nR = diag([0.0006, 0.0006]); |
| 63 | |
| 64 | iter_l = 10;% pocet iteraci ve vypoctu rizeni |
| 65 | |
| 66 | B_l = zeros(6,2); |
| 67 | % B_l(1,1) = c; |
| 68 | % B_l(2,2) = c; |
| 69 | |
| 70 | % o t Po Pot Pt |
| 71 | Q_l = diag([1 0 1 0 0 0]); % spravne z teoretickeho hlediska |
| 72 | % Q_l = diag([1 0 1 0 1 0]); |
| 73 | % Q_l = diag([1 0 0 0 0 0]); |
| 74 | r = 0.0001; |
| 75 | R_l = diag([r, r]); |
| 76 | |
| 77 | |
| 78 | |
| 79 | % PROMENNE |
| 80 | x_s = zeros(4,T); %stav |
| 81 | y_s = zeros(2,T); %mereni |
| 82 | x_k = zeros(5,T); %odhad hyperstavu |
| 83 | % P_k = zeros(5); %kovariance hyperstavu |
| 84 | u_l = zeros(2,T); %rizeni |
| 85 | % S_l = zeros(6); %jadro ztraty |
| 86 | % pre_k = zeros(3,1); %predikce stavu |
| 87 | |
| 88 | |
| 89 | % POCATECNI HODNOTY |
| 90 | noise = 1; %prepinac sumu |
| 91 | % noise = 0; |
| 92 | |
| 93 | % theta0 = 0; %pocatecni poloha odhadu (nejde pro stav kvuli simulatoru) |
| 94 | Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) |
| 95 | Pk0 = eye(5); %pocatecni kovariance hyperstavu |
| 96 | ST = zeros(6); %koncova ztrata |
| 97 | |
| 98 | |
| 99 | % INICIALIZACE |
| 100 | x_k(2,1) = theta0; |
| 101 | x_k(3,1) = Ps0(1,1); |
| 102 | x_k(4,1) = Ps0(1,2); |
| 103 | x_k(5,1) = Ps0(2,2); |
| 104 | P_k = Pk0; |
| 105 | S_l = ST; |
| 106 | |
| 107 | ref_ome = zeros(1, T); |
| 108 | for k = 1:T, |
| 109 | index = floor(k*dt); |
| 110 | if(index>0) |
| 111 | lower = ref_profile(index); |
| 112 | else |
| 113 | lower = 0; |
| 114 | end |
| 115 | if(index<T*dt) |
| 116 | upper = ref_profile(index+1); |
| 117 | else |
| 118 | upper = 0; |
| 119 | end |
| 120 | ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); |
| 121 | end |
| 122 | |
| 123 | % Derivace pro prvni EKF |
| 124 | [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0, inddq); |
| 125 | |
| 126 | ri = 0.0001; |
| 127 | ai = (1-a*a)/c/c; |
| 128 | Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; |
| 129 | Li = a*c*Si/(c*c*Si+ri); |
| 130 | |
| 131 | Pia = 1; |
| 132 | Pib = 1; |
| 133 | qi = 0.1; |
| 134 | ri = 0.05; |
| 135 | y = [0;0]; |
| 136 | |
| 137 | % HLAVNI SMYCKA |
| 138 | for t = 1:T-1, |
| 139 | % EKF |
| 140 | Pp = Pia*(a*a+b*b+b*b*cos(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; |
| 141 | Pia = Pp-Pp*Pp/(Pp+ri); |
| 142 | y(1) = (1-Pp/(Pp+ri))*(a*y(1)+b*x_k(1,t)*sin(x_k(2,t))+c*u_l(1,t)) + Pp/(Pp+ri)*y_s(1,t); |
| 143 | Pp = Pib*(a*a+b*b+b*b*sin(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; |
| 144 | Pib = Pp-Pp*Pp/(Pp+ri); |
| 145 | y(2) = (1-Pp/(Pp+ri))*(a*y(2)-b*x_k(1,t)*cos(x_k(2,t))+c*u_l(2,t)) + Pp/(Pp+ri)*y_s(2,t); |
| 146 | [x_k(:,t+1), P_k] = extKF(x_k(:,t), y, u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k); |
| 147 | % [x_k(:,t+1), P_k] = extKF(x_k(:,t), y_s(:,t), u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k); |
| 148 | |
| 149 | % Q_l(1,1) = 1/(1+exp(-2*x_k(1,t+1)+6))+0.1; |
| 150 | % Q_l(3,3) = x_k(5,t+1)^5*50; |
| 151 | % Q_l(5,5) = Q_l(3,3); |
| 152 | |
| 153 | % Derivace |
| 154 | [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,t+1), y_s(:,t), x_k(:,t+1), Q_k, R_k, ref_ome(t), inddq); |
| 155 | |
| 156 | % LQ |
| 157 | B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))]; |
| 158 | % [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, S_l, Q_l, R_l, iter_l); |
| 159 | [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, ST, Q_l, R_l, iter_l); |
| 160 | u_l(:,t+1) = b/c*x_k(1,t+1)*[-sin(x_k(2,t+1));cos(x_k(2,t+1))] + u_l(:,t+1) - Li*y_s(:,t); |
| 161 | |
| 162 | if u_l(1,t+1) > 100 |
| 163 | u_l(1,t+1) = 100; |
| 164 | elseif u_l(1,t+1) < -100 |
| 165 | u_l(1,t+1) = -100; |
| 166 | end |
| 167 | if u_l(2,t+1) > 100 |
| 168 | u_l(2,t+1) = 100; |
| 169 | elseif u_l(2,t+1) < -100 |
| 170 | u_l(2,t+1) = -100; |
| 171 | end |
| 172 | |
| 173 | % Vyvoj systemu |
| 174 | [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator); |