/* * 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. */ /***************************************************************************/ /* */ /* KFA . cpp */ /* */ /* Recoursive Kalman Filter Algorithm inplementation. */ /* */ /* */ /***************************************************************************/ #include "KFA.hpp" //----------------------------------------------------------------------------- //Initialization: //----------------------------------------------------------------------------- /* function [theta,P_KF,R,Q] = KF_init(model,r,q) % %---------------------------------------------------------- % 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 % % % Initialisations of weight wector : theta = eye((model(1,1)+model(1,2)+model(1,3)+model(1,4)),1); theta = 0;% 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)); Adaptive Kalman filter part for R and Q: R = r ; initialized as r R = var(v); , where v= y - theta' * psy Q = q*I ; initialized as q diagonal matrix matrix; Q = var(theta); calculated on the privious estimation segment */ KFA_alg::KFA_alg(float r,float q, int is,int ia,int ib,int ic) : sysid(is,ia,ib,ic) { eye_matrix_alloc(&P_KF, m_s+m_a+m_b+m_c); // P_KF: cmulmat(&P_KF, (float)1e12, &P_KF); R = r; // R: eye_matrix_alloc(&Q, m_s+m_a+m_b+m_c); // Q: cmulmat(&Q, q, &Q); matrix_alloc(&k, m_s+m_a+m_b+m_c, 1); //temporary matrix matrix_alloc(&MtempA, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix matrix_alloc(&MtempB, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix } //----------------------------------------------------------------------------- // 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 % % 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 = 0 ;% 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, see Ljung pp368.: R= var(v); Q = var(theta); %%%%%%%% End of insert % Calculate the uodate gain: r_KF_inv = (R + psy' * P * psy)^(-1); % [ 1 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; % 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 KFA_alg::update(float y_n, float u_n_1) { // y_n the systems output at time n*T (present time) float v2; float r_KF_inv; int i; //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][1]; // Residual in system identification //v2 = v_n *v_n; // Calculate the uodate step: // % 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 + MtempB.mtx[1][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; //% The riccaty recursion is: // P = P - P * psy * r_KF_inv * psy' * P + Q // P = P - k * psy' * P + Q mulmat_t2(&MtempA, &k, &psy); // MtempA = k * psy' mulmat(&MtempB, &MtempA, &P_KF); // MtempB = k * psy' * P submat(&MtempA,&P_KF, &MtempB); // MtempA = P - k * psy' * P addmat(&P_KF, &Q, &MtempA); // //% 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: { R = v_qume; // residual variance mulmat_t2(&Q, &theta_vari,&theta_vari); // estimated paramethers covariance // 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 } } //----------------------------------------------------------------------------- KFA_alg::~KFA_alg() // deInitialization { // Remouve the alocation from the heep matrix_delete(&P_KF); matrix_delete(&Q); matrix_delete(&k); matrix_delete(&MtempA); matrix_delete(&MtempB); } //-----------------------------------------------------------------------------