/*
 *  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.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     FQR_RLS . cpp	                                                   */ 
/*                                                                         */ 
/*     Recoursive Fast QR RLS Algorithm inplementation.			   */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 
#include "FQR_RLS.hpp" 
 
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 
/* 
function [theta,s,f,lambda] = FQR_RLS_init(model,lambdai) 
 
lambda=lambdai; 
 
N=model(1,1)+model(1,2)+model(1,3)+model(1,4); 
 
 
for i=1:N 
  theta(i,1)=0 ; 
  s(i)=0;    
  %f(i,i)=1; 
  f(i)=1; 
end 
*/ 
 
FQR_RLS_alg::FQR_RLS_alg(float fa,int is,int ia,int ib,int ic) : sysid(is,ia,ib,ic)						   
{ 
	int i; 
 
	s	= vectora(1, m_dim); 
	for(i=1;i<=m_dim; i++) s[i] = 0; 
 
	f	= vectora(1, m_dim); 
	for(i=1;i<=m_dim; i++) f[i] = 1; 
	 
	fp	= vectora(1, m_dim); 
	fs	= vectora(1, m_dim+1); 
	beta= vectora(1, m_dim); 
 
	matrix_alloc(&Mtemp, m_dim, m_dim); //temporary matrix 
	lambda = fa; 
 
} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
/* 
function [theta,v,s,f] = FQR_RLS(psy,dn,theta,s,f,model,lambda) 
% Fasr QR RLS Adaptive Algorithm 
% Zheng-She Liu:IEEE Transaction on Signal Processing 
%  Vol 43, No.3, pp720-728, March 1995 
% 
%  Recursion: s, f vectors 
% 2 Recursive operation: 
% 2.1 Multiply witing factor to old equations 
%------------------------------------------------- 
elements=(model(1,1)+model(1,2)+model(1,3)+model(1,4)); 
% Prediction error: 
   v=0; 
  for i=1:elements   
    v=v+psy(i,1)*theta(i,1) ;       
 end 
 v = dn - v; 
%---------------------------------------------- 
 
 
 
for i = 1:elements 
   f(i)=lambda * f(i) ; 
   fp(i)=-lambda * s(i) ; 
 
end 
 
% 2.2 Add New Equations: 
 
for i = 1:elements  
  fs(i) = psy(i,1);  
end 
fs(elements+1) = -dn; 
 
% 2.3 Transformation 
 
PI=1; 
PIpr=fs(1); 
 
for i = 1:elements 
    
   PIsc = PIpr^2; 
    
   alfa = sqrt((f(i)^2)+PIsc); 
   sigma = alfa * ( alfa + abs( f(i))); 
   ro = 1 -(PIsc/sigma); 
   eta = f(i) + sign(f(i))*alfa; 
   beta(i) = -sign(f(i))*( PI * PIpr ) / alfa ; 
   f(i) = -sign(f(i)) * alfa; 
   tau = (eta * fp(i) +PIpr * fs(elements+1))/ sigma; 
   fp(i)= fp(i)- eta * tau; 
   fs(elements+1) = fs(elements+1) - PIpr*tau; 
   PI=PI*ro; 
   PIpr=PI*fs(i+1); 
    
    
end 
    
   % 2.4 Backsolving 
    
   gama = 0; 
   s(elements) = -fp(elements); 
   theta(elements) = s(elements) / f(elements); 
    
    
 for i = elements-1:-1:1 
     
    gama = gama + fs(i+1) * theta(i+1); 
    s(i) = -fp(i) - beta(i) * gama; 
    theta(i) = s(i) / f(i); 
     
 end 
*/ 
 
void FQR_RLS_alg::update(float y_n, float u_n_1) 
{ 
	// y_n the systems output at time n*T (present time) 
 
	int i; 
	float PI; 
	float PIpr; 
	float PIsc; 
	float alfa; 
	float sigma; 
	float ro; 
	float eta; 
	float tau; 
	float gama; 
 
	//Pre update: 
	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 step: 
 
	for(i=1;i<=m_dim; i++) 
	{ 
		f[i] = lambda * f[i]; 
		fp[i] = -lambda * s[i]; 
	} 
 
	//% 2.2 Add New Equations: 
	for(i=1;i<=m_dim; i++) fs[i] = psy.mtx[i][1];	 
	fs[m_dim+1] = -y_n; 
 
	// % 2.3 Transformation 
 
	PI =(float) 1.0; 
	PIpr = fs[1]; 
 
	for(i=1;i<=m_dim; i++) 
	{ 
 
		PIsc = PIpr*PIpr; 
    
		alfa = sqrt(f[i]*f[i] + PIsc); 
		
                sigma = alfa * ( alfa + fabs(f[i])); 
		 ro = 1 - (PIsc/sigma); 
		eta = f[i] + (sign(f[i])*alfa);  
		beta[i] = sign(f[i])*( -PI * PIpr ) / alfa ; 
		f[i] = sign(f[i]) * (-alfa); 
		tau = (eta * fp[i] + PIpr * fs[m_dim+1]) / sigma; 
		fp[i]= fp[i] - eta * tau; 
		fs[m_dim+1] = fs[m_dim+1] - PIpr*tau; 
		PI=PI*ro; 
		PIpr=PI*fs[i+1];		 
	} 
 
	// % 2.4 Backsolving 
 
	gama = 0; 
	s[m_dim] = -fp[m_dim]; 
	theta.mtx[m_dim][1] = s[m_dim] / f[m_dim]; 
 
	for(i=m_dim-1; i>=1; i--) 
	{  
    gama = gama + fs[i+1] * theta.mtx[i+1][1]; 
    s[i] = -fp[i] - beta[i] * gama; 
    theta.mtx[i][1] = s[i] / f[i]; 
     
	} 
 
	 
	// Post Update: 
	update_psy(y_n, v_n); // Updates the psy vector with time shift 
	 //(u_n will be calculated based on the estimated theta vector in future) 
 
 
	// Statistic calculation for the system identification: 
    if(update_stat() == 1 ) // The results are redy: 
	{ 
#if 0
		printf("\n FQR_RLS ---->"); 
		printf("\n v: N(%g, %g), v_H2 = %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 
	 
	} 
 
 
 
} 
 
//----------------------------------------------------------------------------- 
 
FQR_RLS_alg::~FQR_RLS_alg()  // deInitialization 
{ 
// Remouve the alocation from the heep 
	matrix_delete(&Mtemp); 
	free_vector(s, 1, m_dim);  
	free_vector(f, 1, m_dim); 
	free_vector(fp, 1, m_dim); 
	free_vector(fs, 1, m_dim+1); 
	free_vector(beta, 1, m_dim); 
	 
} 
 
//-----------------------------------------------------------------------------