/*
 *  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) : sysid()						   
{ 
	int i; 
 
	//s	= vectora(1, m_dim); 
	//float s_v[ARMAX_dim+2];
	s = s_v;
    s_v[0] = SATRT_OF_VECTOR;
    s_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim; i++) s_v[i] = 0.0; 


	//f	= vectora(1, ARMAX_dim);
	//float f_v[ARMAX_dim+2];
	f = f_v;
    f_v[0] = SATRT_OF_VECTOR;
    f_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim; i++) f_v[i] = 1.0; 
  
	 
	//fp	= vectora(1, m_dim);
	//float fp_v[ARMAX_dim+2];
	fp = fp_v;
    fp_v[0] = SATRT_OF_VECTOR;
    fp_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim; i++) fp_v[i] = 0.0; 
	
	//fs	= vectora(1, m_dim+1);
	//float fs_v[ARMAX_dim+2];
	fs = fs_v;	
    fs_v[0] = SATRT_OF_VECTOR;
    fs_v[ARMAX_dim+1+1] = END_OF_VECTOR;
	for(i=1;i<= (ARMAX_dim+1); i++) fs_v[i] = 0.0; 
	
	//beta= vectora(1, m_dim); 
	//float beta_v[ARMAX_dim+2];
	beta = beta_v;
    beta_v[0] = SATRT_OF_VECTOR;
    beta_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim; i++) beta_v[i] = 0.0;
 
	//matrix_alloc(&Mtemp, m_dim, m_dim); //temporary matrix 
	//float Mtemp_v[1+2];
	Mtemp.mtx = Mtemp_v;
	Mtemp.row = 1;
	Mtemp.col = 1;    
	Mtemp.dim = 1;
    Mtemp_v[0] = SATRT_OF_VECTOR;
    Mtemp_v[1+1] = END_OF_VECTOR;
	Mtemp_v[1] = 0.0;

	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];		// Residual in system identification  
 
	// Calculate the uodate step: 
 
	for(i=1;i<=ARMAX_dim; i++) 
	{ 
		f[i] = lambda * f[i]; 
		fp[i] = -lambda * s[i]; 
	} 
 
	//% 2.2 Add New Equations: 
	for(i=1;i<=ARMAX_dim; i++) fs[i] = psy.mtx[i];	 
	fs[ARMAX_dim+1] = -y_n; 
 
	// % 2.3 Transformation 
 
	PI =(float) 1.0; 
	PIpr = fs[1]; 
 
	for(i=1;i<=ARMAX_dim; i++) 
	{ 
		PIsc = PIpr*PIpr; 
		alfa = sqrt(f[i]*f[i] + PIsc); 
		
        sigma = alfa * ( alfa + fabs(f[i])); 
	    ro = 1.0 - (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[ARMAX_dim+1]) / sigma; 
		fp[i]= fp[i] - eta * tau; 
		fs[ARMAX_dim+1] = fs[ARMAX_dim+1] - PIpr*tau; 
		PI=PI*ro; 
		PIpr=PI*fs[i+1];		 
	} 
 
	// % 2.4 Backsolving 
	gama = 0; 
	s[ARMAX_dim] = -fp[ARMAX_dim]; 
	theta.mtx[ARMAX_dim] = s[ARMAX_dim] / f[ARMAX_dim]; 
 
	for(i=ARMAX_dim-1; i>=1; i--) 
	{  
    gama = gama + fs[i+1] * theta.mtx[i+1]; 
    s[i] = -fp[i] - beta[i] * gama; 
    theta.mtx[i] = 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: 
	{ 
		count_n = NxT; // Start the next statistical evaluation 	 
	} 
} 
 
//----------------------------------------------------------------------------- 
 
FQR_RLS_alg::~FQR_RLS_alg()  // deInitialization 
{ 

	 
} 
 
//-----------------------------------------------------------------------------