/* 
 *  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,int is, int ia, int ib, int ic) : sysid(is,ia,ib,ic)						  
{ 
 
 
 
	eye_matrix_alloc(&P_RLS, m_s+m_a+m_b+m_c);	// P_RLS: 
	cmulmat(&P_RLS, (float)1e-16, &P_RLS); 
	//loacal matrices: 
	matrix_alloc(&pi, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c);	// pi: 
	matrix_alloc(&Mtemp, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix 
	matrix_alloc(&k, m_s+m_a+m_b+m_c, 1);	// k: 
	 
	// parameters for identification: 
	lambda = lambda_RLS; 
	lambda_inv = 1/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<<lambda <1) 
% 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 = eye((m(1,1)+m(1,2)+m(1,3)),1); 
%   theta = theta-theta ;% zero matrix 
% 
%              ____________ 
%             |            | 
%    u(n) -+->|   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][1];		// Residual in system identification  
 
	// Calculate the uodate gain: k = pi/(lambda + psy' * pi);  
	mulmat(&pi, &P_RLS, &psy); 
	mulmat_t1(&Mtemp, &psy, &pi); 
	cmulmat(&k,(1/(lambda+Mtemp.mtx[1][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: 
 
	{	
 #if 0  
	        printf("\n\n\n ======================================="); 
			printf("\n RLS ---->"); 
		printf("\n v: N(%g, %g), v_RMS = %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); 

		print_matrix(&theta_moav,"theta_movimg_averadge RLS"); 
		print_matrix(&theta_vari,"theta_variance RLS"); 
		print_matrix(&theta_qume,"theta_quadratic_mean RLS"); 
		 
		print_matrix(&theta_maxd,"theta_max RLS"); 
		print_matrix(&theta_mind,"theta_min RLS"); 
#endif 
		count_n = NxT; // Start the next statistical evaluation 
	 
	} 
 
	 //(u_n will be calculated based on the estimated theta vector in future) 
 
	// print_matrix(&P_RLS,"P_RLS"); 
} 
 
 
//----------------------------------------------------------------------------- 
// DeInitialisation: 
//----------------------------------------------------------------------------- 
 
RLS_alg::~RLS_alg()  // deInitialization 
{ 
// Remouve the alocation from the heep 
	matrix_delete(&P_RLS); 
	matrix_delete(&pi); 
	matrix_delete(&k); 
	matrix_delete(&Mtemp); 
} 
 
//-----------------------------------------------------------------------------