/* 
 *  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.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     KF . cpp	                                                           */ 
/*                                                                         */ 
/*     Recoursive Kalman Filter Algorithm inplementation.                  */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 
#include "KF.hpp" 
 
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 
/* 
function [theta,P_KF,R,Q] = KF_init(model,r,q) 
% Recoursive Least Square (RLS) 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  
% lambda : forgetting factor (0 << lambda < 1) 
%  
% Outputs: 
%   theta : zero vector - etimated system paramethers  
%   P: estimated error covariance diagonal matrix  
%   lambda : forgetting factor (0 << lambda < 1) 
% 
 
% Initialisations of weight wector : 
theta = eye((model(1,1)+model(1,2)+model(1,3)+model(1,4)),1); 
theta = theta-theta ;% zero matrix 
 
% Estimated error covariance diagonal matrix initialization                        
P_KF=10^12 * eye(model(1,1)+model(1,2)+model(1,3)+model(1,4)); 
 
R = 1*r; 
 
Q = q * eye(model(1,1)+model(1,2)+model(1,3)+model(1,4)); 
 
*/ 
 
KF_alg::KF_alg(float r,float q) : sysid()						  
{ 
	int i;

	//eye_matrix_alloc(&P_KF, m_s+m_a+m_b+m_c);	// P_KF:
	//cmulmat(&P_KF, (float)1e12, &P_KF); 
	//float P_KF_v[(ARMAX_dim*ARMAX_dim)+2];
	P_KF.mtx = P_KF_v;
	P_KF.row = ARMAX_dim;
	P_KF.col = ARMAX_dim;    
	P_KF.dim = ARMAX_dim*ARMAX_dim;
    P_KF_v[0] = SATRT_OF_VECTOR;
    P_KF_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<= (ARMAX_dim*ARMAX_dim); i++) P_KF_v[i] = 0.0;
    for(i=1;i<= ARMAX_dim; i++) P_KF_v[i+((i-1)*ARMAX_dim)] = (float)1e12;

	 
	//eye_matrix_alloc(&R, 1);	// R: 
	//cmulmat(&R, r, &R); 
	//float R_v[1+2];
	R.mtx = R_v;
	R.row = 1;
	R.col = 1;    
	R.dim = 1;
    R_v[0] = SATRT_OF_VECTOR;
    R_v[1+1] = END_OF_VECTOR;
	R_v[i] = r;
    
	//eye_matrix_alloc(&Q, m_s+m_a+m_b+m_c);	// Q: 
	//cmulmat(&Q, q, &Q); 
	//float Q_v[(ARMAX_dim*ARMAX_dim)+2];
	Q.mtx = Q_v;
	Q.row = ARMAX_dim;
	Q.col = ARMAX_dim;    
	Q.dim = (ARMAX_dim*ARMAX_dim);
    Q_v[0] = SATRT_OF_VECTOR;
    Q_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<= (ARMAX_dim*ARMAX_dim); i++) Q_v[i] = 0.0;
    for(i=1;i<= ARMAX_dim; i++) Q_v[i+((i-1)*ARMAX_dim)] = q;
	
	
	//matrix_alloc(&k, m_s+m_a+m_b+m_c, 1); //temporary matrix 
 	//float k_v[ARMAX_dim+2];
	k.mtx = k_v;
	k.row = ARMAX_dim;
	k.col = 1;    
	k.dim = ARMAX_dim;
    k_v[0] = SATRT_OF_VECTOR;
    k_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<= ARMAX_dim; i++) k_v[i] = 0.0;

	//matrix_alloc(&MtempA, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix 
	//float MtempA_v[(ARMAX_dim*ARMAX_dim)+2];
	MtempA.mtx = MtempA_v;
	MtempA.row = ARMAX_dim;
	MtempA.col = ARMAX_dim;  
	MtempA.dim = (ARMAX_dim*ARMAX_dim);
    MtempA_v[0] = SATRT_OF_VECTOR;
    MtempA_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<= (ARMAX_dim*ARMAX_dim); i++) MtempA_v[i] = 0.0;

	//matrix_alloc(&MtempB, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix
	//float MtempB_v[(ARMAX_dim*ARMAX_dim)+2];
	MtempB.mtx = MtempB_v;
	MtempB.row = ARMAX_dim;
	MtempB.col = ARMAX_dim;    
	MtempB.dim = (ARMAX_dim*ARMAX_dim);
    MtempB_v[0] = SATRT_OF_VECTOR;
    MtempB_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<= (ARMAX_dim*ARMAX_dim); i++) MtempB_v[i] = 0.0;	
} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
/* 
function [theta,v,P,R,Q] = KF(psy,y_n,theta,P,R,Q) 
%  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) 
% P: estimated error covariance : 
%         E[(theta_^0 - theta_n-1)*(theta^0 - theta_n-1)^T] 
%         where theta_^0 is the "thrue" system paramethers 
% lambda : forgeting factor (0<<lambda <1) 
% m : model order [ma,mb,mc] 
%  
% Outputs: 
%   theta : etimated system paramethers  
%   v: residual in system identification 
% 
% For recursion:  
%   P : P_n-1 (estimated error covariance) 
%   theta : theta_n-1 (estimated paramethers) 
% 
% Initialization: 
%   P = eye(m(1,1)+m(1,2)+m(1,3)) * 10^22; 
%   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] 
 %%%%%%%% Insert 
 R=R*0.99999; 
 R=max((v^2)*5,R); 
 %%%%%%%% End of insert 
 % Calculate the uodate gain: 
  r_KF_inv = 1 / (R + psy' * P * psy);              %  [ (ma+mb+mc) x 1]   
  k = P * psy * r_KF_inv ;   %  [ (ma+mb+mc) x 1]   
% Update recursively: 
  %%theta = theta + k * v ;       % [ (ma+mb+mc) x 1 ]  
  delta_theta = k*v; 
  theta = theta + delta_theta; 
  %%%%%%%%%%%%%%% Insert *********************** 
  Q=Q*0.99999; 
  [iq,jq] = size(Q); 
  for i=1:iq 
      Q(i,i)=max(Q(i,i),(delta_theta(i,1)^2)*3);     
  end 
  %%%%% end of insert **************************** 
  % The riccaty recursion is: 
  P = P + Q - P * psy * r_KF_inv * psy' & P ; % [ (ma+mb+mc) x (ma+mb+mc)] 
 % End of adaptation 
 
*/ 
 
void KF_alg::update(float y_n, float u_n_1) 
{ 
	// y_n the systems output at time n*T (present time) 
	
	float r_KF_inv; 
 
	//Pre update: 
	update_psy(u_n_1); // control signal from the end of privious calculation  
 
	// Residual in system identification (predicion error): 
	mulmat_t1(&MtempA, &psy, &theta);  // v = y_n - psy' * theta ;  
	v_n = y_n - MtempA.mtx[1];		// Residual in system identification  
//	v2 = v_n *v_n; 
 
	// Calculate the uodate step: 
	//%%%%%%%% Insert 
	//R.mtx[1][1]=R.mtx[1][1]*0.99 + 0.001; 
	//if(v2*5 > R.mtx[1][1]) R.mtx[1][1] = v2*5; 
	//if(R.mtx[1][1] > 10) R.mtx[1][1] = 10; 
	// %%%%%%%% End of insert 
 
	// % Calculate the uodate gain: 
	//r_KF_inv = 1 / (R + psy' * P * psy);  
	mulmat(&MtempA, &P_KF, &psy);			// MtempA = P * psy  
	mulmat_t1(&MtempB, &psy, &MtempA);     
	r_KF_inv = 1 / (R.mtx[1] + MtempB.mtx[1]); 
 
	//k = P * psy * r_KF_inv ;   %  [ (ma+mb+mc) x 1]  
	cmulmat(&k,r_KF_inv,&MtempA); 
 
	//% Update recursively: 
	// %%theta = theta + k * v ;       % [ (ma+mb+mc) x 1 ]  
	cmulmat(&MtempA, v_n, &k);	// delta_theta = k*v; 
	addmat(&theta, &theta, &MtempA); 	//theta = theta + delta_theta; 
 
	//%%%%%%%%%%%%%%% Insert *********************** 
//	cmulmat(&Q, (float)0.99, &Q);// Q=Q*0.99999; 
   
	//for( i=1;i < Q.col; i++) { 
	///		R.mtx[1][1] = R.mtx[1][1] * 0.99 + 0.0001; 
	//	if(Q.mtx[i][i] < (MtempA.mtx[i][1]*MtempA.mtx[i][1]*3))  
	//				Q.mtx[i][i] = (MtempA.mtx[i][1]*MtempA.mtx[i][1]*3);  
	//	if(Q.mtx[i][i] > 100) Q.mtx[i][i] = 100;  
	//} 
	//%%%%% end of insert **************************** 
   
	//% The riccaty recursion is: 
	// P = P + Q - P * psy * r_KF_inv * psy' * P 
	// P = P + Q - k * psy' * P 
	mulmat_t2(&MtempA, &k, &psy);	 // MtempA = k * psy'
	mulmat(&MtempB, &MtempA, &P_KF); // MtempB = psy * r_KF_inv * psy' * P 
	 
	submat(&MtempA,&Q, &MtempB);    //Q - P * psy * r_KF_inv * psy' * P 
	addmat(&P_KF, &P_KF, &MtempA);  // <= P_KF
 
	//% End of adaptation 
 
 
	 
	// 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: 
	{ 
	    //	printf("\n KF ---->"); 
	    //	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); 
 
		count_n = NxT; // Start the next statistical evaluation 
	 
	} 
 
 
 
} 
 
//----------------------------------------------------------------------------- 
 
KF_alg::~KF_alg()  // deInitialization 
{ 

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