/* 
 *  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) : sysid()						  
{ 
	int i; 
 
	//matrix_alloc(&Mtemp, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //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;

	// 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); 
	//float R_v[(ARMAX_dim* ARMAX_dim)+2];
	R = R_v;
	R_v[0] = SATRT_OF_VECTOR;
    R_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;	
	for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++)  R[i]=(float)0.0; 
	for(i=1; i<= ARMAX_dim;i++) R[i+((i-1)*ARMAX_dim)] = (float)1e12;
	
	//u = matrixa(1, m_s+m_a+m_b+m_c+1, 1, m_s+m_a+m_b+m_c+1);  
	//float u_v[((ARMAX_dim+1)* (ARMAX_dim+1))+2];
	u = u_v;
	u_v[0] = SATRT_OF_VECTOR;
    u_v[((ARMAX_dim+1)* (ARMAX_dim+1))+1] = END_OF_VECTOR;
	for(i=1;i<=((ARMAX_dim+1)*(ARMAX_dim+1));i++)  u_v[i]=(float)0.0; 	
	
	//b = vectora( 1, m_s+m_a+m_b+m_c+1);
	//float b_v[ARMAX_dim+2];
	b = b_v;
	b_v[0] = SATRT_OF_VECTOR;
    b_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim;i++)  b_v[i]=(float)0.0; 

	//a = vectora( 1, m_s+m_a+m_b+m_c+1);
	//float a_v[ARMAX_dim+2];
	a = a_v;
	a_v[0] = SATRT_OF_VECTOR;
    a_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim;i++)  a_v[i]=(float)0.0;

	//s = vectora( 1, m_s+m_a+m_b+m_c+1);
	//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]=(float)0.0;

	//c = vectora( 1, m_s+m_a+m_b+m_c+1);	
	//float c_v[ARMAX_dim+2];
	c = c_v;
	c_v[0] = SATRT_OF_VECTOR;
    c_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim;i++)  c_v[i]=(float)0.0;
	
	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];		// Residual in system identification  
 
	// Calculate the uodate step: 
//========================== 
	int i,j;	 
	float z,sum; 
 
	for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++)  u[i] = 0.0; 
  
	b[1] =1.0;  
	for(i=1;i<=ARMAX_dim;i++) 
	{   
		sum=0.0; 
		for(j=1;j<=i;j++)  sum = sum+R[i+((j-1)*ARMAX_dim)] * psy.mtx[j]; 
 
		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-1)*ARMAX_dim)]=c[i]*u[i+((j-1)*ARMAX_dim)]+lambdaisqrt*s[i]*R[i+((j-1)*ARMAX_dim)]; 
			R[i+((j-1)*ARMAX_dim)]= lambdaisqrt * c[i] * R[i+((j-1)*ARMAX_dim)] - s[i] * u[i+((j-1)*ARMAX_dim)]; 
		} 
	} 

	z=v_n/b[ARMAX_dim+1];  
	for(i=1;i<=ARMAX_dim;i++)    
	theta.mtx[i]=theta.mtx[i]+z*u[ARMAX_dim+1+((i-1)*ARMAX_dim)];   
//========================== 
	// 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 
	} 
} 
 
//----------------------------------------------------------------------------- 
 
IQR_RLS_alg::~IQR_RLS_alg()  // deInitialization 
{ 
 

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