/* * Copyright 2004 by Soos, Antal * All rights reserved. Property of Soos, Antal. * Restricted rights to use, duplicate or disclose this code are * granted through contract. */ /***************************************************************************/ /* */ /* H8_MPC . cpp */ /* */ /* Linear H8 Model Predictive Control algorithm inplementation. */ /* */ /* */ /***************************************************************************/ #include "H8_MPC.hpp" /* % Discrete H8_MPC Control. %---------------------------------------------------------------------------- % % MEASUREMENT-FEEDBACK CONTROL: % ___________ % [l] w__i ---->| | % | |-----> z__i [p] % | system | % | |---\ % /-->|___________| | y__i [q] % | | % [r] u__i | | % | ____________ | % | | | | % \---| controller |--/ % |____________| % % %------------------------------------------------------------------------------- % % x = Ax + B1*w + B2*u <=> x = A*x + B*u + D*w A~mxm, B~mxr, D~mxl % y = C1*x + D12*w <=> y = C*x + E*w C~qxm, E~qxl,; H~pxm, G~pxr % z = C2*x + D21*u <=> z = H*x + G*u m=4,p=5,l=4,r=q=1 size(A) = [m x m] size(B) = [m x r] size(D) = [m x l]; size(C) = [q x m] size(E) = [q x l] size(H) = [p x m] size(G) = [p x r] % Naming Convention: % % Xx_x_x % % ^^ ^ ^ % || | |_____ Sup script i+1 <=> ip % || |_______ Super Script i-1 <=> in % ||_________ Name extention % |__________ Name % % %------------------------------------------ % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */ #define PRINT_0x(a,b) print_matrix(a,b) #define PRINT_00(a,b) // print_matrix(a,b) // basic matrices: A,B,C,D,E,G,H #define PRINT_01(a,b) // print_matrix(a,b) #define PRINT_02(a,b) // print_matrix(a,b) #define PRINT_03(a,b) // print_matrix(a,b) // gama search #define PRINT_LOG_00(c) printlog(c) // For basic program flow #define PRINT_LOG_01(c) // printlog(c) #define PRINT_LOG_02(c) // printlog(c) #define PRINT_LOG_03(c) // printlog(c) // gama search #define MAXgama 2e+6 H8MPC_alg::H8MPC_alg( int is,int ia,int ib,int ic,int il,int ir,int ip,int iq) : syscr(is,ia,ib,ic) { sys_l = il; // # of system disturbances sys_r = ir; // # of system control input sys_p = ip; // # of system controlled output sys_q = iq; // # of system measured outputs sys_m = ia; // The system dimension // System matrice for the state space model: //----------------------------------------------------------------------- // x__np = mA * x__n + mB * u__n + mD * w__n // y__n = mC * x__n + mE * w__n // z__n = mH * x__n + mG * u__n matrix_alloc(&mA, sys_m, sys_m); matrix_alloc(&mB, sys_m, sys_r); matrix_alloc(&mD, sys_m, sys_l); matrix_alloc(&mC, sys_q, sys_m); matrix_alloc(&mE, sys_q, sys_l); matrix_alloc(&mH, sys_p, sys_m); matrix_alloc(&mG, sys_p, sys_r); matrix_alloc(&mQ, sys_m, sys_m); // Q = H'*H matrix_alloc(&mS, sys_m, sys_r); // S = H'*G matrix_alloc(&mR, sys_r, sys_r); // R = G'*G matrix_alloc(&mR_inv, sys_r, sys_r); // R_inv = inv(R) matrix_alloc(&mW, sys_m, sys_m); // W = D*D' matrix_alloc(&mL, sys_m, sys_q); // L = D*E' matrix_alloc(&mV, sys_q, sys_q); // V = E*E' matrix_alloc(&mV_inv, sys_q, sys_q); // V_inv = inv(V) matrix_alloc(&mA_bar, sys_m, sys_m); // A_bar = A - B*R_inv*S' matrix_alloc(&mQ_bar, sys_m, sys_m); // Q_bar = Q - S*R_inv*S' matrix_alloc(&mRinvS, sys_r, sys_m); // mRinvS = R_inv*S' matrix_alloc(&mA_tilde, sys_m, sys_m); // A_tilde = A - L*V_inv*C; matrix_alloc(&mW_tilde, sys_m, sys_m); // W_tilde = W - L*V_inv*L' matrix_alloc(&mLVinv, sys_m, sys_q); // mLVinv = L*V_inv matrix_alloc(&mP1tar, sys_m, sys_m); // P1tar [mxm] Riccati result matrix_alloc(&mP2tar, sys_m, sys_m); // P2tar [mxm] Riccati result matrix_alloc(&mP1, sys_m, sys_m); // P1 [mxm] Riccati calculation matrix_alloc(&mP2, sys_m, sys_m); // P2 [mxm] Riccati calculation matrix_alloc(&mP1_inv, sys_m, sys_m); // P1 [mxm] Riccati calculation matrix_alloc(&mP2_inv, sys_m, sys_m); // P2 [mxm] Riccati calculation matrix_alloc(&mP1_tilde, sys_m, sys_m);// P1 [mxm] Riccati1 test matrix_alloc(&mP2_tilde, sys_m, sys_m);// P2 [mxm] Riccati2 test matrix_alloc(&mBD,sys_m,sys_r+sys_l); // matrix_alloc(&mGama_inv_C,sys_r+sys_l,sys_r+sys_l); // inv(Gama__C) matrix_alloc(&mGDPD,sys_l,sys_l); // -gama2 * eye(l) + D'*P1*D (test) matrix_alloc(&mCH,sys_q+sys_p,sys_m); // matrix_alloc(&mGama_inv_F,sys_q+sys_p,sys_q+sys_p); // inv(Gama__F) matrix_alloc(&mGHPH,sys_p,sys_p); // -gama2 * eye(p) + H*P2*H' (test) matrix_alloc(&mKcx, sys_r,sys_m); // u__n = Kcx * x__n + Kcy * y__n matrix_alloc(&mKcy, sys_r,sys_q); // matrix_alloc(&mKfx, sys_m,sys_m); // x__np = Kfx*x__n + Kfu*u__n + Kfy*y__n matrix_alloc(&mKfu, sys_m,sys_r); // matrix_alloc(&mKfy, sys_m,sys_q); // matrix_alloc(&mT1, sys_p+sys_q, sys_p+sys_q); // temporary matrix matrix_alloc(&mT2, sys_p+sys_q, sys_p+sys_q); // temporary matrix matrix_alloc(&mT3, sys_p+sys_q, sys_p+sys_q); // temporary matrix matrix_alloc(&mT4, sys_p+sys_q, sys_p+sys_q); // temporary matrix matrix_alloc(&mru, sys_r,1); // system output vector matrix_alloc(&mry, sys_q,1); // system input vector matrix_alloc(&mrx, sys_m,1); // system state matrix_alloc(&mrT1, sys_m,sys_m); // real-time temporary matrix matrix_alloc(&mrT2, sys_m,sys_m); // real-time temporary matrix gama_0 = 0.0 ; // gama recursion initialization } //------------------------------------------------------------------------------ // Implementation: //------------------------------------------------------------------------------ /* %=============================================================================== */ void H8MPC_alg::update(Matrix *psy, float gama_i) { int i,j; char ch; float gama; float gama_F; float gama_P; float ftemp1; float ftemp2; int gama_iteration; int gama_found; int error_flag; int break_flag; float gama2; // = gama^2 float gama2_inv; // = 1/(gama^2) //% system definition and update: //%------------------------- // mA = [-a1 1 0 0 <> // -a2 0 1 0 // -a3 0 0 1 // -a4 0 0 0] for(i=1; i<=sys_m;i++) mA.mtx[i][i+1] = 1; for(i=1; i<=sys_m;i++) mA.mtx[i][1] = -psy->mtx[m_s+i][1]; PRINT_00(&mA,"mA"); // mB = [b1; b2; b3; b4] <> for(i=1; i<=sys_m;i++) mB.mtx[i][1] = psy->mtx[m_s+m_a+i][1]; PRINT_00(&mB,"mB"); // mC = [1 0 0 0] <> for(i=1; i<=sys_m;i++) mC.mtx[1][i] = 0; mC.mtx[1][1] = (float) 1; PRINT_00(&mC,"mC"); //D <- c_i <----------------------------- for(i=1; i<=sys_m;i++) { for(j=1; j<=sys_l;j++) mD.mtx[i][i] = 0.0 ;} for(i=1; i<=sys_m;i++) mD.mtx[i][i] = psy->mtx[m_s+m_a+m_b+i][1]; PRINT_00(&mD,"mD"); ftemp1 =0.0; for(i=1; i<=sys_m;i++) ftemp1 += psy->mtx[m_s+m_a+m_b+i][1]; // mE = [ e e e] <> for(i=1; i<=sys_q;i++) { for(j=1; j<=sys_l;j++) mE.mtx[i][j] = ftemp1;}//*e_value[j] ; } // 0.025 PRINT_00(&mE,"mE"); // Calculate the matrices: //---------------------------------------------- mulmat_t1(&mQ, &mH, &mH); // Q = H'*H mulmat_t1(&mS, &mH, &mG); // S = H'*G mulmat_t1(&mR, &mG, &mG); // R = G'*G minv(&mR_inv, &mR); // R_inv = inv(R) PRINT_03(&mQ,"mQ"); PRINT_03(&mS,"mS"); PRINT_03(&mR,"mR"); PRINT_03(&mR_inv,"mR_inv"); //---------------------------------------------- mulmat_t2(&mW, &mD, &mD); // W = D*D' <-- mulmat_t2(&mL, &mD, &mE); // L = D*E' <-- mulmat_t2(&mV, &mE, &mE); // V = E*E' minv(&mV_inv, &mV); // V_inv = inv(V) PRINT_03(&mW,"mW"); // <-- PRINT_03(&mL,"mL"); // <-- PRINT_03(&mV,"mV"); PRINT_03(&mV_inv,"mV_inv"); //---------------------------------------------- //mQ_bar; // Q_bar = Q - S*R_inv*S' mulmat_t2(&mRinvS, &mR_inv, &mS); // mRinvS = R_inv*S' PRINT_01(&mRinvS,"mRinvS"); mulmat(&mT1, &mS, &mRinvS); // <= S*R_inv*S' submat(&mQ_bar, &mQ, &mT1); // Q_bar = Q - S*R_inv*S' PRINT_03(&mQ_bar,"mQ_bar"); //mW_tilde; // W_tilde = W - L*V_inv*L' mulmat(&mLVinv, &mL, &mV_inv); // mLVinv = L*V_inv <-- PRINT_01(&mLVinv,"mLVinv"); // <-- mulmat_t2(&mT1, &mLVinv, &mL); // <= L*V_inv*L' <-- PRINT_00(&mT1,"L*Vinv*L'"); submat(&mW_tilde, &mW, &mT1); // W_tilde = W - L*V_inv*L' <-- PRINT_03(&mW_tilde,"mW_tilde"); // <-- /* //for(i=1; i<=sys_m;i++) mD.mtx[i][1] = psy->mtx[m_s+i][1]; //for(i=2; i<=sys_m;i++) mD.mtx[i][2] = psy->mtx[m_s+i][1]; ////////for(i=1; i<=sys_m;i++) mD.mtx[i][3] = -psy->mtx[m_s+i][1]*0.1; /////////for(i=1; i<=sys_m;i++) mD.mtx[i][4] = psy->mtx[m_s+i][1]*0.1; //for(i=3; i<=sys_m;i++) mD.mtx[4][i] = psy->mtx[m_s+i][1]; //for(i=1; i<=sys_m-2;i++) mD.mtx[i][5] = psy->mtx[m_s+i][1]; PRINT_00(&mD,"mD"); mulmat_t2(&mW, &mD, &mD); // W = D*D' mulmat_t2(&mL, &mD, &mE); // L = D*E' PRINT_01(&mW,"mW"); PRINT_01(&mL,"mL"); //---------------------------------------------- //mW_tilde; // W_tilde = W - L*V_inv*L' mulmat(&mLVinv, &mL, &mV_inv); // mLVinv = L*V_inv PRINT_01(&mLVinv,"mLVinv"); mulmat_t2(&mT1, &mLVinv, &mL); // <= L*V_inv*L' submat(&mW_tilde, &mW, &mT1); // W_tilde = W - L*V_inv*L' PRINT_01(&mW_tilde,"mW_tilde"); //D <- a <------------------------------------------ */ // Calculate the matrices: // mBD <- [B D] concat_row(&mBD,&mB,&mD); // BD <- [B D] // mCH <- [C; H] concat_col(&mCH, &mC, &mH); // CH <- [C; H] PRINT_01(&mBD,"mBD"); PRINT_01(&mCH,"mCH"); //mA_bar; // A_bar = A - B*R_inv*S' mulmat(&mT1, &mB, &mRinvS); // <= B*R_inv*S' = B*mRinvS submat(&mA_bar, &mA, &mT1); // A_bar = A - B*R_inv*S' PRINT_01(&mA_bar,"mA_bar"); //mA_tilde; // A_tilde = A - L*V_inv*C mulmat(&mT1, &mLVinv, &mC); // <= L*V_inv*C = mLVinv*C submat(&mA_tilde, &mA, &mT1); // A_tilde = A - L*V_inv*C PRINT_01(&mA_tilde,"mA_tilde"); // Resursion initialization if(gama_0<=0.0) gama=gama_i; //If gama is not initializef proprtly else gama = gama_0; gama_F = 0.0; gama_P = 0.0; gama_iteration = 1; gama_found = FAIL; // for the last iteration with gama = 1.1 gama_0 // ---->printf("\n **##** gama = %g \n",gama); while(gama_iteration) // The loop for the optimal gama search { PRINT_LOG_01("while: in gama_iteration...########################------->"); // printf("\n gama = %g \n",gama); // PRINT for test error_flag = FAIL; // set the eroor_flaf to > 0 while(error_flag) //This is used to jump out if the ptest() fail ======================= { PRINT_LOG_01("while in error_flag loop ...--------->"); break_flag = 0; // retrive the values from the privious optimization period: copymat(&mP1, &mP1tar); //P1 <- P1tar copymat(&mP2, &mP2tar); //P2 <- P2tar PRINT_01(&mP1,"P1_retrive"); PRINT_01(&mP2,"P2_retrive"); gama2 = gama*gama; gama2_inv = 1/gama2; /*for i=1:500 Gama_C = [R + B'*P1*B B'*P1*D; D'*P1*B -gama2 * eye(l) + D'*P1*D ]; Gama_inv_C = pinv(Gama__C); P1 = A_bar' * P1 * A_bar - A_bar'* P1 * [B D] * Gama_inv_C * [B'; D'] * P1 *A_bar + Q_bar; end*/ PRINT_LOG_02("P1_iteration...*** 50x ------------>"); for(i=50; i>0;i--) { // Gama__C = :: // R + B'*P1*B mulmat_t1(&mT3,&mB,&mP1); // mT3 <- B'*P1 mulmat(&mT2,&mT3,&mB); // B'*P1*B addmat(&mT1,&mR,&mT2); // mT1 <- R + B'*P1*B PRINT_01(&mT1,"R + B'*P1*B"); mulmat(&mT2,&mT3,&mD); // mT2 <- B'*P1*D PRINT_01(&mT2,"B'*P1*D"); // R + B'*P1*B B'*P1*D; concat_row(&mT3,&mT1,&mT2); // mT3 <- R + B'*P1*B B'*P1*D PRINT_01(&mT3,"R + B'*P1*B B'*P1*D"); // D'*P1*B mulmat_t1(&mT4,&mD,&mP1); // mT4 <- D'*P1 mulmat(&mT1,&mT4,&mB); // mT1 <- D'*P1*B PRINT_01(&mT1,"D'*P1*B "); // -gama2 * eye(l) + D'*P1*D mulmat(&mT2,&mT4,&mD); // mT2 <- D'*P1*D PRINT_01(&mT2,"D'*P1*D "); // -gama2 * eye(l) + D'*P1*D diagn(&mT4,sys_l,(-1*gama2)); // <- -gama2 * eye(l) addmat(&mGDPD, &mT4, &mT2); // mGDPD <- [ ] PRINT_01(&mGDPD,"mGDPD "); // [ D'*P1*B -gama2 * eye(l) + D'*P1*D ] concat_row(&mT4,&mT1,&mGDPD); // mT4 <- [ ] PRINT_01(&mT4," D'*P1*B -gama2 * eye(l) + D'*P1*D "); // [ R+B'*P1*B B'*P1*D ] // [ D'*P1*B -gama2*eye(l)+D'*P1*D ] concat_col(&mT1, &mT3, &mT4); PRINT_01(&mT1," R + B'*P1*B B'*P1*D; D'*P1*B -gama2 * eye(l) + D'*P1*D "); minv(&mGama_inv_C,&mT1); PRINT_01(&mGama_inv_C,"mGama_inv_C "); //P1 = :: // + A_bar' * P1 * A_bar mulmat_t1(&mT2,&mA_bar,&mP1); // mT2 <- A_bar' * P1 mulmat(&mT1,&mT2,&mA_bar); // mT1 <- A_bar' * P1 * A_bar PRINT_01(&mT1,"A_bar' * P1 * A_bar "); // - A_bar'* P1 * [B D] * Gama_inv_C * [B'; D'] * P1 * A_bar mulmat(&mT3,&mT2,&mBD); // A_bar' * P1 * [B D] mulmat(&mT2,&mT3,&mGama_inv_C); // * Gama_inv_C mulmat_t2(&mT3,&mT2,&mBD); // * [B'; D'] mulmat(&mT2,&mT3,&mP1); // * P1 mulmat(&mT3,&mT2,&mA_bar); // mT3 <- * A_bar PRINT_01(&mT3,"- A_bar'* P1 * [B D] * Gama_inv_C * [B'; D'] * P1 * A_bar "); //A_bar'*P1*A_bar-A_bar'*P1*[BD]*Gama_inv_C*[B';D']*P1*A_bar submat(&mT2,&mT1,&mT3); // [ ] - [ ] addmat(&mP1,&mT2,&mQ_bar); // P1 <= PRINT_01(&mP1,"mP1 after recursion"); // printlog(" positivity 1 tests FOLOWS ->->->->->->->->->>->->->->"); PRINT_LOG_02("pre P1 positivity test"); copymat(&mT1, &mP1); //if(ptest(&mT1)==0) break; if(ptest(&mT1)==0) { PRINT_01(&mP1,"mP1 Fail Test0"); PRINT_LOG_03(" P1 positivity test FAIL !"); break_flag = 1; break; } PRINT_01(&mT1,"mP1 after ptest"); PRINT_LOG_02(" positivity tests 1 PASS "); PRINT_LOG_02("pre -GDPD positivity test folows"); cmulmat(&mT1,-1.0,&mGDPD); // gama2_inv*W if(ptest(&mT1)==0) { PRINT_01(&mGDPD,"-GDPD Fail Test1"); PRINT_LOG_03("-GDPD positivity test FAIL !"); break_flag = 1; break; } PRINT_LOG_02(" positivity tests P1 & -GDPD PASS >><"); } if(break_flag) PRINT_LOG_03(" ### P1 test FAIL <<<<<<<<<<< "); if(break_flag)break ; PRINT_LOG_03(" P1 found >>>>>>>>>>>>>><<<<<<<<<<"); PRINT_01(&mGDPD,"mGDPD Test1 after"); //ptest(&mGDPD); PRINT_01(&mGDPD,"mGDPD Test1 x 2 after"); PRINT_01(&mP1,"mP1 prior"); minv(&mP1_inv,&mP1); //P1_inv = inv(P1); PRINT_01(&mP1,"mP1 after"); PRINT_01(&mP1_inv,"mP1_inv after"); PRINT_LOG_01("P2_iteration...*** 50x ------------->"); /* for i=1:500 Gama__F = [V + C*P2*C' C*P2*H'; H*P2*C' -gama2 * eye(p) + H*P2*H' ]; Gama_inv_F = pinv(Gama__F); P2 = A_tilde*P2*A_tilde' - A_tilde*P2* [C' H'] * Gama_inv_F * [C; H] *P2*A_tilde' + W_tilde PRINT_LOG_01(" positivity 3+ tests FOLOWS ->->->->->->->->->>->->->->"); end */ for(i=50; i>0;i--) { // Gama__F :: //V + C*P2*C' mulmat(&mT3,&mC,&mP2); // mT3 <- C*P2 mulmat_t2(&mT2,&mT3,&mC); // * C' addmat(&mT1,&mV,&mT2); // mT1 <- V + C*P2*C' PRINT_01(&mT1,"V + C*P2*C' "); //C*P2*H' mulmat_t2(&mT2,&mT3,&mH); // mT2 <- C*P2* H' PRINT_01(&mT2,"C*P2* H' "); concat_row(&mT3,&mT1,&mT2); // mT3 <- [V+C*P2*C' C*P2*H'] PRINT_01(&mT3,"[V+C*P2*C' C*P2*H'] "); //H*P2*C' mulmat(&mT4,&mH,&mP2); // mT4 <- C*P2 mulmat_t2(&mT1,&mT4,&mC); // mT1 <- H*P2*C' PRINT_01(&mT1,"H*P2*C' "); //-gama2 * eye(p) + H*P2*H' mulmat_t2(&mT2,&mT4,&mH); // mT2 <- H*P2*H' diagn(&mT4,sys_p,(-1*gama2)); // <- -gama2 * eye(p) addmat(&mGHPH, &mT4, &mT2); // mGHPH <- [ ] PRINT_00(&mGHPH,"-gama2 * eye(p) + H*P2*H' "); // [ H*P2*C' -gama2 * eye(p) + H*P2*H' ] concat_row(&mT4,&mT1,&mGHPH); // mT4 <- [ ] PRINT_01(&mT4,"[ H*P2*C' -gama2 * eye(p) + H*P2*H' ] "); // [V + C*P2*C' C*P2*H' ] // [H*P2*C' -gama2*eye(p)+H*P2*H' ] concat_col(&mT1, &mT3, &mT4); PRINT_01(&mT1,"[V + C*P2*C' C*P2*H'; H*P2*C' -gama2 * eye(p) + H*P2*H' ] "); PRINT_01(&mT1,"mGama__F"); minv(&mGama_inv_F,&mT1); //<- inv(mGama__F) PRINT_01(&mGama_inv_F,"mGama_inv_F"); // P2 :: // A_tilde*P2*A_tilde' mulmat(&mT2,&mA_tilde,&mP2); // mT2 <- A_ tilde* P2 mulmat_t2(&mT1,&mT2,&mA_tilde); // mT1 <- A_tilde*P2*A_tilde' PRINT_01(&mT1,"A_tilde*P2*A_tilde' "); // - A_tilde*P2* [C' H'] * Gama_inv_F * [C; H] *P2*A_tilde' mulmat_t2(&mT3,&mT2,&mCH); // * [C' H'] mulmat(&mT2,&mT3,&mGama_inv_F); // * mGama_inv_F mulmat(&mT3,&mT2,&mCH); // * mCH mulmat(&mT2,&mT3,&mP2); // * P2 mulmat_t2(&mT3,&mT2,&mA_tilde); // * mA_tilde' PRINT_01(&mT3,"- A_tilde*P2* [C' H'] * Gama_inv_F * [C; H] *P2*A_tilde' "); //A_tilde*P2*A_tilde' - A_tilde*P2*[C'H']*Gama_inv_F*[C;H]*P2*A_tilde' submat(&mT2,&mT1,&mT3); // [ ] - [ ] PRINT_00(&mT2,"A_tilde*P2*A_tilde' - A_tilde*P2*[C'H']*Gama_inv_F*[C;H]*P2*A_tilde' "); //+ W_tilde addmat(&mP2,&mT2,&mW_tilde); // P2 <= PRINT_00(&mP2,"mP2 "); PRINT_LOG_01(" positivity P2 tests FOLOWS ->->->->->->->->->>->->->->"); //pause(); copymat(&mT1, &mP2); //if(ptest(&mT1)==0) break; if(ptest(&mT1)==0) { PRINT_01(&mP2,"mP2 Fail Test2"); PRINT_LOG_01(" positivity P2 tests FAIL"); break_flag = 1; break; } PRINT_LOG_01(" positivity tests P2 PASS >>>>>>>>>>>>>><<<<<<<<<<"); //pause(); PRINT_LOG_01(" positivity GHPH tests FOLOWS ->->->->->->->->->>->->->->"); cmulmat(&mT1,-1.0,&mGHPH); // <- -GHPH if(ptest(&mT1)==0) { PRINT_01(&mGHPH,"-mGHPH Fail Test3"); PRINT_LOG_01(" positivity -GHPH tests FAIL"); break_flag = 1; break; } PRINT_LOG_01(" positivity tests P2 & -GHPH PASS >>>>>>>>>>>>>><<<<<<<<<<"); } if(break_flag) PRINT_LOG_03(" >>>>>>>>>>>>>><<<<<<<<< P2 test FAIL ><<<<<<<<<< "); if(break_flag)break ; PRINT_LOG_03(" >>>>>>>>>>>>>><<<<<<<<<< P2 found >>>>>>>>>>>>>><<<<<<<<<< "); minv(&mP2_inv,&mP2);// P2_inv = inv(P2); // P1_tilde = A_bar'*inv(P1_inv-gama2_inv*W)*A_bar + Q_bar; cmulmat(&mT1,gama2_inv,&mW); // gama2_inv*W submat(&mT2,&mP1_inv,&mT1); // P1_inv-gama2_inv*W minv(&mT1,&mT2); // <- inv(P1_inv-gama2_inv*W) mulmat_t1(&mT2,&mA_bar,&mT1); // mA_bar' * mulmat(&mT1,&mT2,&mA_bar); // * mA_bar addmat(&mP1_tilde,&mT1,&mQ_bar); // P1_tilde <= [] + Q_bar // P2_tilde = A_tilde*inv(P2_inv-gama2_inv*Q)*A_tilde' + W_tilde; cmulmat(&mT1,gama2_inv,&mQ); // gama2_inv*Q submat(&mT2,&mP2_inv,&mT1); // P2_inv-gama2_inv*Q minv(&mT1,&mT2); // <- inv(P2_inv-gama2_inv*Q) mulmat(&mT2,&mA_tilde,&mT1); // mA_tilde * mulmat_t2(&mT1,&mT2,&mA_tilde); // * mA_tilde addmat(&mP2_tilde,&mT1,&mW_tilde); // P2_tilde <= [] + W_tilde PRINT_LOG_01(" positivity P1_tilde tests FOLOWS ->->->->->->->->->>->->->->"); copymat(&mT1, &mP1_tilde); if(ptest(&mT1)==0) { PRINT_01(&mP1_tilde,"mP1_tilde Fail Test4"); PRINT_LOG_03(" positivity P1_tilde tests FAIL"); break; } PRINT_LOG_01(" positivity P1_tilde tests PASS "); PRINT_LOG_01(" positivity tests P2*P1_tilde FOLOWS ->->->->->->->->->>->->->->"); mulmat(&mT2,&mP2,&mP1_tilde);// mT2 <- P2*P1_tilde diagn(&mT1,sys_m,gama2); // mT1 <- gama2 * eye(m) submat(&mT3,&mT1,&mT2); // <- gama2*eye(m) - P2*P1_tilde if(ptest(&mT3)==0) { PRINT_01(&mT3 ,"P2*P1_tilde Fail Test5"); PRINT_LOG_03(" positivity P2*P1_tilde tests FAIL"); break; } PRINT_LOG_01(" positivity P2*P1_tilde tests PASS "); PRINT_LOG_01(" positivity tests P2_tilde FOLOWS ->->->->->->->->->>->->->->"); copymat(&mT1, &mP2_tilde); if(ptest(&mT1)==0) { PRINT_01(&mP2_tilde,"mP2_tilde Fail Test6"); PRINT_LOG_03(" positivity P2_tilde tests FAIL"); break; } PRINT_LOG_01(" positivity tests P2_tilde PASS "); PRINT_LOG_01(" positivity tests P2_tilde*P1 FOLOWS ->->->->->->->->->>->->->->"); mulmat(&mT2,&mP2_tilde,&mP1);// mT2 <- P2_tilde*P1 diagn(&mT1,sys_m,gama2); // mT1 <- gama2 * eye(m) submat(&mT3,&mT1,&mT2); // <- gama2*eye(m)-P2_tilde*P1 if(ptest(&mT3)==0) { PRINT_01(&mT3 ,"P2_tilde*P1 Fail Test7"); PRINT_LOG_03(" positivity tests P2_tilde*P1 FAIL"); break; } PRINT_LOG_01(" positivity tests PASS "); PRINT_LOG_03(" All positivity tests P2_tilde*P1 PASS #@@@@@@@@@@@@@@@@@"); error_flag = OK; // clear the eroor_flag to 0 <=> while loop end } //while(error_flag) ================================================= //if (gama > 10000) pause(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< PRINT_LOG_01(" gama search folows:::::::::::::::::::::::::::::::::"); if((error_flag+gama_found)>0) { if(error_flag == OK) { gama_P = gama, gama_0 = gama ;} else gama_F = gama; if(gama_P <= 0.0) gama = 2* gama; else gama = (gama_F+gama_P)/2; ftemp1 = (gama_P-gama_F); if(ftemp1 > 0) { //Is the iteration at the end if((ftemp1/gama_F)< GAMA_TOL){ // The iteration is finished gama = gama_0 * 1.1; gama_found = OK; gama_P = 0.0; // If an error ocurs is the last ieration start ower. } } } else gama_iteration = 0 ; // end the iteration!! if( gama > MAXgama) { // PRINT_LOG_00("--> gama > MAXgama "); PRINT_LOG_00("--> gama > MAXgama --> return:: gama > ## @ ## "); return; } /// <------------------- Big trouble }//while(gama_iteration) //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ PRINT_LOG_01("The controller hase been found ------>:::::::::::::::::::::"); // Save the values until the next optimization period copymat(&mP1tar, &mP1); //P1tar <- P1 copymat(&mP2tar, &mP2); //P2tar <- P2 PRINT_01(&mP1,"P1 after gama iteration"); PRINT_01(&mP2,"P2 after gama iteration"); /* % The controller calculation: %------------------------------------------- */ PRINT_LOG_01("delta_c "); // delta_c = inv(P1_inv + B*R_inv*B' - gama2_inv*W); mulmat(&mT1,&mB,&mR_inv); // mT1 <- B*R_inv mulmat_t2(&mT2,&mT1,&mB); // mT2 <- B*R_inv*B' cmulmat(&mT1,gama2_inv,&mW);// mT1 <- gama2_inv*Q submat(&mT3,&mT2,&mT1); // mT3 <- B*R_inv*B' - gama2_inv*W addmat(&mT2,&mT3,&mP1_inv); // P1_inv + B*R_inv*B' - gama2_inv*W minv(&mT1,&mT2); // mT1 = delta_c <- inv( ) <<< PRINT_LOG_01(" delta_s "); // delta_s = inv(eye(m) - gama2_inv*P2*P1); mulmat(&mT2,&mP2,&mP1); // mT2 <- P2*P1 cmulmat(&mT3,gama2_inv,&mT2);// mT3 <- gama2_inv*P2*P1 diagn(&mT4,sys_m,1.0); // mT4 <- eye(m) submat(&mT3,&mT4,&mT3); // mT3 <- B*R_inv*B' - gama2_inv*W minv(&mT2,&mT3); // mT2 = delta_s <- inv( ) <<< PRINT_LOG_01(" Kcx "); // Kcx = -(R_inv * (B' * delta_c * A_bar + S') * delta_s) ; mulmat_t1(&mT3,&mB,&mT1); // mT3 <- B' * delta_c mulmat(&mT1,&mT3,&mA_bar); // mT1 <- * A_bar transpose(&mT3,&mS); // mT3 <- S' addmat(&mT4,&mT1,&mT3); // mT4 <- (B'*delta_c*A_bar+S') mulmat(&mT1,&mR_inv,&mT4); // mT1 <- R_inv * mulmat(&mT3,&mT1,&mT2); // mT3 <- * delta_s cmulmat(&mKcx,-1.0,&mT3); // Kcx <- -() // Kcy = 0; //zeromat(&mKcy); /*% The prediction system weights calculation: %-------------------------------------------------- */ PRINT_LOG_01("Delta_F "); // Delta_F = inv(P2_inv + C'*V_inv*C - gama2_inv*Q ); mulmat_t1(&mT1,&mC,&mV_inv); // mT1 <- C' * V_inv mulmat(&mT2,&mT1,&mC); // mT2 <- * C addmat(&mT1,&mP2_inv,&mT2); // mT1 <- P2_inv + C'*V_inv*C cmulmat(&mT2,gama2_inv,&mQ); // mT2 <- gama2_inv*Q submat(&mT3,&mT1,&mT2); // mT3 <- P2_inv+C'*V_inv*C-gama2_inv*Q minv(&mT2,&mT3); // mT2 = Delta_F <- inv( ) mulmat(&mT1,&mA_tilde,&mT2); // mT1 = A_tilde*Delta_F <<< PRINT_LOG_01("Kfu "); // Kfu = B + gama2_inv*A_tilde*Delta_F*S; mulmat(&mT4,&mT1,&mS); // mT4 <- A_tilde*Delta_F*S cmulmat(&mT3,gama2_inv,&mT4); // mT3 < gama2_inv*A_tilde*Delta_F*S addmat(&mKfu,&mB,&mT3); // Kfu <-- PRINT_LOG_01(" Kfy "); // Kfy = (A_tilde*Delta_F*C' +L)*V_inv; mulmat_t2(&mT3,&mT1,&mC); // mT3 <- A_tilde*Delta_F*C' addmat(&mT2,&mT3,&mL); // mT2 <- (A_tilde*Delta_F*C' +L) mulmat(&mKfy,&mT2,&mV_inv); // Kfy <-- (A_tilde*Delta_F*C' +L)*V_inv PRINT_LOG_01(" Kfx "); // Kfx = A + gama2_inv*A_tilde*Delta_F*Q - (A_tilde*Delta_F*C' + L)*V_inv*C; mulmat(&mT4,&mT1,&mQ); // mT4 <- A_tilde*Delta_F*Q cmulmat(&mT3,gama2_inv,&mT4); // mT3 < gama2_inv*A_tilde*Delta_F*Q addmat(&mT1,&mA,&mT3); // mT1 <- A + gama2_inv*A_tilde*Delta_F*Q mulmat(&mT2,&mKfy,&mC); // mT2 <- (A_tilde*Delta_F*C' + L)*V_inv*C submat(&mKfx,&mT1,&mT2); // Kfx <-- newsystem = 1; // Flag for the new contrlo system PRINT_LOG_01("---------------> end H8MPC_update "); //pause(); // %------------------------------------------------------------ printf("||| gama = %g |||",gama); // PRINT for test } // end H8MPC_update //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ float H8MPC_alg::rtm_control(float fy__i, float fu__i) { int i; volatile float ftemp; // Kip the memory currnt: for (i=1; i0; i--) ftemp = rtm_update(y_i_vector[i],u_i_vector[i]) ; return rtm_update( fy__i, fu__i) ; } else return rtm_update( fy__i, fu__i) ; } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ float H8MPC_alg::rtm_update(float fy__i, float fu__i) { // fy__i is a BP filtered signal float ftemp, control; extern long int idnum; extern double time_s; extern int icomments; extern FILE *fp_H8; extern FILE *fp_H8x; int i; if(icomments == 0 ) fprintf(fp_H8,"# time, \t u_sys, \t y_sys, \t y_fil, \t u_con \n"); if(icomments == 0 ) fprintf(fp_H8x,"# time, \t x(1),...,x(m) \n"); fprintf(fp_H8x," %f ",time_s); fprintf(fp_H8," %f %f %f ", time_s,fu__i,fy__i); // yi_1 = fy__i + yi_1 * 0.3 ; // fy__i = yi_1; // yi_5 = yi_4; // yi_4 = yi_3; // yi_3 = yi_2; // yi_2 = yi_1; // yi_1 = fy__i; // fy__i = (yi_1 + yi_2 + yi_3 + yi_4 + yi_5) /5.0 ; // <-- // fy__i = (yi_1 + yi_2 + yi_3 + yi_4 ) /4.0 ; // fy__i = (yi_1 + yi_2 + yi_3 ) /3.0 ; // fy__i = (yi_1 + yi_2 ) /2.0 ; mry.mtx[1][1] = fy__i; mru.mtx[1][1] = fu__i; // x__np = Kfx * x__n + Kfu * u__n + Kfy * y__n mulmat(&mrT1,&mKfx,&mrx); // mrT1 <- Kfx * x__n mulmat(&mrT2,&mKfu,&mru); // mrT2 <- Kfu * u__n addmat(&mrx,&mrT1,&mrT2); // <- Kfx * x__n + Kfu * u__n mulmat(&mrT1,&mKfy,&mry); // mrT1 <- Kfy * y__n addmat(&mrx,&mrx,&mrT1); // <-- x__np mulmat(&mru,&mKcx,&mrx); // <-- u__n PRINT_01(&mKcx,"m"); control = mru.mtx[1][1]; for(i=1;i<=sys_m;i++) fprintf(fp_H8x," %f ",mrx.mtx[i][1]); fprintf(fp_H8x," \n"); // control = 0; fprintf(fp_H8," %f %f \n",fy__i,control); yo_5 = yo_4; yo_4 = yo_3; yo_3 = yo_2; yo_2 = yo_1; yo_1 = control; //control = yo_1 + 0.6*yo_2 + 0.36*yo_3 + 0.22*yo_4 + 0.13*yo_5 ; //control = yo_1 + 0.3*yo_2 + 0.15*yo_3 + 0.08*yo_4 + 0.04*yo_5 ; // control = (yo_1 + yo_2 + yo_3 + yo_4 + yo_5)/5.0 ; // control = (yo_1 + yo_2 + yo_3 + yo_4 )/4.0 ; // control = (yo_1 + yo_2 + yo_3 )/3.0 ; // control = (yo_1 + yo_2 )/2.0 ; return control ; } //------------------------------------------------------------------------------ //----------------------------------------------------------------------------- int H8MPC_alg::weightSet() { int i,j; //float e_value[] = {10, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,1.0, 1.0, 10, 0};// if e ++ => gama -- //float h_value[] = {10, 1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 10,0}; // //float h_value[] = {10, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.85, 0.99, 10,0}; // // float h_value[] = {10, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10,0}; // float g_value = 1.2 ; // float d1_value[] = {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.0}; // float d2_value[] = {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0.0}; // float d3_value[] = {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0.0}; // float d4_value[] = {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0.0}; // float d5_value[] = {0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0.0}; //% x = Ax + B1*w + B2*u <=> x = A*x + B*u + D*w A~mxm, B~mxr, D~mxl //% y = C1*x + D12*w <=> y = C*x + E*w C~qxm, E~qxl,; H~pxm, G~pxr //% z = C2*x + D21*u <=> z = H*x + G*u // m=5, r=1, l=m=5, p=m+r=6, q=1 // mD = [d11 d12 d13 <> <-> [5 x 5] // d21 d22 d23 // d31 d32 d33 // d41 d42 d43] // for(i=1; i<=sys_m;i++) { // for(j=1; j<=sys_l;j++) mD.mtx[i][j] = 0; } // for(i=1; i<=sys_m;i++) mD.mtx[1][i] = d1_value[i]; // 1...m -0.3 // for(i=1; i<=sys_m;i++) mD.mtx[2][i] = d2_value[i]; // 1...m 0.5 // for(i=1; i<=sys_m;i++) mD.mtx[3][i] = d3_value[i]; // 1...m 0.4 // for(i=1; i<=sys_m;i++) mD.mtx[4][i] = d4_value[i]; // 1...m 0.5 // for(i=1; i<=sys_m;i++) mD.mtx[5][i] = d5_value[i]; // 1...m 0.4 #define wG 1.0 // 1.0 0.8 ; #define wD 0.06 // 0.01 0.01; 0.06 ,0.02 #define wE 0.0004 // 0.001 0.00001; 0.001 .0.18 #define wH 0.06 // 0.2 0.1 ; for(i=1; i<=sys_m;i++) { for(j=1; j<=sys_l;j++) mD.mtx[i][i] = wD ;}//* mD.mtx[i][j]; } // 0.00125 PRINT_00(&mD,"mD"); // mE = [ e e e] <> for(i=1; i<=sys_q;i++) { for(j=1; j<=sys_l;j++) mE.mtx[i][j] = wE ;}//*e_value[j] ; } // 0.025 PRINT_00(&mE,"mE"); // mH = [h11 0 0 0 <

> // 0 h22 0 0 // 0 0 h33 0 // 0 0 0 h44 // 0 0 0 0] for(i=1; i<=sys_p;i++) { for(j=1; j<=sys_m;j++) mH.mtx[i][j] = 0.0;} for(i=1; i<=sys_m;i++) mH.mtx[i][i] = wH; //*h_value[i]; //0.08 PRINT_00(&mH,"mH"); // mG = [0; 0; 0; 0; g1] <

> for(i=1; i<=sys_p;i++) { for(j=1; j<=sys_r;j++) mG.mtx[i][j] = 0.0 ;} mG.mtx[sys_p][sys_r] = wG ; // 1 PRINT_00(&mG,"mG"); // Calculate the matrices: //---------------------------------------------- mulmat_t1(&mQ, &mH, &mH); // Q = H'*H mulmat_t1(&mS, &mH, &mG); // S = H'*G mulmat_t1(&mR, &mG, &mG); // R = G'*G minv(&mR_inv, &mR); // R_inv = inv(R) PRINT_03(&mQ,"mQ"); PRINT_03(&mS,"mS"); PRINT_03(&mR,"mR"); PRINT_03(&mR_inv,"mR_inv"); //---------------------------------------------- mulmat_t2(&mW, &mD, &mD); // W = D*D' <-- mulmat_t2(&mL, &mD, &mE); // L = D*E' <-- mulmat_t2(&mV, &mE, &mE); // V = E*E' minv(&mV_inv, &mV); // V_inv = inv(V) PRINT_03(&mW,"mW"); // <-- PRINT_03(&mL,"mL"); // <-- PRINT_03(&mV,"mV"); PRINT_03(&mV_inv,"mV_inv"); //---------------------------------------------- //mQ_bar; // Q_bar = Q - S*R_inv*S' mulmat_t2(&mRinvS, &mR_inv, &mS); // mRinvS = R_inv*S' PRINT_01(&mRinvS,"mRinvS"); mulmat(&mT1, &mS, &mRinvS); // <= S*R_inv*S' submat(&mQ_bar, &mQ, &mT1); // Q_bar = Q - S*R_inv*S' PRINT_03(&mQ_bar,"mQ_bar"); //mW_tilde; // W_tilde = W - L*V_inv*L' mulmat(&mLVinv, &mL, &mV_inv); // mLVinv = L*V_inv <-- PRINT_01(&mLVinv,"mLVinv"); // <-- mulmat_t2(&mT1, &mLVinv, &mL); // <= L*V_inv*L' <-- PRINT_00(&mT1,"L*Vinv*L'"); submat(&mW_tilde, &mW, &mT1); // W_tilde = W - L*V_inv*L' <-- PRINT_03(&mW_tilde,"mW_tilde"); // <-- return 1; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- H8MPC_alg::~H8MPC_alg() // deInitialization { // Remouve the alocation from the heep matrix_delete(&mA); matrix_delete(&mB); matrix_delete(&mD); matrix_delete(&mC); matrix_delete(&mE); matrix_delete(&mH); matrix_delete(&mG); matrix_delete(&mQ); matrix_delete(&mS); matrix_delete(&mR); matrix_delete(&mR_inv); matrix_delete(&mW); matrix_delete(&mL); matrix_delete(&mV); matrix_delete(&mV_inv); matrix_delete(&mA_bar); matrix_delete(&mQ_bar); matrix_delete(&mRinvS); matrix_delete(&mA_tilde); matrix_delete(&mW_tilde); matrix_delete(&mLVinv); matrix_delete(&mP1tar); matrix_delete(&mP2tar); matrix_delete(&mP1); matrix_delete(&mP2); matrix_delete(&mP1_inv); matrix_delete(&mP2_inv); matrix_delete(&mP1_tilde); matrix_delete(&mP2_tilde); matrix_delete(&mBD); matrix_delete(&mGama_inv_C); matrix_delete(&mGDPD); matrix_delete(&mCH); matrix_delete(&mGama_inv_F); matrix_delete(&mGHPH); matrix_delete(&mKcx); matrix_delete(&mKcy); matrix_delete(&mKfx); matrix_delete(&mKfu); matrix_delete(&mKfy); matrix_delete(&mT1); matrix_delete(&mT2); matrix_delete(&mT3); matrix_delete(&mT4); matrix_delete(&mru); matrix_delete(&mry); matrix_delete(&mrx); matrix_delete(&mrT1); matrix_delete(&mrT2); } //------------------------------------------------------------------------------