/*
 *  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.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     QR_LMS . cpp	                                                   */ 
/*                                                                         */ 
/*     Recoursive Least Mean Square Algorithm inplementation.              */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 
#include "QR_LMS.hpp" 
 
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 
/* 
function [theta] = QR_LMS_init(model) 
% Neccessery initialization for QR-LMS 
 
m=model(1,1)+model(1,2)+model(1,3)+model(1,4); 
 
for i=1:m 
  theta(i,1)=0 ; 
end 
*/ 
 
QR_LMS_alg::QR_LMS_alg(int is, int ia, int ib, int ic) : sysid(is,ia,ib,ic)						  
{ 
 
//	matrix_alloc(&Mtemp, m_dim, m_dim); //temporary matrix 
	FI = vectora( 1, m_dim+1); 
	FIm = vectora( 1, m_dim+1); 
	PI = vectora( 1, m_dim+1); 
	PIpr = vectora( 1, m_dim+1); 
	PIsc = vectora( 1, m_dim+1); 
	alfa = vectora( 1, m_dim); 
	beta = vectora( 1, m_dim); 
	sigma = vectora( 1, m_dim); 
	ro = vectora( 1, m_dim); 
	eta = vectora( 1, m_dim); 
	gama = vectora( 1, m_dim); 
	r = vectora( 1, m_dim); 
		 
 
} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
#if 0
/* 
function [theta,p_error] = QR_LMS(FI,dn,theta) 
% QR-LMS based adaptive algorithm 
 
% Zheng-She Liu:IEEE Transaction on ircuits and systems -II 
%  Vol 45, no 3, March 1998 pp324 
[nr,nc]=size(theta); 
N=nr; 
 
FI(N+1)=-dn; 
p_error=dn; 
for i=1:N 
   p_error=p_error-(theta(i)*FI(i)); 
end 
 
PI(1) = 1; 
PIpr(1)=PI(1)*FI(1); 
FIm(1)=FI(N+1); 
 
wm=0; 
for i=1:N 
   wm=wm+FI(i)^2; 
end 
wm=(wm/N) ; % <=> lambda=0.5 
 
for i = 1:N 
 
PIsc(i)=PIpr(i)^2; 
alfa(i)=sqrt(wm^2+PIsc(i)); 
sigma(i)=alfa(i)*(alfa(i)+wm); 
ro(i)=1-(PIsc(i)/sigma(i)); 
PI(i+1)=ro(i)*PI(i); 
PIpr(i+1)=PI(i+1)*FI(i+1); 
eta(i)=wm+alfa(i); 
beta(i)=-(PI(i)*PIpr(i))/alfa(i); 
tau_pr=((PIpr(i)*FIm(i))-(eta(i)*wm*theta(i)))/sigma(i); 
r(i)=-(wm*theta(i))-(eta(i)*tau_pr); 
FIm(i+1)=FIm(i)-(PIpr(i)*tau_pr); 
 
end   
   % 2.4 Backsolving 
    
   gama(N) = 0; 
   theta(N) =r(N) / alfa(N); 
    
 for i = N-1:-1:1 
     
    gama(i) = gama(i+1) + FI(i+1) * theta(i+1); 
    theta(i) = (r(i) +(beta(i)*gama(i)))/alfa(i); 
     
 end 
*/
#endif  
 
void QR_LMS_alg::update(float y_n, float u_n_1) 
{ 
	// y_n the systems output at time n*T (present time) 
 
   
	//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: 
 
//----------------------------------------------------------------------------- 
	int i; 
	float p_error,wm,tau_pr; 
 
	for (i=1;i<=m_dim;i++)  FI[i] = psy.mtx[i][1]; 
	FI[m_dim+1]=-y_n; 
 
	p_error=y_n; 
	for (i=1;i<=m_dim;i++)   p_error=p_error-(theta.mtx[i][1]*FI[i]); 
	 
	v_n = p_error;  
 
	PI[1] = 1; 
	PIpr[1]=PI[1]*FI[1]; 
	FIm[1]=FI[m_dim+1]; 
 
	wm=0; 
	for (i=1;i<=m_dim;i++)   wm=wm+(FI[i]*FI[i]); 


 	wm=(wm/m_dim) * 1.0 ; //% wm=(wm/m_dim) * 0.001 ;  <=> lambda=0.5 if mupltiplier =1.0
	// original 2004 Feb 10 was:	wm=(wm/m_dim) * 2.80 ; //% <=> lambda=0.5 if mupltiplier =1.0
 
	for(i=1;i<=m_dim;i++) 
	{ 
		PIsc[i]=PIpr[i]*PIpr[i]; 
		alfa[i]=sqrt((wm*wm)+PIsc[i]); 
		sigma[i]=alfa[i]*(alfa[i]+wm); 
		ro[i]=1-(PIsc[i]/sigma[i]); 
		PI[i+1]=ro[i]*PI[i]; 
		PIpr[i+1]=PI[i+1]*FI[i+1]; 
		eta[i]=wm+alfa[i]; 
		beta[i] = -(PI[i]*PIpr[i] ) / alfa[i]; 
		tau_pr = ( (PIpr[i]*FIm[i]) - (eta[i]*wm*theta.mtx[i][1]) ) / sigma[i]; 
		r[i]=-(wm*theta.mtx[i][1])-(eta[i]*tau_pr); 
		FIm[i+1]=FIm[i]-(PIpr[i]*tau_pr); 
	}  
 
   //% 2.4 Backsolving 
    
	gama[m_dim] = 0; 
	theta.mtx[m_dim][1] =r[m_dim] / alfa[m_dim]; 
    
	for(i=m_dim-1;i>=1;i--)	// i = N-1:-1:1 
    {  
    gama[i] = gama[i+1] + FI[i+1] * theta.mtx[i+1][1]; 
    theta.mtx[i][1] = (r[i] +(beta[i]*gama[i]))/alfa[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 QR_LMS ---->"); 
		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 
	 
	} 
 
 
 
} 
 
//----------------------------------------------------------------------------- 
 
QR_LMS_alg:: ~QR_LMS_alg()  // deInitialization 
{ 
// Remouve the alocation from the heep 
//	matrix_delete(&Mtemp);
    free_vector(FI, 1, m_dim+1);
    free_vector(FIm, 1, m_dim+1);
    free_vector(PI, 1, m_dim+1);
    free_vector(PIpr, 1, m_dim+1);
    free_vector(PIsc, 1, m_dim+1);
    free_vector(alfa, 1, m_dim);
    free_vector(beta, 1, m_dim);
    free_vector(sigma, 1, m_dim);
    free_vector(ro, 1, m_dim);
    free_vector(eta, 1, m_dim);
    free_vector(gama, 1, m_dim);
    free_vector(r, 1, m_dim);

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