/* 
 *  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.    
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     IQR_RLS . cpp	                                                   */ 
/*                                                                         */ 
/*     Recoursive Least Mean Square Algorithm inplementation.              */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 
#include "IQR_RLS.hpp" 
 
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 
/* 
function [theta,R,lambdisqrt] = IQR_RLS_init(model,lambda) 
 
N=model(1,1)+model(1,2)+model(1,3)+model(1,4); 
 
for i=1:N 
   theta(i,1)=0; 
   psy(i,1)=0;  
end 
 
beta = 10^12; 
R= eye(N,N); 
 
R=beta*R; 
 
lambdisqrt=1/sqrt(lambda); 
 
*/ 
 
IQR_RLS_alg::IQR_RLS_alg(float fa,int is,int ia,int ib,int ic) : sysid(is,ia,ib,ic)						  
{ 
	int i; 
 
	matrix_alloc(&Mtemp, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix 
	 
	// Matrix and wector in elementary form: 
	R = matrixa(1, m_s+m_a+m_b+m_c, 1, m_s+m_a+m_b+m_c);  
	for(i=1;i<=m_dim;i++) for(int j=1;j<=m_dim;j++) R[i][j]=(float)0; 
	for(i=1;i<=m_dim;i++) R[i][i] = (float)1e12; 
 
	u = matrixa(1, m_s+m_a+m_b+m_c+1, 1, m_s+m_a+m_b+m_c+1);  
	 
	b = vectora( 1, m_s+m_a+m_b+m_c+1); 
	a = vectora( 1, m_s+m_a+m_b+m_c+1); 
	s = vectora( 1, m_s+m_a+m_b+m_c+1); 
	c = vectora( 1, m_s+m_a+m_b+m_c+1);	 
	 
	 
 
	lambdaisqrt = 1/sqrt(fa); 
} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
/* 
function [theta,v,Rn] = IQR_RLS(psy,dn,theta,R,model,lambdaisqrt) 
% Inverse QR RLS algorithm 
% S. Thomas Alexande,..:IEEE Transaction on Signal Processing 
%  Vol 41, No. 1., pp.20-30, January 1993 
 
N = model(1,1)+model(1,2)+model(1,3)+model(1,4); 
 
v= dn - theta'*psy ; 
 
for j = 1:N 
   for k = 1:N 
		u(k,j) = 0; 
	end 
end 
 
b(1) =1; 
 
 
for i = 1:N 
    
   sum=0; 
   for j=1:i       
      sum=sum+R(i,j)*psy(j,1); 
   end 
    
   a(i) = lambdaisqrt *sum; 
    
   b(i+1) = sqrt((b(i)^2)+(a(i)^2)); 
   s(i)=a(i)/b(i+1); 
   c(i)=b(i)/b(i+1); 
    
 for j = 1:i 
    u(i+1,j)=c(i)*u(i,j)+lambdaisqrt*s(i)*R(i,j); 
    R(i,j)= lambdaisqrt*c(i)*R(i,j)- s(i)*u(i,j); 
 end 
end 
 
z=v/b(N+1); 
 
for k=1:N 
    
   theta(k)=theta(k)+z*u(N+1,k); 
    
end  
    
  Rn=R;   
*/ 
 
void IQR_RLS_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,j;	 
	float z,sum; 
 
	for(i=1;i<=m_dim;i++) for(j=1;j<=m_dim;j++) u[i][j] = 0; 
 
 
	b[1] =1; 
 
	for(i=1;i<=m_dim;i++) 
	{   
		sum=0; 
		for(j=1;j<=i;j++)  sum = sum+R[i][j] * psy.mtx[j][1]; 
 
		a[i] = lambdaisqrt *sum; 
    
		b[i+1] = sqrt((b[i]*b[i])+(a[i]*a[i])); 
		s[i]=a[i]/b[i+1]; 
		c[i]=b[i]/b[i+1]; 
    
		for(j=1;j<=i;j++) 
		{ 
			u[i+1][j]=c[i]*u[i][j]+lambdaisqrt*s[i]*R[i][j]; 
			R[i][j]= lambdaisqrt * c[i] * R[i][j] - s[i] * u[i][j]; 
		} 
	} 
 
	z=v_n/b[m_dim+1]; 
 
	for(i=1;i<=m_dim;i++) 
    
	theta.mtx[i][1]=theta.mtx[i][1]+z*u[m_dim+1][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 IQR_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 
	 
	} 
 
 
 
} 
 
//----------------------------------------------------------------------------- 
 
IQR_RLS_alg::~IQR_RLS_alg()  // deInitialization 
{ 
// Remouve the alocation from the heep 
//	matrix_delete(&Mtemp); 

	matrix_delete(&Mtemp); 
	free_matrix(R, 1,m_s+m_a+m_b+m_c, 1, m_s+m_a+m_b+m_c);  
	free_matrix(u, 1,m_s+m_a+m_b+m_c, 1, m_s+m_a+m_b+m_c+1);
		free_vector(b, 1, m_s+m_a+m_b+m_c+1); 	 
		free_vector(a, 1, m_s+m_a+m_b+m_c+1); 
		free_vector(s, 1, m_s+m_a+m_b+m_c+1); 
		free_vector(c, 1, m_s+m_a+m_b+m_c+1); 
} 
 
//--------------------------------------------------------------------------