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

	//FIm = vectora( 1, ARMAX_dim+1); 
	//float FIm_v[ARMAX_dim+1+2];
	FIm = FIm_v;
	FIm_v[0] = SATRT_OF_VECTOR;
	FIm_v[ARMAX_dim+1+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim+1; i++) FIm_v[i] = 0.0;

	//PI = vectora( 1, ARMAX_dim+1); 
	//float PI_v[ARMAX_dim+1+2];
	PI = PI_v;
	PI_v[0] = SATRT_OF_VECTOR;
	PI_v[ARMAX_dim+1+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim+1; i++) PI_v[i] = 0.0;

	//PIpr = vectora( 1, ARMAX_dim+1);
	//float PIpr_v[ARMAX_dim+1+2];
	PIpr = PIpr_v;
	PIpr_v[0] = SATRT_OF_VECTOR;
	PIpr_v[ARMAX_dim+1+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim+1; i++) PIpr_v[i] = 0.0;
	
	//PIsc = vectora( 1, ARMAX_dim+1); 
	//float PIsc_v[ARMAX_dim+1+2];
	PIsc = PIsc_v;
	PIsc_v[0] = SATRT_OF_VECTOR;
	PIsc_v[ARMAX_dim+1+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim+1; i++) PIsc_v[i] = 0.0;

	//alfa = vectora( 1, ARMAX_dim); 
	//float alfa_v[ARMAX_dim+2];
	alfa = alfa_v;
	alfa_v[0] = SATRT_OF_VECTOR;
	alfa_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) alfa_v[i] = 0.0;

	//beta = vectora( 1, ARMAX_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;

	//sigma = vectora( 1, ARMAX_dim); 
	//float sigma_v[ARMAX_dim+2];
	sigma = sigma_v;
	sigma_v[0] = SATRT_OF_VECTOR;
	sigma_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) sigma_v[i] = 0.0;

	//ro = vectora( 1, ARMAX_dim); 
	//float ro_v[ARMAX_dim+2];
	ro = ro_v;
	ro_v[0] = SATRT_OF_VECTOR;
	ro_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) ro_v[i] = 0.0;

	//eta = vectora( 1, ARMAX_dim); 
	//float eta_v[ARMAX_dim+2];
	eta = eta_v;
	eta_v[0] = SATRT_OF_VECTOR;
	eta_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) eta_v[i] = 0.0;

	//gama = vectora( 1, ARMAX_dim); 
	//float gama_v[ARMAX_dim+2];
	gama = gama_v;
	gama_v[0] = SATRT_OF_VECTOR;
	gama_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) gama_v[i] = 0.0;

	//r = vectora( 1, ARMAX_dim);
	//float r_v[ARMAX_dim+2];
	r = r_v;
	r_v[0] = SATRT_OF_VECTOR;
	r_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) r_v[i] = 0.0;	
} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
/* 
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 
*/ 
 
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<=ARMAX_dim;i++)  FI[i] = psy.mtx[i]; 
	FI[ARMAX_dim+1]=-y_n; 
 
	p_error=y_n; 
	for (i=1;i<=ARMAX_dim;i++)   p_error=p_error-(theta.mtx[i]*FI[i]); 
	 
	v_n = p_error;  
 
	PI[1] = 1; 
	PIpr[1]=PI[1]*FI[1]; 
	FIm[1]=FI[ARMAX_dim+1]; 
 
	wm=0; 
	for (i=1;i<=ARMAX_dim;i++)   wm=wm+FI[i]*FI[i]; 


 	wm=(float)(wm/(ARMAX_dim*1.0)) * 2.80 ; //% <=> lambda=0.5 if mupltiplier =1.0
	// original 2004 Feb 10 was:	wm=(wm/ARMAX_dim) * 2.80 ; //% <=> lambda=0.5 if mupltiplier =1.0
 
	for(i=1;i<=ARMAX_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]) ) / sigma[i]; 
		r[i]=-(wm*theta.mtx[i])-(eta[i]*tau_pr); 
		FIm[i+1]=FIm[i]-(PIpr[i]*tau_pr); 
	}  
 
   //% 2.4 Backsolving 
    
	gama[ARMAX_dim] = 0; 
	theta.mtx[ARMAX_dim] =r[ARMAX_dim] / alfa[ARMAX_dim]; 
    
	for(i=ARMAX_dim-1;i>=1;i--)	// i = N-1:-1:1 
    {  
    gama[i] = gama[i+1] + FI[i+1] * theta.mtx[i+1]; 
    theta.mtx[i] = (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: 
	{  
		count_n = NxT; // Start the next statistical evaluation 
	} 
 } 
 
//----------------------------------------------------------------------------- 
 
QR_LMS_alg:: ~QR_LMS_alg()  // deInitialization 
{ 

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