| 1 | function [A_k, C_k, pre_k, A_l] = assembDeriv4(ksi, uab, ksi0, Q, R, ref_ome, inddq) |
|---|
| 2 | a = 0.9898; |
|---|
| 3 | b = 0.0072; |
|---|
| 4 | c = 0.0361; |
|---|
| 5 | d = 1.0; |
|---|
| 6 | e = 0.0149; |
|---|
| 7 | dt = 0.000125; |
|---|
| 8 | |
|---|
| 9 | Rs = 0.28; |
|---|
| 10 | Ls = 0.003465; |
|---|
| 11 | psi = 0.1989; |
|---|
| 12 | B = 0; |
|---|
| 13 | kp = 1.5; |
|---|
| 14 | pp = 4.0; |
|---|
| 15 | J = 0.04; |
|---|
| 16 | Lq = 1.0*Ls; |
|---|
| 17 | Ld = 0.9*Ls; |
|---|
| 18 | kpp = kp*pp*pp; |
|---|
| 19 | kppj = kpp/J; |
|---|
| 20 | |
|---|
| 21 | ia = ksi(1); |
|---|
| 22 | ib = ksi(2); |
|---|
| 23 | ome = ksi(3); |
|---|
| 24 | the = ksi(4); |
|---|
| 25 | P = [ksi(5), ksi(6), ksi(8), ksi(11);... |
|---|
| 26 | ksi(6), ksi(7), ksi(9), ksi(12);... |
|---|
| 27 | ksi(8), ksi(9), ksi(10), ksi(13);... |
|---|
| 28 | ksi(11), ksi(12), ksi(13), ksi(14)]; |
|---|
| 29 | A_k = zeros(14); |
|---|
| 30 | A_l = zeros(15); |
|---|
| 31 | |
|---|
| 32 | |
|---|
| 33 | %puvodni matice derivaci |
|---|
| 34 | if(inddq == 0) |
|---|
| 35 | %stejne indukcnosti |
|---|
| 36 | A = [a, 0, b*sin(the), b*ome*cos(the);... |
|---|
| 37 | 0, a, -b*cos(the), b*ome*sin(the);... |
|---|
| 38 | -e*sin(the), e*cos(the), d, -e*(ib*sin(the)+ia*cos(the));... |
|---|
| 39 | 0, 0, dt, 1.0]; |
|---|
| 40 | else |
|---|
| 41 | %ruzne indukcnosti |
|---|
| 42 | A = [[ (Lq - Rs*dt*sin(the)^2)/Lq - (dt*ome*sin(the)*Lq^2*cos(the) + Rs*dt*Lq*cos(the)^2)/(Ld*Lq) + (Ld*dt*ome*cos(the)*sin(the))/Lq, (dt*(Ld - Lq)*(- Lq*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Ld*ome*sin(the)^2))/(Ld*Lq), dt*cos(the)*(ia*sin(the) - ib*cos(the) + (Lq*(ib*cos(the) - ia*sin(the)))/Ld) + dt*sin(the)*(psi/Lq - ia*cos(the) - ib*sin(the) + (Ld*(ia*cos(the) + ib*sin(the)))/Lq), (dt*(ome*psi*cos(the) + Rs*ib*cos(2*the) - Rs*ia*sin(2*the)))/Lq + (Ld*dt*(ia*ome*cos(2*the) + ib*ome*sin(2*the)))/Lq - (dt*(Lq^2*ia*ome*cos(2*the) + Lq^2*ib*ome*sin(2*the) + Lq*Rs*ib*cos(2*the) - Lq*Rs*ia*sin(2*the)))/(Ld*Lq)];... |
|---|
| 43 | [ (dt*(Ld - Lq)*(- Ld*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Lq*ome*sin(the)^2))/(Ld*Lq), (Lq - Rs*dt*cos(the)^2)/Lq - (Lq*Rs*dt*sin(the)^2 - Lq^2*dt*ome*cos(the)*sin(the))/(Ld*Lq) - (Ld*dt*ome*cos(the)*sin(the))/Lq, (dt*(Lq*ia - psi*cos(the)))/Lq + (dt*((Lq^2*ia*cos(2*the))/2 - (Lq^2*ia)/2 + (Lq^2*ib*sin(2*the))/2))/(Ld*Lq) - (Ld*dt*(ia/2 + (ia*cos(2*the))/2 + (ib*sin(2*the))/2))/Lq, (dt*ome*psi*sin(the) - Rs*dt*ia*(2*sin(the)^2 - 1) + Rs*dt*ib*sin(2*the))/Lq + (Ld*(dt*ib*ome*(2*sin(the)^2 - 1) + dt*ia*ome*sin(2*the)))/Lq - (Lq*Rs*dt*ib*sin(2*the) + Lq^2*dt*ib*ome*(2*sin(the)^2 - 1) + Lq^2*dt*ia*ome*sin(2*the) - Lq*Rs*dt*ia*(2*sin(the)^2 - 1))/(Ld*Lq)];... |
|---|
| 44 | [ -dt*kppj*(psi*sin(the) - cos(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the)) + sin(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the))), dt*kppj*(psi*cos(the) + cos(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the)) + sin(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the))), 1.0, -dt*kppj*(psi*(ia*cos(the) + ib*sin(the)) + (Ld - Lq)*(ia*cos(the) + ib*sin(the))^2 - (Ld - Lq)*(ib*cos(the) - ia*sin(the))^2)];... |
|---|
| 45 | [ 0.0, 0.0, dt, 1.0]]; |
|---|
| 46 | end |
|---|
| 47 | C = [1, 0, 0, 0;... |
|---|
| 48 | 0, 1, 0, 0]; |
|---|
| 49 | |
|---|
| 50 | %dalsi matice EKF |
|---|
| 51 | Pp = A*P*A' + Q; |
|---|
| 52 | S = C*Pp*C' + R; |
|---|
| 53 | K = Pp*C'/S; |
|---|
| 54 | Pnew = (eye(4) - K*C)*Pp; |
|---|
| 55 | |
|---|
| 56 | %derivace zakladnich matic EKF stavu |
|---|
| 57 | dAdia = [0, 0, 0, 0;... |
|---|
| 58 | 0, 0, 0, 0;... |
|---|
| 59 | 0, 0, 0, -e*cos(the);... |
|---|
| 60 | 0, 0, 0, 0]; |
|---|
| 61 | dAdib = [0, 0, 0, 0;... |
|---|
| 62 | 0, 0, 0, 0;... |
|---|
| 63 | 0, 0, 0, -e*sin(the);... |
|---|
| 64 | 0, 0, 0, 0]; |
|---|
| 65 | dAdom = [0, 0, 0, b*cos(the);... |
|---|
| 66 | 0, 0, 0, b*sin(the);... |
|---|
| 67 | 0, 0, 0, 0;... |
|---|
| 68 | 0, 0, 0, 0]; |
|---|
| 69 | dAdth = [0, 0, b*cos(the), -b*ome*sin(the);... |
|---|
| 70 | 0, 0, b*sin(the), b*ome*cos(the);... |
|---|
| 71 | -e*cos(the), -e*sin(the), 0, -e*(ib*cos(the)-ia*sin(the));... |
|---|
| 72 | 0, 0, 0, 0]; |
|---|
| 73 | % dAdP = zeros(4); |
|---|
| 74 | % dCdiaibomth = zeros(2,4); |
|---|
| 75 | % dCdP = zeros(2,4); |
|---|
| 76 | % dPdiaibomth = zeros(4); |
|---|
| 77 | dPdP5 = zeros(4); |
|---|
| 78 | dPdP5(1,1) = 1; |
|---|
| 79 | dPdP6 = zeros(4); |
|---|
| 80 | dPdP6(1,2) = 1; |
|---|
| 81 | dPdP6(2,1) = 1; |
|---|
| 82 | dPdP7 = zeros(4); |
|---|
| 83 | dPdP7(2,2) = 1; |
|---|
| 84 | dPdP8 = zeros(4); |
|---|
| 85 | dPdP8(1,3) = 1; |
|---|
| 86 | dPdP8(3,1) = 1; |
|---|
| 87 | dPdP9 = zeros(4); |
|---|
| 88 | dPdP9(2,3) = 1; |
|---|
| 89 | dPdP9(3,2) = 1; |
|---|
| 90 | dPdP10 = zeros(4); |
|---|
| 91 | dPdP10(3,3) = 1; |
|---|
| 92 | dPdP11 = zeros(4); |
|---|
| 93 | dPdP11(1,4) = 1; |
|---|
| 94 | dPdP11(4,1) = 1; |
|---|
| 95 | dPdP12 = zeros(4); |
|---|
| 96 | dPdP12(2,4) = 1; |
|---|
| 97 | dPdP12(4,2) = 1; |
|---|
| 98 | dPdP13 = zeros(4); |
|---|
| 99 | dPdP13(3,4) = 1; |
|---|
| 100 | dPdP13(4,3) = 1; |
|---|
| 101 | dPdP14 = zeros(4); |
|---|
| 102 | dPdP14(4,4) = 1; |
|---|
| 103 | |
|---|
| 104 | |
|---|
| 105 | %derivace dalsich matic EKF stavu |
|---|
| 106 | dPpdia = dAdia*P*A' + A*P*dAdia'; |
|---|
| 107 | dPpdib = dAdib*P*A' + A*P*dAdib'; |
|---|
| 108 | dPpdom = dAdom*P*A' + A*P*dAdom'; |
|---|
| 109 | dPpdth = dAdth*P*A' + A*P*dAdth'; |
|---|
| 110 | dPpdP5 = A*dPdP5*A'; |
|---|
| 111 | dPpdP6 = A*dPdP6*A'; |
|---|
| 112 | dPpdP7 = A*dPdP7*A'; |
|---|
| 113 | dPpdP8 = A*dPdP8*A'; |
|---|
| 114 | dPpdP9 = A*dPdP9*A'; |
|---|
| 115 | dPpdP10 = A*dPdP10*A'; |
|---|
| 116 | dPpdP11 = A*dPdP11*A'; |
|---|
| 117 | dPpdP12 = A*dPdP12*A'; |
|---|
| 118 | dPpdP13 = A*dPdP13*A'; |
|---|
| 119 | dPpdP14 = A*dPdP14*A'; |
|---|
| 120 | |
|---|
| 121 | dSdia = C*dPpdia*C'; |
|---|
| 122 | dSdib = C*dPpdib*C'; |
|---|
| 123 | dSdom = C*dPpdom*C'; |
|---|
| 124 | dSdth = C*dPpdth*C'; |
|---|
| 125 | dSdP5 = C*dPpdP5*C'; |
|---|
| 126 | dSdP6 = C*dPpdP6*C'; |
|---|
| 127 | dSdP7 = C*dPpdP7*C'; |
|---|
| 128 | dSdP8 = C*dPpdP8*C'; |
|---|
| 129 | dSdP9 = C*dPpdP9*C'; |
|---|
| 130 | dSdP10 = C*dPpdP10*C'; |
|---|
| 131 | dSdP11 = C*dPpdP11*C'; |
|---|
| 132 | dSdP12 = C*dPpdP12*C'; |
|---|
| 133 | dSdP13 = C*dPpdP13*C'; |
|---|
| 134 | dSdP14 = C*dPpdP14*C'; |
|---|
| 135 | |
|---|
| 136 | dKdia = dPpdia*C'/S - Pp*C'/S*dSdia/S; |
|---|
| 137 | dKdib = dPpdib*C'/S - Pp*C'/S*dSdib/S; |
|---|
| 138 | dKdom = dPpdom*C'/S - Pp*C'/S*dSdom/S; |
|---|
| 139 | dKdth = dPpdth*C'/S - Pp*C'/S*dSdth/S; |
|---|
| 140 | dKdP5 = dPpdP5*C'/S - Pp*C'/S*dSdP5/S; |
|---|
| 141 | dKdP6 = dPpdP6*C'/S - Pp*C'/S*dSdP6/S; |
|---|
| 142 | dKdP7 = dPpdP7*C'/S - Pp*C'/S*dSdP7/S; |
|---|
| 143 | dKdP8 = dPpdP8*C'/S - Pp*C'/S*dSdP8/S; |
|---|
| 144 | dKdP9 = dPpdP9*C'/S - Pp*C'/S*dSdP9/S; |
|---|
| 145 | dKdP10 = dPpdP10*C'/S - Pp*C'/S*dSdP10/S; |
|---|
| 146 | dKdP11 = dPpdP11*C'/S - Pp*C'/S*dSdP11/S; |
|---|
| 147 | dKdP12 = dPpdP12*C'/S - Pp*C'/S*dSdP12/S; |
|---|
| 148 | dKdP13 = dPpdP13*C'/S - Pp*C'/S*dSdP13/S; |
|---|
| 149 | dKdP14 = dPpdP14*C'/S - Pp*C'/S*dSdP14/S; |
|---|
| 150 | |
|---|
| 151 | dPnewdia = dPpdia - dKdia*C*Pp - K*C*dPpdia; |
|---|
| 152 | dPnewdib = dPpdib - dKdib*C*Pp - K*C*dPpdib; |
|---|
| 153 | dPnewdom = dPpdom - dKdom*C*Pp - K*C*dPpdom; |
|---|
| 154 | dPnewdth = dPpdth - dKdth*C*Pp - K*C*dPpdth; |
|---|
| 155 | dPnewdP5 = dPpdP5 - dKdP5*C*Pp - K*C*dPpdP5; |
|---|
| 156 | dPnewdP6 = dPpdP6 - dKdP6*C*Pp - K*C*dPpdP6; |
|---|
| 157 | dPnewdP7 = dPpdP7 - dKdP7*C*Pp - K*C*dPpdP7; |
|---|
| 158 | dPnewdP8 = dPpdP8 - dKdP8*C*Pp - K*C*dPpdP8; |
|---|
| 159 | dPnewdP9 = dPpdP9 - dKdP9*C*Pp - K*C*dPpdP9; |
|---|
| 160 | dPnewdP10 = dPpdP10 - dKdP10*C*Pp - K*C*dPpdP10; |
|---|
| 161 | dPnewdP11 = dPpdP11 - dKdP11*C*Pp - K*C*dPpdP11; |
|---|
| 162 | dPnewdP12 = dPpdP12 - dKdP12*C*Pp - K*C*dPpdP12; |
|---|
| 163 | dPnewdP13 = dPpdP13 - dKdP13*C*Pp - K*C*dPpdP13; |
|---|
| 164 | dPnewdP14 = dPpdP14 - dKdP14*C*Pp - K*C*dPpdP14; |
|---|
| 165 | |
|---|
| 166 | |
|---|
| 167 | A_k(1:4,1:4) = A; |
|---|
| 168 | %A_k(1:4,5:14) = zeros(4,10); |
|---|
| 169 | |
|---|
| 170 | pttrn = [1 5 6 9 10 11 13 14 15 16]'; %pattern for selecting proper elements from P matrices |
|---|
| 171 | A_k(5:14,1) = dPnewdia(pttrn); |
|---|
| 172 | A_k(5:14,2) = dPnewdib(pttrn); |
|---|
| 173 | A_k(5:14,3) = dPnewdom(pttrn); |
|---|
| 174 | A_k(5:14,4) = dPnewdth(pttrn); |
|---|
| 175 | A_k(5:14,5) = dPnewdP5(pttrn); |
|---|
| 176 | A_k(5:14,6) = dPnewdP6(pttrn); |
|---|
| 177 | A_k(5:14,7) = dPnewdP7(pttrn); |
|---|
| 178 | A_k(5:14,8) = dPnewdP8(pttrn); |
|---|
| 179 | A_k(5:14,9) = dPnewdP9(pttrn); |
|---|
| 180 | A_k(5:14,10) = dPnewdP10(pttrn); |
|---|
| 181 | A_k(5:14,11) = dPnewdP11(pttrn); |
|---|
| 182 | A_k(5:14,12) = dPnewdP12(pttrn); |
|---|
| 183 | A_k(5:14,13) = dPnewdP13(pttrn); |
|---|
| 184 | A_k(5:14,14) = dPnewdP14(pttrn); |
|---|
| 185 | |
|---|
| 186 | phi = zeros(14,1); |
|---|
| 187 | phi(1) = a*ksi0(1) + b*ksi0(3)*sin(ksi0(4)) + c*uab(1); |
|---|
| 188 | phi(2) = a*ksi0(2) - b*ksi0(3)*cos(ksi0(4)) + c*uab(2); |
|---|
| 189 | phi(3) = d*ksi0(3) + e*(ksi0(2)*cos(ksi0(4)) - ksi0(1)*sin(ksi0(4))); |
|---|
| 190 | phi(4) = ksi0(4) + dt*ksi0(3); |
|---|
| 191 | phi(5:14) = Pnew(pttrn); |
|---|
| 192 | |
|---|
| 193 | A_l(1:14,1:14) = A_k; |
|---|
| 194 | A_l(1:14,15) = phi - A_k*ksi0; |
|---|
| 195 | A_l(15,15) = 1.0; |
|---|
| 196 | |
|---|
| 197 | C_k = zeros(2,14); |
|---|
| 198 | C_k(1:2,1:4) = C; |
|---|
| 199 | |
|---|
| 200 | pre_k = Pnew(pttrn); |
|---|
| 201 | %max x(14) = pi^2/3 ... variance of uniform -pi,pi |
|---|
| 202 | if(pre_k(10) > pi^2/3) |
|---|
| 203 | pre_k(10) = pi^2/3; |
|---|
| 204 | end |
|---|
| 205 | end |
|---|
