/* * 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" #include "global_var.h" /* % 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_0x(c) printlog(c) #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 H8MPC_alg::H8MPC_alg(float notused) : syscr() { int i; // 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, H8_m, H8_m); mA.mtx = mA_v; mA.row = H8_m; mA.col = H8_m; mA.dim = H8_m*H8_m; mA_v[0] = SATRT_OF_VECTOR; mA_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mA_v[i]=(float)0.0; //matrix_alloc(&mB, H8_m, H8_r); mB.mtx = mB_v; mB.row = H8_m; mB.col = H8_r; mB.dim = H8_m*H8_r; mB_v[0] = SATRT_OF_VECTOR; mB_v[(H8_m*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_r);i++) mB_v[i]=(float)0.0; //matrix_alloc(&mD, H8_m, H8_l); mD.mtx = mD_v; mD.row = H8_m; mD.col = H8_l; mD.dim = H8_m*H8_l; mD_v[0] = SATRT_OF_VECTOR; mD_v[(H8_m*H8_l)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_l);i++) mD_v[i]=(float)0.0; //matrix_alloc(&mC, H8_q, H8_m); mC.mtx = mC_v; mC.row = H8_q; mC.col = H8_m; mC.dim = H8_q*H8_m; mC_v[0] = SATRT_OF_VECTOR; mC_v[(H8_q*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_q*H8_m);i++) mC_v[i]=(float)0.0; //matrix_alloc(&mE, H8_q, H8_l); mE.mtx = mE_v; mE.row = H8_q; mE.col = H8_l; mE.dim = H8_q*H8_l; mE_v[0] = SATRT_OF_VECTOR; mE_v[(H8_q*H8_l)+1] = END_OF_VECTOR; for(i=1;i<=(H8_q*H8_l);i++) mE_v[i]=(float)0.0; //matrix_alloc(&mH, H8_p, H8_m); mH.mtx = mH_v; mH.row = H8_p; mH.col = H8_m; mH.dim = H8_p*H8_m; mH_v[0] = SATRT_OF_VECTOR; mH_v[(H8_p*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_p*H8_m);i++) mH_v[i]=(float)0.0; //matrix_alloc(&mG, H8_p, H8_r); mG.mtx = mG_v; mG.row = H8_p; mG.col = H8_r; mG.dim = H8_p*H8_r; mG_v[0] = SATRT_OF_VECTOR; mG_v[(H8_p*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_p*H8_r);i++) mG_v[i]=(float)0.0; //matrix_alloc(&mQ, H8_m, H8_m); // Q = H'*H mQ.mtx = mQ_v; mQ.row = H8_m; mQ.col = H8_m; mQ.dim = H8_m*H8_m; mQ_v[0] = SATRT_OF_VECTOR; mQ_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mQ_v[i]=(float)0.0; //matrix_alloc(&mS, H8_m, H8_r); // S = H'*G mS.mtx = mS_v; mS.row = H8_m; mS.col = H8_r; mS.dim = H8_m*H8_r; mS_v[0] = SATRT_OF_VECTOR; mS_v[(H8_m*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_r);i++) mS_v[i]=(float)0.0; //matrix_alloc(&mR, H8_r, H8_r); // R = G'*G mR.mtx = mR_v; mR.row = H8_r; mR.col = H8_r; mR.dim = H8_r*H8_r; mR_v[0] = SATRT_OF_VECTOR; mR_v[(H8_r*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_r);i++) mR_v[i]=(float)0.0; //matrix_alloc(&mR_inv, H8_r, H8_r); // R_inv = inv(R) mR_inv.mtx = mR_inv_v; mR_inv.row = H8_r; mR_inv.col = H8_r; mR_inv.dim = H8_r*H8_r; mR_inv_v[0] = SATRT_OF_VECTOR; mR_inv_v[(H8_r*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_r);i++) mR_inv_v[i]=(float)0.0; //matrix_alloc(&mW, H8_m, H8_m); // W = D*D' mW.mtx = mW_v; mW.row = H8_m; mW.col = H8_m; mW.dim = H8_m*H8_m; mW_v[0] = SATRT_OF_VECTOR; mW_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mW_v[i]=(float)0.0; //matrix_alloc(&mL, H8_m, H8_q); // L = D*E' mL.mtx = mL_v; mL.row = H8_m; mL.col = H8_q; mL.dim = H8_m*H8_q; mL_v[0] = SATRT_OF_VECTOR; mL_v[(H8_m*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_q);i++) mL_v[i]=(float)0.0; //matrix_alloc(&mV, H8_q, H8_q); // V = E*E' mV.mtx = mV_v; mV.row = H8_q; mV.col = H8_q; mV.dim = H8_q*H8_q; mV_v[0] = SATRT_OF_VECTOR; mV_v[(H8_q*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_q*H8_q);i++) mV_v[i]=(float)0.0; //matrix_alloc(&mV_inv, H8_q, H8_q); // V_inv = inv(V) mV_inv.mtx = mV_inv_v; mV_inv.row = H8_q; mV_inv.col = H8_q; mV_inv.dim = H8_q*H8_q; mV_inv_v[0] = SATRT_OF_VECTOR; mV_inv_v[(H8_q*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_q*H8_q);i++) mV_inv_v[i]=(float)0.0; //matrix_alloc(&mA_bar, H8_m, H8_m); // A_bar = A - B*R_inv*S' mA_bar.mtx = mA_bar_v; mA_bar.row = H8_m; mA_bar.col = H8_m; mA_bar.dim = H8_m*H8_m; mA_bar_v[0] = SATRT_OF_VECTOR; mA_bar_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mA_bar_v[i]=(float)0.0; //matrix_alloc(&mQ_bar, H8_m, H8_m); // Q_bar = Q - S*R_inv*S' mQ_bar.mtx = mQ_bar_v; mQ_bar.row = H8_m; mQ_bar.col = H8_m; mQ_bar.dim = H8_m*H8_m; mQ_bar_v[0] = SATRT_OF_VECTOR; mQ_bar_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mQ_bar_v[i]=(float)0.0; //matrix_alloc(&mRinvS, H8_r, H8_m); // mRinvS = R_inv*S' mRinvS.mtx = mRinvS_v; mRinvS.row = H8_r; mRinvS.col = H8_m; mRinvS.dim = H8_r*H8_m; mRinvS_v[0] = SATRT_OF_VECTOR; mRinvS_v[(H8_r*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_m);i++) mRinvS_v[i]=(float)0.0; //matrix_alloc(&mA_tilde, H8_m, H8_m); // A_tilde = A - L*V_inv*C; mA_tilde.mtx = mA_tilde_v; mA_tilde.row = H8_m; mA_tilde.col = H8_m; mA_tilde.dim = H8_m*H8_m; mA_tilde_v[0] = SATRT_OF_VECTOR; mA_tilde_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mA_tilde_v[i]=(float)0.0; //matrix_alloc(&mW_tilde, H8_m, H8_m); // W_tilde = W - L*V_inv*L' mW_tilde.mtx = mW_tilde_v; mW_tilde.row = H8_m; mW_tilde.col = H8_m; mW_tilde.dim = H8_m*H8_m; mW_tilde_v[0] = SATRT_OF_VECTOR; mW_tilde_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mW_tilde_v[i]=(float)0.0; //matrix_alloc(&mLVinv, H8_m, H8_q); // mLVinv = L*V_inv mLVinv.mtx = mLVinv_v; mLVinv.row = H8_m; mLVinv.col = H8_m; mLVinv.dim = H8_m*H8_m; mLVinv_v[0] = SATRT_OF_VECTOR; mLVinv_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mLVinv_v[i]=(float)0.0; //matrix_alloc(&mP1tar, H8_m, H8_m); // P1tar [mxm] Riccati result mP1tar.mtx = mP1tar_v; mP1tar.row = H8_m; mP1tar.col = H8_m; mP1tar.dim = H8_m*H8_m; mP1tar_v[0] = SATRT_OF_VECTOR; mP1tar_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP1tar_v[i]=(float)0.0; //matrix_alloc(&mP2tar, H8_m, H8_m); // P2tar [mxm] Riccati result mP2tar.mtx = mP2tar_v; mP2tar.row = H8_m; mP2tar.col = H8_m; mP2tar.dim = H8_m*H8_m; mP2tar_v[0] = SATRT_OF_VECTOR; mP2tar_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP2tar_v[i]=(float)0.0; //matrix_alloc(&mP1, H8_m, H8_m); // P1 [mxm] Riccati calculation mP1.mtx = mP1_v; mP1.row = H8_m; mP1.col = H8_m; mP1.dim = H8_m*H8_m; mP1_v[0] = SATRT_OF_VECTOR; mP1_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP1_v[i]=(float)0.0; //matrix_alloc(&mP2, H8_m, H8_m); // P2 [mxm] Riccati calculation mP2.mtx = mP2_v; mP2.row = H8_m; mP2.col = H8_m; mP2.dim = H8_m*H8_m; mP2_v[0] = SATRT_OF_VECTOR; mP2_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP2_v[i]=(float)0.0; //matrix_alloc(&mP1_inv, H8_m, H8_m); // P1 [mxm] Riccati calculation mP1_inv.mtx = mP1_inv_v; mP1_inv.row = H8_m; mP1_inv.col = H8_m; mP1_inv.dim = H8_m*H8_m; mP1_inv_v[0] = SATRT_OF_VECTOR; mP1_inv_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP1_inv_v[i]=(float)0.0; //matrix_alloc(&mP2_inv, H8_m, H8_m); // P2 [mxm] Riccati calculation mP2_inv.mtx = mP2_inv_v; mP2_inv.row = H8_m; mP2_inv.col = H8_m; mP2_inv.dim = H8_m*H8_m; mP2_inv_v[0] = SATRT_OF_VECTOR; mP2_inv_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP2_inv_v[i]=(float)0.0; //matrix_alloc(&mP1_tilde, H8_m, H8_m);// P1 [mxm] Riccati1 test mP1_tilde.mtx = mP1_tilde_v; mP1_tilde.row = H8_m; mP1_tilde.col = H8_m; mP1_tilde.dim = H8_m*H8_m; mP1_tilde_v[0] = SATRT_OF_VECTOR; mP1_tilde_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP1_tilde_v[i]=(float)0.0; //matrix_alloc(&mP2_tilde, H8_m, H8_m);// P2 [mxm] Riccati2 test mP2_tilde.mtx = mP2_tilde_v; mP2_tilde.row = H8_m; mP2_tilde.col = H8_m; mP2_tilde.dim = H8_m*H8_m; mP2_tilde_v[0] = SATRT_OF_VECTOR; mP2_tilde_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mP2_tilde_v[i]=(float)0.0; //matrix_alloc(&mBD,H8_m,H8_r+H8_l); // mBD.mtx = mBD_v; mBD.row = H8_m; mBD.col = H8_r+H8_l; mBD.dim = H8_m*(H8_r+H8_l); mBD_v[0] = SATRT_OF_VECTOR; mBD_v[(H8_m*(H8_r+H8_l))+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*(H8_r+H8_l));i++) mBD_v[i]=(float)0.0; //matrix_alloc(&mGama_inv_C,H8_r+H8_l,H8_r+H8_l); // inv(Gama__C) mGama_inv_C.mtx = mGama_inv_C_v; mGama_inv_C.row = H8_r+H8_l; mGama_inv_C.col = H8_r+H8_l; mGama_inv_C.dim = (H8_r+H8_l)*(H8_r+H8_l); mGama_inv_C_v[0] = SATRT_OF_VECTOR; mGama_inv_C_v[((H8_r+H8_l)*(H8_r+H8_l))+1] = END_OF_VECTOR; for(i=1;i<=((H8_r+H8_l)*(H8_r+H8_l));i++) mGama_inv_C_v[i]=(float)0.0; //matrix_alloc(&mGDPD,H8_l,H8_l); // -gama2 * eye(l) + D'*P1*D (test) mGDPD.mtx = mGDPD_v; mGDPD.row = H8_l; mGDPD.col = H8_l; mGDPD.dim = H8_l*H8_l; mGDPD_v[0] = SATRT_OF_VECTOR; mGDPD_v[(H8_l*H8_l)+1] = END_OF_VECTOR; for(i=1;i<=(H8_l*H8_l);i++) mGDPD_v[i]=(float)0.0; //matrix_alloc(&mCH,H8_q+H8_p,H8_m); // mCH.mtx = mCH_v; mCH.row = H8_q+H8_p; mCH.col = H8_m; mCH.dim = (H8_q+H8_p)*H8_m; mCH_v[0] = SATRT_OF_VECTOR; mCH_v[((H8_q+H8_p)*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=((H8_q+H8_p)*H8_m);i++) mCH_v[i]=(float)0.0; //matrix_alloc(&mGama_inv_F,H8_q+H8_p,H8_q+H8_p); // inv(Gama__F) mGama_inv_F.mtx = mGama_inv_F_v; mGama_inv_F.row = H8_q+H8_p; mGama_inv_F.col = H8_q+H8_p; mGama_inv_F.dim = (H8_q+H8_p)*(H8_q+H8_p); mGama_inv_F_v[0] = SATRT_OF_VECTOR; mGama_inv_F_v[((H8_q+H8_p)*(H8_q+H8_p))+1] = END_OF_VECTOR; for(i=1;i<=((H8_q+H8_p)*(H8_q+H8_p));i++) mGama_inv_F_v[i]=(float)0.0; //matrix_alloc(&mGHPH,H8_p,H8_p); // -gama2 * eye(p) + H*P2*H' (test) mGHPH.mtx = mGHPH_v; mGHPH.row = H8_p; mGHPH.col = H8_p; mGHPH.dim = H8_p*H8_p; mGHPH_v[0] = SATRT_OF_VECTOR; mGHPH_v[(H8_p*H8_p)+1] = END_OF_VECTOR; for(i=1;i<=(H8_p*H8_p);i++) mGHPH_v[i]=(float)0.0; //matrix_alloc(&mKcx, H8_r,H8_m); // u__n = Kcx * x__n + Kcy * y__n mKcx.mtx = mKcx_v; mKcx.row = H8_r; mKcx.col = H8_m; mKcx.dim = H8_r*H8_m; mKcx_v[0] = SATRT_OF_VECTOR; mKcx_v[(H8_r*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_m);i++) mKcx_v[i]=(float)0.0; // Real Time Control mrtKcx.mtx = mrtKcx_v; mrtKcx.row = H8_r; mrtKcx.col = H8_m; mrtKcx.dim = H8_r*H8_m; mrtKcx_v[0] = SATRT_OF_VECTOR; mrtKcx_v[(H8_r*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_m);i++) mrtKcx_v[i]=(float)0.0; //matrix_alloc(&mKcy, H8_r,H8_q); // mKcy.mtx = mKcy_v; mKcy.row = H8_r; mKcy.col = H8_q; mKcy.dim = H8_r*H8_q; mKcy_v[0] = SATRT_OF_VECTOR; mKcy_v[(H8_r*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_q);i++) mKcy_v[i]=(float)0.0; // Real Time Control mrtKcy.mtx = mrtKcy_v; mrtKcy.row = H8_r; mrtKcy.col = H8_q; mrtKcy.dim = H8_r*H8_q; mrtKcy_v[0] = SATRT_OF_VECTOR; mrtKcy_v[(H8_r*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*H8_q);i++) mrtKcy_v[i]=(float)0.0; //matrix_alloc(&mKfx, H8_m,H8_m); // x__np = Kfx*x__n + Kfu*u__n + Kfy*y__n mKfx.mtx = mKfx_v; mKfx.row = H8_m; mKfx.col = H8_m; mKfx.dim = H8_m*H8_m; mKfx_v[0] = SATRT_OF_VECTOR; mKfx_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mKfx_v[i]=(float)0.0; // Real Time Control mrtKfx.mtx = mrtKfx_v; mrtKfx.row = H8_m; mrtKfx.col = H8_m; mrtKfx.dim = H8_m*H8_m; mrtKfx_v[0] = SATRT_OF_VECTOR; mrtKfx_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mrtKfx_v[i]=(float)0.0; //matrix_alloc(&mKfu, H8_m,H8_r); // mKfu.mtx = mKfu_v; mKfu.row = H8_m; mKfu.col = H8_r; mKfu.dim = H8_m*H8_r; mKfu_v[0] = SATRT_OF_VECTOR; mKfu_v[(H8_m*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_r);i++) mKfu_v[i]=(float)0.0; // Real Time Control mrtKfu.mtx = mrtKfu_v; mrtKfu.row = H8_m; mrtKfu.col = H8_r; mrtKfu.dim = H8_m*H8_r; mrtKfu_v[0] = SATRT_OF_VECTOR; mrtKfu_v[(H8_m*H8_r)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_r);i++) mrtKfu_v[i]=(float)0.0; //matrix_alloc(&mKfy, H8_m,H8_q); // mKfy.mtx = mKfy_v; mKfy.row = H8_m; mKfy.col = H8_q; mKfy.dim = H8_m*H8_q; mKfy_v[0] = SATRT_OF_VECTOR; mKfy_v[(H8_m*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_q);i++) mKfy_v[i]=(float)0.0; // Real Time Control mrtKfy.mtx = mrtKfy_v; mrtKfy.row = H8_m; mrtKfy.col = H8_q; mrtKfy.dim = H8_m*H8_q; mrtKfy_v[0] = SATRT_OF_VECTOR; mrtKfy_v[(H8_m*H8_q)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_q);i++) mrtKfy_v[i]=(float)0.0; // temporary matrix //matrix_alloc(&mT1, H8_p+H8_q, H8_p+H8_q); mT1.mtx = mT1_v; mT1.row = H8_p+H8_q; mT1.col = H8_p+H8_q; mT1.dim = (H8_p+H8_q)*(H8_p+H8_q); mT1_v[0] = SATRT_OF_VECTOR; mT1_v[((H8_p+H8_q)*(H8_p+H8_q))+1] = END_OF_VECTOR; for(i=1;i<=((H8_p+H8_q)*(H8_p+H8_q));i++) mT1_v[i]=(float)0.0; //matrix_alloc(&mT2, H8_p+H8_q, H8_p+H8_q); // temporary matrix mT2.mtx = mT2_v; mT2.row = H8_p+H8_q; mT2.col = H8_p+H8_q; mT2.dim = (H8_p+H8_q)*(H8_p+H8_q); mT2_v[0] = SATRT_OF_VECTOR; mT2_v[((H8_p+H8_q)*(H8_p+H8_q))+1] = END_OF_VECTOR; for(i=1;i<=((H8_p+H8_q)*(H8_p+H8_q));i++) mT2_v[i]=(float)0.0; //matrix_alloc(&mT3, H8_p+H8_q, H8_p+H8_q); // temporary matrix mT3.mtx = mT3_v; mT3.row = H8_p+H8_q; mT3.col = H8_p+H8_q; mT3.dim = (H8_p+H8_q)*(H8_p+H8_q); mT3_v[0] = SATRT_OF_VECTOR; mT3_v[((H8_p+H8_q)*(H8_p+H8_q))+1] = END_OF_VECTOR; for(i=1;i<=((H8_p+H8_q)*(H8_p+H8_q));i++) mT3_v[i]=(float)0.0; //matrix_alloc(&mT4, H8_p+H8_q, H8_p+H8_q); // temporary matrix mT4.mtx = mT4_v; mT4.row = H8_p+H8_q; mT4.col = H8_p+H8_q; mT4.dim = (H8_p+H8_q)*(H8_p+H8_q); mT4_v[0] = SATRT_OF_VECTOR; mT4_v[((H8_p+H8_q)*(H8_p+H8_q))+1] = END_OF_VECTOR; for(i=1;i<=((H8_p+H8_q)*(H8_p+H8_q));i++) mT4_v[i]=(float)0.0; //matrix_alloc(&mru, H8_r,1); // H8tem output vector mru.mtx = mru_v; mru.row = H8_r; mru.col = 1; mru.dim = H8_r*1; mru_v[0] = SATRT_OF_VECTOR; mru_v[H8_r+1] = END_OF_VECTOR; for(i=1;i<=(H8_r*1);i++) mru_v[i]=(float)0.0; //matrix_alloc(&mry, H8_q,1); // system input vector mry.mtx = mry_v; mry.row = H8_q; mry.col = 1; mry.dim = H8_q*1; mry_v[0] = SATRT_OF_VECTOR; mry_v[H8_q+1] = END_OF_VECTOR; for(i=1;i<=(H8_q*1);i++) mry_v[i]=(float)0.0; //matrix_alloc(&mrx, H8_m,1); // system state mrx.mtx = mrx_v; mrx.row = H8_m; mrx.col = 1; mrx.dim = H8_m*1; mrx_v[0] = SATRT_OF_VECTOR; mrx_v[H8_m+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*1);i++) mrx_v[i]=(float)0.0; //matrix_alloc(&mrT1, H8_m,H8_m); // real-time temporary matrix mrT1.mtx = mrT1_v; mrT1.row = H8_m; mrT1.col = H8_m; mrT1.dim = H8_m*H8_m; mrT1_v[0] = SATRT_OF_VECTOR; mrT1_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mrT1_v[i]=(float)0.0; //matrix_alloc(&mrT2, H8_m,H8_m); // real-time temporary matrix mrT2.mtx = mrT2_v; mrT2.row = H8_m; mrT2.col = H8_m; mrT2.dim = H8_m*H8_m; mrT2_v[0] = SATRT_OF_VECTOR; mrT2_v[(H8_m*H8_m)+1] = END_OF_VECTOR; for(i=1;i<=(H8_m*H8_m);i++) mrT2_v[i]=(float)0.0; gama_0 = 0.0 ; // gama recursion initialization newsystem = 0; } //------------------------------------------------------------------------------ // Implementation: //------------------------------------------------------------------------------ /* %=============================================================================== */ void H8MPC_alg::update(Matrix *psy) { #if H8_CONTROL int i; float gama; float gama_F; float gama_P; float ftemp1; int gama_iteration; int gama_found; int error_flag; int break_flag; float gama2; // = gama^2 float gama2_inv; // = 1/(gama^2) DC_LED_on(DC_LED_R); // To indicate the calculation process // [ARMAX_DC,a1,a2,a3,a4,a5,b1,b2,b3,b4,b5,c1,..] // // b1*u(n-1)+b2*u(n-2+b3*u(n-3)+b4*u(n-4)+b5*u(n-5) // y(n) = --------------------------------------------------- // a1*y(n-1)+a2*y(n-2+a3*y(n-3)+a4*y(n-4)+a5*y(n-5) //% system definition and update: //%------------------------- // mA = [-a1 1 0 0 <> // -a2 0 1 0 // -a3 0 0 1 // -a4 0 0 0] for(i=1; imtx[ARMAX_DC+i]; PRINT_00(&mA,"mA"); // mB = [b1; b2; b3; b4] <> for(i=1; i<=H8_m;i++) mB.mtx[i] = psy->mtx[ARMAX_DC+ARMAX_a+i]; PRINT_00(&mB,"mB"); // mC = [1 0 0 0] <> for(i=1; i<=H8_m;i++) mC.mtx[1+(mC.row*(i-1))] = 0.0; mC.mtx[1] = 1.0; PRINT_00(&mC,"mC"); // mBD <- [B D] concat_row(&mBD,&mB,&mD); // BD <- [B D] PRINT_01(&mBD,"mBD"); // mCH <- [C; H] concat_col(&mCH, &mC, &mH); // CH <- [C; H] PRINT_01(&mCH,"mCH"); // Calculate the matrices: //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 while(gama_iteration) // The loop for the optimal gama search { error_flag = FAIL; // set the eroor_flaf to > 0 //while(error_flag) //This is used to jump out if the ptest() fail do{ break_flag = 0; // retrive the values from the privious optimization period: copymat(&mP1, &mP1tar); //P1 <- P1tar copymat(&mP2, &mP2tar); //P2 <- P2tar gama2 = gama*gama; gama2_inv = 1/gama2; for(i=15; 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,H8_l,(-1*gama2)); // <- -gama2 * eye(l) addmat(&mGDPD, &mT4, &mT2); // mGDPD <- [ ] PRINT_01(&mGDPD,"mGDPD "); PRINT_LOG_02("pre -GDPD positivity test folows"); cmulmat(&mT4,-1.0,&mGDPD); // gama2_inv*W if(ptest(&mT4)==0) { break_flag = 1; break; } PRINT_LOG_02(" positivity tests 1 PASS "); // [ 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"); PRINT_LOG_02("pre P1 positivity test"); copymat(&mT1, &mP1); if(ptest(&mT1)==0) { 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 ; minv(&mP1_inv,&mP1); //P1_inv = inv(P1); PRINT_00(&mP1_inv,"mP1_inv"); for(i=15; 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,H8_p,(-1*gama2)); // <- -gama2 * eye(p) addmat(&mGHPH, &mT4, &mT2); // mGHPH <- [ ] PRINT_00(&mGHPH,"-gama2 * eye(p) + H*P2*H' "); cmulmat(&mT4,-1.0,&mGHPH); // <- -GHPH if(ptest(&mT4)==0) { break_flag = 1; break; } // [ 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 "); copymat(&mT1, &mP2); if(ptest(&mT1)==0) { break_flag = 1; break; } } //if(break_flag) PRINT_LOG_03(" >>>< P2 test FAIL ><<<< "); if(break_flag)break ; minv(&mP2_inv,&mP2);// P2_inv = inv(P2); PRINT_00(&mP2_inv,"mP2_inv"); // 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 copymat(&mT1, &mP1_tilde); if(ptest(&mT1)==0) break; mulmat(&mT2,&mP2,&mP1_tilde);// mT2 <- P2*P1_tilde diagn(&mT1,H8_m,gama2); // mT1 <- gama2 * eye(m) submat(&mT3,&mT1,&mT2); // <- gama2*eye(m) - P2*P1_tilde if(ptest(&mT3)==0) break; copymat(&mT1, &mP2_tilde); if(ptest(&mT1)==0) break; mulmat(&mT2,&mP2_tilde,&mP1);// mT2 <- P2_tilde*P1 diagn(&mT1,H8_m,gama2); // mT1 <- gama2 * eye(m) submat(&mT3,&mT1,&mT2); // <- gama2*eye(m)-P2_tilde*P1 if(ptest(&mT3)==0) break; PRINT_LOG_00(" All positivity tests PASS "); error_flag = OK; // clear the eroor_flag to 0 <=> while loop end } while(0) ; //================================================= 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 start ower. } } } else gama_iteration = 0 ; // end the iteration!! if( gama > MAXgama) { // PRINT_LOG_00("--> gama > MAXgama "); DC_LED_on(DC_LED_R); // gamma limit indikator DC_LED_off(DC_LED_Y); // gamma limit indikator //PRINT_LOG_00("--> gama > MAXgama "); return; } else { DC_LED_off(DC_LED_R); // gamma limit indikator DC_LED_on(DC_LED_Y); // gamma limit indikator } }//while(gama_iteration) //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ PRINT_LOG_01("The controller 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,H8_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 <-- PRINT_LOG_0x(" newsystem "); newsystem = 1; // Flag for the new contrlo system #endif } // end H8MPC_update //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ float H8MPC_alg::rtm_control(float fy__i, float fu__i) { int i; float ftemp; // Kip the memory currnt: for (i=RETUNE-1; i>0; i--) { y_i_vector[i] = y_i_vector[i-1]; u_i_vector[i] = u_i_vector[i-1]; } y_i_vector[0] = fy__i; u_i_vector[0] = fu__i; if(newsystem) // retune the control algorithm { copymat(&mrtKcx, &mKcx); // Control copymat(&mrtKfx, &mKfx); // Filter copymat(&mrtKfu, &mKfu); // Filter copymat(&mrtKfy, &mKfy); // Filter newsystem = 0; //PRINT_0x(&mrtKcx,"mrtKcx"); //PRINT_0x(&mrtKfx,"mrtKfx"); //PRINT_0x(&mrtKfu,"mrtKfu"); //PRINT_0x(&mrtKfy,"mrtKfy"); for (i= (RETUNE-1); i>0; i--) ftemp = rtm_update(y_i_vector[i],u_i_vector[i]) ; } ftemp = rtm_update( fy__i, fu__i) ; return ftemp; } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ float H8MPC_alg::rtm_update(float fy__i, float fu__i) { float control; mry.mtx[1] = fy__i; mru.mtx[1] = fu__i; //--------------------------------------------------------------- // x__np = Kfx * x__n + Kfu * u__n + Kfy * y__n mulmat(&mrT1,&mrtKfx,&mrx); // mrT1 <- Kfx * x__n <-[mx1] mulmat(&mrT2,&mrtKfu,&mru); // mrT2 <- Kfu * u__n <-[mx1] addmat(&mrx,&mrT1,&mrT2); // <- Kfx * x__n + Kfu * u__n mulmat(&mrT1,&mrtKfy,&mry); // mrT1 <- Kfy * y__n <-[mx1] addmat(&mrx,&mrx,&mrT1); // <-- x__np <-[mx1] //PRINT_0x(&mrx,"mrx"); //--------------------------------------------------------------- mulmat(&mru,&mrtKcx,&mrx); // <-- u__n control = mru.mtx[1]; // C * x_n where C=[1 0 ... 0] // PRINT_0x(&mru,"mru"); //--------------------------------------------------------------- // Statistic calculation for the control: y_sys_n = fy__i; u_sys_n = fu__i; if(update_stat() == 1 ) // The results are redy: { count_n = NxT; // Start the next statistical evaluation controlStatRedy = 1; LOG_printf(&trace2,"t= %g",t_n); LOG_printf(&trace3,"PI_y = %g", get_output_error()); LOG_printf(&trace2,"PI_u = %g", get_control_effort()); } else controlStatRedy = 0; control = LIMIT(-1.0, control, 1.0); return control ; } //------------------------------------------------------------------------------ //----------------------------------------------------------------------------- void H8MPC_alg::weightSet(void) { int i,j; //% 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 #define wG 1.0 /* 1.0 */ #define wD 0.1 /* 0.1 */ #define wE 0.005 /* 0.005*/ #define wH 0.2 /* 0.4 */ for(i=1; i<=H8_m;i++) mD.mtx[i+(mD.row*(i-1))] = wD ;//* mD.mtx[i][j]; // 0.00125 PRINT_03(&mD,"mD"); // mE = [ e e e] <> for(i=1; i<=H8_q;i++) { for(j=1; j<=H8_l;j++) mE.mtx[i+(mE.row*(j-1))] = wE ;}//*e_value[j] ; } // 0.025 PRINT_03(&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<=H8_m;i++) mH.mtx[i+(mH.row*(i-1))] = wH; //*h_value[i]; //0.08 PRINT_03(&mH,"mH"); // mG = [0; 0; 0; 0; g1] <

> for(i=1; i<=H8_p;i++) { for(j=1; j<=H8_r;j++) mG.mtx[i+(mG.row*(j-1))] = 0.0 ;} mG.mtx[H8_p*H8_r] = wG ; // 1 PRINT_03(&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"); // <-- } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- float H8MPC_alg::get_gama(void) // the control effort { return gama_0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- H8MPC_alg::~H8MPC_alg() // deInitialization { } //------------------------------------------------------------------------------