/* 
 *  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.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     LMS . cpp	                                                       */ 
/*                                                                         */ 
/*     Recoursive Least Mean Square Algorithm inplementation.              */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 
#include "LMS.hpp" 
 
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 
/* 
function [theta,a] = LMS_init(model,a) 
% Least Mean Square (LMS) algorithm initialization(Conventional) 
%---------------------------------------------------------- 
%  Inputs: 
%  model : model = [ma,mb,mc]   where-> 
%        psy = [-y_n-1,...-y_n-ma,u_n-1,...u_n-mb,v_n-1,...v_n-mc] 
%         vhere the y_xx index _xx is the time: t=T*xx  
% a : step size waigting factor a >= 0 
%  
% Outputs: 
%   theta : zero vector etimated system paramethers  
%   a : step size waigting factor a >= 0 
% 
 
% Zero initialization: 
  theta = eye((model(1,1)+model(1,2)+model(1,3)+model(1,4)),1); 
  theta = theta-theta ;% zero matrix 
*/ 
 
LMS_alg::LMS_alg(float fa) : sysid()						  
{ 
	a = fa; //step size waigting factor a >= 0 initialization 
	//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 = ARMAX_dim;
	Mtemp.col = 1;    
	Mtemp.dim = ARMAX_dim;
    Mtemp_v[0] = SATRT_OF_VECTOR;
    Mtemp_v[ARMAX_dim+1] = END_OF_VECTOR;
    Mtemp_v[1] = 0.0;
} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
/* 
function [theta,v] = LMS(psy,y_n,theta,a) 
%  Recoursive Least Square (RLS) algorithm (Conventional) 
%---------------------------------------------------------- 
%  Inputs: 
% psy : [-y_n-1,...-y_n-ma,u_n-1,...u_n-mb,v_n-1,...v_n-mc] 
%       vhere the y_xx index _xx is the time: t=T*xx  
% y_n : system output at time t=T*n  
% theta : etimated system paramethers at t=T*(n-1) (theta_n-1) 
% a : step size waigting factor a >= 0 
%  
% Outputs: 
%   theta : etimated system paramethers  
%   v: residual in system identification 
% 
% For recursion:  
%   theta : theta_n-1 (estimated paramethers) 
% 
% Initialization: 
%   theta = eye((m(1,1)+m(1,2)+m(1,3)),1); 
%   theta = theta-theta ;% zero matrix 
% 
%              ____________ 
%             |            | 
%    u(n) -+->|   System   |-----+----> y(n) 
%          |  |            |     | 
%          |  `------------'     | 
%          |                     | 
%          |      _________      | 
%          |     |         |     v +                   _ 
%          +---->|  Model  |---> + --->  v(n) = y(n) - y(n) 
%                |    w    |   -            
%                `---------'              V 
%                     A                   | 
%                     |     correction    | 
%                     +-------------------+ 
% 
%  
  
  
% Recursion algorithm: 
% Residual in system identification (predicion error) 
 v = y_n - theta' * psy ;      %  [1 x 1] 
 % Calculate the uodate step: 
 miu = 1/(a + psy' * psy); 
% Update recursion: 
  theta = theta + miu * psy * v ;       % [ (ma+mb+mc) x 1 ]  
 % End of adaptation 
*/ 
 
void LMS_alg::update(float y_n, float u_n_1) 
{ 
	// y_n the systems output at time n*T (present time) 
 
  
float miu; // step size 
 
	//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: 
	mulmat_t1(&Mtemp, &psy, &psy);  // Mtemp[1][1] = psy' * psy ;  
	miu = 1.0 / (a + a * Mtemp.mtx[1]); 
 
	// Update recursion: 
	cmulmat(&Mtemp, (miu*v_n), &psy); 
	addmat(&theta, &theta, &Mtemp); 
	 
	// 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 
	 
	} 
 
 
} 
 
//----------------------------------------------------------------------------- 
 
LMS_alg::~LMS_alg()  // deInitialization 
{ 

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