/* * 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. */ /***************************************************************************/ /* */ /* RLS . cpp */ /* */ /* Recoursive Least Square Algorithm inplementation. */ /* */ /* */ /***************************************************************************/ #include "RLS.hpp" //----------------------------------------------------------------------------- //Initialization: //----------------------------------------------------------------------------- /* function [theta,P_RLS,lambda] = RLS_init(model,lambda) % Recoursive Least Square (RLS) algorithm initialization(Conventional) %---------------------------------------------------------- % Inputs: % model : model = [ma,mb,mc] where-> % 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 % lambda : forgetting factor (0 << lambda < 1) % % Outputs: % theta : zero vector - etimated system paramethers % P: estimated error covariance diagonal matrix % lambda : forgetting factor (0 << lambda < 1) % % Initialisations of weight wector : theta = eye((model(1,1)+model(1,2)+model(1,3)+model(1,4)),1); theta = theta-theta ;% zero matrix % Estimated error covariance diagonal matrix initialization P_RLS=10^12 * eye(model(1,1)+model(1,2)+model(1,3)+model(1,4)); */ RLS_alg::RLS_alg(float lambda_RLS) : sysid() { int i; //eye_matrix_alloc(&P_RLS, m_s+m_a+m_b+m_c); // P_RLS: //cmulmat(&P_RLS, (float)1e-16, &P_RLS); P_RLS.mtx = P_RLS_v; P_RLS.row = ARMAX_dim; P_RLS.col = ARMAX_dim; P_RLS.dim = ARMAX_dim*ARMAX_dim; P_RLS_v[0] = SATRT_OF_VECTOR; P_RLS_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR; for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++) P_RLS_v[i]=(float)0.0; for(i=1; i<= ARMAX_dim;i++) P_RLS_v[i+((i-1)*ARMAX_dim)] = (float)1e-10; //loacal matrices: //matrix_alloc(&pi, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); // pi: pi.mtx = pi_v; pi.row = ARMAX_dim; pi.col = ARMAX_dim; pi.dim = ARMAX_dim*ARMAX_dim; pi_v[0] = SATRT_OF_VECTOR; pi_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR; for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++) pi_v[i]=(float)0.0; //matrix_alloc(&k, m_s+m_a+m_b+m_c, 1); // k: k.mtx = k_v; k.row = ARMAX_dim; k.col = 1; k.dim = ARMAX_dim; k_v[0] = SATRT_OF_VECTOR; k_v[ARMAX_dim+1] = END_OF_VECTOR; for(i=1;i<=ARMAX_dim;i++) k_v[i]=(float)0.0; //matrix_alloc(&Mtemp, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix Mtemp.mtx = Mtemp_v; Mtemp.row = ARMAX_dim; Mtemp.col = ARMAX_dim; Mtemp.dim = ARMAX_dim*ARMAX_dim; Mtemp_v[0] = SATRT_OF_VECTOR; Mtemp_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR; for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++) Mtemp_v[i]=(float)0.0; // parameters for identification: lambda = lambda_RLS; lambda_inv = 1.0/lambda_RLS; } //----------------------------------------------------------------------------- // Implementation: //----------------------------------------------------------------------------- /* function [theta,v,P] = RLS(psy,y_n,theta,P,lambda) % Recoursive Least Square (RLS) algorithm (Conventional) %---------------------------------------------------------- % 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: estimated error covariance : % E[(theta_^0 - theta_n-1)*(theta^0 - theta_n-1)^T] % where theta_^0 is the "thrue" system paramethers % lambda : forgeting factor (0<| System |-----+----> y(n) % | | | | % | `------------' | % | | % | _________ | % | | | v + _ % +---->| Model |---> + ---> v(n) = y(n) - y(n) % | w | - % `---------' V % A | % | correction | % +-------------------+ % % % % Recursion algorithm: % Residual in system identification (predicion error) v = y_n - theta' * psy ; % [1 x 1] % Calculate the uodate gain: pi = P * psy; % [ (ma+mb+mc) x 1] k = pi/(lambda + psy' * pi); % [ (ma+mb+mc) x 1] % Update recursively: theta = theta + k * v ; % [ (ma+mb+mc) x 1 ] P = (P - k * psy' * P)/lambda ; % [ (ma+mb+mc) x (ma+mb+mc)] % End of adaptation */ void RLS_alg::update(float y_n, float u_n_1) { // y_n the systems output at time n*T (present time) update_psy(u_n_1); // control signal from the end of privious calculation // Residual in system identification (predicion error): mulmat_t1(&Mtemp, &theta, &psy); // v = y_n - theta' * psy ; v_n = y_n - Mtemp.mtx[1]; // Residual in system identification // Calculate the update gain: k = pi/(lambda + psy' * pi); mulmat(&pi, &P_RLS, &psy); mulmat_t1(&Mtemp, &psy, &pi); cmulmat(&k,(1.0/(lambda+Mtemp.mtx[1])),&pi); //Update recursively: theta = theta + k * v ; cmulmat(&pi,v_n,&k); addmat(&theta, &theta, &pi); // <- The updated paramethers mulmat_t1(&pi, &psy, &P_RLS); // P = (P - k * psy' * P)/lambda ; mulmat(&Mtemp, &k, &pi); submat(&P_RLS, &P_RLS, &Mtemp); cmulmat(&P_RLS,lambda_inv,&P_RLS); 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: { count_n = NxT; // Start the next statistical evaluation } // print_matrix(&P_RLS,"P_RLS"); } //----------------------------------------------------------------------------- // DeInitialisation: //----------------------------------------------------------------------------- RLS_alg::~RLS_alg() // deInitialization { } //-----------------------------------------------------------------------------