/* * Copyright 2002 by Soos, Antal * All rights reserved. Property of Soos, Antal. * Restricted rights to use, duplicate or disclose this code are * granted through contract. */ /***************************************************************************/ /* */ /* WCE . cpp */ /* */ /* Recoursive Worst Case Estimation Algorithm inplementation. */ /* */ /* */ /***************************************************************************/ #define PROB 0 #include "WCE.hpp" //----------------------------------------------------------------------------- //Initialization: //----------------------------------------------------------------------------- /* function [theta,P,delta] = WCE_init(deltai,model) % Worst CASE Estimation (WCE) algorithm initiialization %---------------------------------------------------------- N=model(1,1)+model(1,2)+model(1,3)+model(1,4); P = 10^12*eye(N,N); % Identity matrix for i=1:N theta(i,1) = 0; end %Maximum error => delta max(v) delta = deltai; */ WCE_alg::WCE_alg(float delta_WCE,int is, int ia, int ib, int ic) : sysid(is,ia,ib,ic) { eye_matrix_alloc(&P_WCE, m_s+m_a+m_b+m_c); // P_WCE: cmulmat(&P_WCE, (float) 200, &P_WCE); //loacal matrices: matrix_alloc(&MtempA, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix matrix_alloc(&MtempB, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix // parameters for identification: delta = delta_WCE; } //----------------------------------------------------------------------------- // Implementation: //----------------------------------------------------------------------------- /* function [theta,v,P,delta] = WCE(psy,y_n,theta,P,delta,model) % Recoursive Worst CASE Estimation (WCE) algorithm %---------------------------------------------------------- % Inputs: % psy : [-y_n-1,...-y_n-ma,u_n-1,...u_n-mb,v_n-1,...v_n-mc] % vhere the y_xx index _xx is the time: t=T*xx % y_n : system output at time t=T*n % theta : etimated system paramethers at t=T*(n-1) (theta_n-1) % P: % delta: maximum error max(v) % m : model order [ma,mb,mc] % % Outputs: % theta : etimated system paramethers % v: residual in system identification % % For recursion: % P : P_n-1 (estimated error covariance) % theta : theta_n-1 (estimated paramethers) % % Initialization: % P = eye(m(1,1)+m(1,2)+m(1,3)) * 10^22; % % theta = zero matrix % g=psy'*P*psy; v=y_n-psy'*theta; v2=v^2; %%%%% Insert delta = delta*0.99; delta = max(delta,abs(v)*1.9); %%%%% End of insert d=model(1,1)+model(1,2)+model(1,3)+model(1,4); % Solution for ro -> maximum pozitive root of the equations % (d-1)*g^2*ro^2-((2*d-1)*delta-g-v^2)*g*ro+delta*(d*(delta-v^2)-g)=0 % ro_1/2 = (-b +/- sqrt(b^2-4*a*c))/2a % where a= (d-1)*g^2 % b= ((2*d-1)*delta-g-v^2)*g % c= delta*(d*(delta-v^2)-g) a= (d-1)*g^2; b= ((2*d-1)*delta-g-v2)*g; c= delta*(d*(delta-v2)-g); % Numerical recepies in C, pp. 184: b2_4ac= b^2-(4*a*c); if b2_4ac >= 0 q=(-1/2)*(b - sign(b) * sqrt(b2_4ac)); ro_1=q/a; ro_2=c/q; ro=max([ro_1 ro_2]); if ro < 0 ro=0; end else ro=0; end % The gain term beta: beta = 1 + ro - ((ro*v2)/(delta^2 + ro * psy' * P * psy)); % The size and orientation of the elipsoid is P = beta * (P - ((ro*P*psy*psy'*P)/((delta^2)+(ro*psy'*P*psy)))); % The center of the new elipsoid region become: theta = theta + ((ro*P*psy*v)/((delta^2)+(ro*psy'*P*psy))); */ void WCE_alg::update(float y_n, float u_n_1) { // y_n the systems output at time n*T (present time) float v_abs; float v2; // v2 = v_n * v_n; float g; // g = psy' * P * psy; float ge; // =delta*delta + ro*(psy' * P * psy) float a; // a= (d-1)*g^2; float b; // b= ((2*d-1)*delta-g-v2)*g; float b_sgn; // =sgn(b) float c; // c= delta*(d*(delta-v2)-g); float b2_4ac; // b2_4ac= b^2-(4*a*c); float q; float ro; // ro_1=q/a; float beta; // beta = 1 + ro - ((ro*v2)/(delta^2 + ro * psy' * P * psy)); #if PROB extern FILE *fp_pr1; // <<<<<<<<---------------------------- Test extern double time_s; // <<<<<<<<---------------------------- Test #endif update_psy(u_n_1); // control signal from the end of privious calculation //----------------------------------------------------------------------------- // Residual in system identification (predicion error): mulmat_t1(&MtempA, &psy, &theta); // v = y_n - psy' * theta; v_n = y_n - MtempA.mtx[1][1]; // Residual in system identification v2 = v_n * v_n ; // residual power //printf("\n v_n=%g y_n=%g ",v_n, y_n); //print_matrix(&psy,"psy for WCE[0]"); //print_matrix(&theta,"theta for WCE[0]"); //print_matrix(&MtempA,"theta*psy for WCE[0]"); #if PROB /* 1 */ fprintf(fp_pr1,"\n %f ",time_s); // <<<<<<<<------------------------ Test #endif //----------------------------------------------------------------------------- mulmat_t1(&MtempA, &psy, &P_WCE); // g= psy'* P * psy ; mulmat(&MtempB, &MtempA, &psy); g = MtempB.mtx[1][1]; ///print_matrix(&MtempB,"g"); #if PROB /* 2 */ fprintf(fp_pr1," %f ",g); // <<<<<<<<------------------------ Test #endif //----------------------------------------------------------------------------- //%%%%% Insert // v_abs = fabs(v_n); // delta = delta*0.98 ; // if(delta < 1000*v_abs) delta = 1000*v_abs; //%%%%% End of insert sgn #if PROB /* 3 */ fprintf(fp_pr1," %f ", delta); // <<<<<<<<------------------------ Test #endif //----------------------------------------------------------------------------- //% Solution for ro -> maximum pozitive root of the equations a = (m_dim-1)*(g*g); // a= (d-1)*g^2; b = ((2*m_dim-1)*delta-g-v2)*g; // b= ((2*d-1)*delta-g-v2)*g; c = delta*(m_dim*(delta-v2)-g); // c= delta*(d*(delta-v2)-g); #if PROB /* 4,5,6 */ fprintf(fp_pr1," %f %f %f ", a,b,c); // <<<<<<<<----------------- Test #endif b_sgn = (float)1.0; if(b < 0.0) b_sgn = (float) -1.0; b2_4ac= (b*b) - (4*a*c); if (b2_4ac > (float)1e-32) //----------------------------------------------- { q = 0.5* (b + (b_sgn * sqrt(b2_4ac))); ro = q/a; b= c/q; if(ro < b) ro = b; if (ro > 1e-32) { //% The gain term beta: // ge= delta^2 + ro(psy'* P * psy); ge = (delta * delta)+(ro * g); //beta = 1 + ro - ((ro*v2)/(delta^2 + ro * psy' * P * psy)); beta = 1 + ro - (ro*v2 / ge); //% The center of the new elipsoid region become: //theta = theta + ((ro*P*psy*v)/((delta^2)+(ro*psy'*P*psy))); mulmat(&MtempA, &P_WCE, &psy); //MtempA = P*psy //= ro*P*psy*v/(delta^2 + ro*psy'*P*psy) cmulmat(&MtempA,((ro*v_n)/ge),&MtempA); addmat(&theta,&theta,&MtempA); //theta <- the new //% The size and orientation of the elipsoid is //P = beta * (P - ((ro*P*psy*psy'*P)/((delta^2)+(ro*psy'*P*psy)))); mulmat_t1(&MtempA, &psy, &P_WCE); //MtempA = psy'*P mulmat(&MtempB, &psy, &MtempA); //MtempB = psy*psy'*P mulmat(&MtempA, &P_WCE, &MtempB); //MtempA = P*psy*psy'*P cmulmat(&MtempA,ro/ge,&MtempA); //MtempA = ro*P*psy*psy'*P submat(&P_WCE,&P_WCE,&MtempA); //P_WCE = P - ((ro*P*psy*psy'*P cmulmat(&P_WCE,beta,&P_WCE); //P_WCE <- the new } } //--------------------------------------------------------------------- update_psy(y_n, v_n); // Updates the psy vector with time shift // Statistic calculation for the system identification: if(update_stat() == 1 ) // The results are redy: { #if 0 printf("\n WCE ---->"); printf("\n v: N(%g, %g), v_WCE= %g",v_moav,v_vari,v_qume); printf("\n%g < v < %g <> v_range = %g",v_mind,v_maxd,v_mami); printf("\ttheta_frob = %g",theta_frob); printf("\t space = %g \n",performance_index); #endif count_n = NxT; // Start the next statistical evaluation } //(u_n will be calculated based on the estimated theta vector in future) } //----------------------------------------------------------------------------- // DeInitialisation: //----------------------------------------------------------------------------- WCE_alg::~WCE_alg() // deInitialization { // Remouve the alocation from the heep matrix_delete(&P_WCE); matrix_delete(&MtempA); matrix_delete(&MtempB); } //-----------------------------------------------------------------------------