From 1245d603265ca5b182a29ddb651224b3a8377457 Mon Sep 17 00:00:00 2001 From: george Date: Wed, 10 Jun 2009 06:40:07 +0000 Subject: [PATCH] 1st cut of KF refactoring and simplification git-svn-id: https://www.dynare.org/svn/dynare/trunk@2750 ac1d8469-bf42-47a9-8791-bf33cf982152 --- mex/sources/kalman/cc/kalman.cpp | 3165 +++++++++-------- mex/sources/kalman/cc/kalman.h | 44 + mex/sources/kalman/matlab/Makefile | 30 +- mex/sources/kalman/matlab/kalman_filters.cpp | 64 +- .../kalman/matlab/kalman_filters_testx.cpp | 90 +- 5 files changed, 1836 insertions(+), 1557 deletions(-) diff --git a/mex/sources/kalman/cc/kalman.cpp b/mex/sources/kalman/cc/kalman.cpp index 1efd73e4a..e59c7e299 100644 --- a/mex/sources/kalman/cc/kalman.cpp +++ b/mex/sources/kalman/cc/kalman.cpp @@ -55,14 +55,14 @@ FilterResults::~FilterResults() } } -/***************** -We delete what is allocated and is in the way of the new data. Then -set the new data as copies. + /***************** + We delete what is allocated and is in the way of the new data. Then + set the new data as copies. *****************/ void FilterResults::set(int t,const PLUFact&FFinv,const Vector&vv, - const GeneralMatrix&LL,const Vector&aa, - const GeneralMatrix&PP,double ll) + const GeneralMatrix&LL,const Vector&aa, + const GeneralMatrix&PP,double ll) { TS_RAISE_IF(t<1||t> (int)Finv.size()+1, "Wrong time for FilterResults::set"); @@ -125,11 +125,11 @@ const GeneralMatrix&FilterResults::getP(int t)const return*(P[t-1]); } -/***************** -This adds all the log likelihoods for all periods. If some periods -in the results have not been set, these are initialized to zeros and -thus this method is pretty safe but may not be if the likelihood tends to -be far lower or higher than 0. + /***************** + This adds all the log likelihoods for all periods. If some periods + in the results have not been set, these are initialized to zeros and + thus this method is pretty safe but may not be if the likelihood tends to + be far lower or higher than 0. *****************/ double @@ -201,10 +201,10 @@ DiffuseFilterResults::isPinfZero(int t)const return Pinf_zero[t-1]; } -/***************** -Here we raise an error on attempt to retrieve inverse of $F_\infty$ -for a period in which it was not regular. Caller has to call -|isFinfRegular| first. + /***************** + Here we raise an error on attempt to retrieve inverse of $F_\infty$ + for a period in which it was not regular. Caller has to call + |isFinfRegular| first. *****************/ const PLUFact&DiffuseFilterResults::getFinfInverse(int t)const @@ -216,9 +216,9 @@ const PLUFact&DiffuseFilterResults::getFinfInverse(int t)const return getFInverse(t); } -/***************** -Here we issue an error on attempt to retrieve inverse of $F_*$ for a -period when $F_\infty$ was regular. + /***************** + Here we issue an error on attempt to retrieve inverse of $F_*$ for a + period when $F_\infty$ was regular. *****************/ const PLUFact&DiffuseFilterResults::getFstarInverse(int t)const @@ -230,9 +230,9 @@ const PLUFact&DiffuseFilterResults::getFstarInverse(int t)const return getFInverse(t); } -/***************** -This should be called only when $F_\infty$ was regular, we raise an -error otherwise. + /***************** + This should be called only when $F_\infty$ was regular, we raise an + error otherwise. *****************/ const GeneralMatrix&DiffuseFilterResults::getF2(int t)const @@ -244,9 +244,9 @@ const GeneralMatrix&DiffuseFilterResults::getF2(int t)const return*(F_2[t-1]); } -/***************** -This should be called only when $F_\infty$ was regular, we raise an -error otherwise. + /***************** + This should be called only when $F_\infty$ was regular, we raise an + error otherwise. *****************/ const GeneralMatrix&DiffuseFilterResults::getL1(int t)const @@ -258,9 +258,9 @@ const GeneralMatrix&DiffuseFilterResults::getL1(int t)const return*(L_1[t-1]); } -/***************** -The $P_\infty$ should be retrieved only if it is not zero, (these -are all diffuse periods) + /***************** + The $P_\infty$ should be retrieved only if it is not zero, (these + are all diffuse periods) *****************/ const GeneralMatrix&DiffuseFilterResults::getPinf(int t)const @@ -272,14 +272,14 @@ const GeneralMatrix&DiffuseFilterResults::getPinf(int t)const return*(L_1[t-1]); } -/***************** -This sets the diffuse results for diffuse periods when -$F_\infty$ is regular. Note that in this case the inverse -$F_\infty^{-1}$ is stored as $F^{-1}$ of |FilterResults|, and thus is -returned by call |getFinfInverse|. Also, $L^{(0)}$ is stored as $L$ of -|FilterResults|, and retrieved by |getL0()| which is equivalent to -|FilterResults::getL()| (see |@<|DiffuseFilterResults| class -declaration@>|). + /***************** + This sets the diffuse results for diffuse periods when + $F_\infty$ is regular. Note that in this case the inverse + $F_\infty^{-1}$ is stored as $F^{-1}$ of |FilterResults|, and thus is + returned by call |getFinfInverse|. Also, $L^{(0)}$ is stored as $L$ of + |FilterResults|, and retrieved by |getL0()| which is equivalent to + |FilterResults::getL()| (see |@<|DiffuseFilterResults| class + declaration@>|). *****************/ void DiffuseFilterResults::set(int t,const PLUFact&FFinv,const GeneralMatrix&FF_2, @@ -305,10 +305,10 @@ void DiffuseFilterResults::set(int t,const PLUFact&FFinv,const GeneralMatrix&FF_ Pinf_zero[tm]= false; } -/***************** -This sets the diffuse results for diffuse period when the $F_\infty$ -is zero. We do not set $F^{(2)}$, and we set $F_*^{-1}$ instead of -$F_\infty^{-1}$. + /***************** + This sets the diffuse results for diffuse period when the $F_\infty$ + is zero. We do not set $F^{(2)}$, and we set $F_*^{-1}$ instead of + $F_\infty^{-1}$. *****************/ void DiffuseFilterResults::set(int t,const PLUFact&FFstarinv,const Vector&vv, @@ -327,9 +327,9 @@ void DiffuseFilterResults::set(int t,const PLUFact&FFstarinv,const Vector&vv, Pinf_zero[tm]= false; } -/***************** -This returns number of initial diffuse periods (those having -$P_\infty$ non-zero) + /***************** + This returns number of initial diffuse periods (those having + $P_\infty$ non-zero) *****************/ int DiffuseFilterResults::getDiffusePeriods()const @@ -377,12 +377,12 @@ void SmootherResults::set(int t,const Vector&aalpha,const Vector&eeta, V[tm]= new GeneralMatrix(VV); } -/***************** -This takes a |SmootherResults| coming from a univariate filter with -a given number of observations at one time. This number of -observations becomes a periodicity in the univariate -|SmootherResults|. If this perioidicty is 10, we take data for -$t=10,20,30,\ldots$.) + /***************** + This takes a |SmootherResults| coming from a univariate filter with + a given number of observations at one time. This number of + observations becomes a periodicity in the univariate + |SmootherResults|. If this perioidicty is 10, we take data for + $t=10,20,30,\ldots$.) *****************/ void SmootherResults::import(const SmootherResults&sres,int period) @@ -406,8 +406,8 @@ void SmootherResults::import(const SmootherResults&sres,int period) mint= 1; } -/***************** - This saves |alpha| to the given matrix. + /***************** + This saves |alpha| to the given matrix. *****************/ void @@ -426,8 +426,8 @@ SmootherResults::exportAlpha(GeneralMatrix&out)const } } -/***************** - This saves |eta| to the given matrix. + /***************** + This saves |eta| to the given matrix. *****************/ void @@ -446,10 +446,10 @@ SmootherResults::exportEta(GeneralMatrix&out)const } } -/***************** - This saves $V$ to the given two dimensional matrix. We store the $V$ -matrices one by one columnwise. The storage corresponds to Matlab -storage of three dimensional matrices. + /***************** + This saves $V$ to the given two dimensional matrix. We store the $V$ + matrices one by one columnwise. The storage corresponds to Matlab + storage of three dimensional matrices. *****************/ void SmootherResults::exportV(GeneralMatrix&out)const @@ -468,6 +468,52 @@ void SmootherResults::exportV(GeneralMatrix&out)const } } +BasicKalmanTask::BasicKalmanTask(const GeneralMatrix&d,const GeneralMatrix&ZZ, + const GeneralMatrix&HH,const GeneralMatrix&TT, + const GeneralMatrix&RR,const GeneralMatrix&QQ, + const StateInit&init_state) + : // ssf(Z,H,T,R,Q), +data(d), Zt(*(new ConstGeneralMatrix(ZZ))), +Ht(*(new ConstGeneralMatrix(HH))), +Tt(*(new ConstGeneralMatrix(TT))), +Rt(*(new ConstGeneralMatrix(RR))), +Qt(*(new ConstGeneralMatrix(QQ))), +init(init_state) + { + TS_RAISE_IF(d.numRows()!=Zt.numRows(), + "Data not compatible with BasicKalmanTask constructor"); + // TS_RAISE_IF(ssf.m!=init.getM(), + // "State initialization not compatible with SSF in KalmanTask constructor"); + } + +BasicKalmanTask::BasicKalmanTask(const GeneralMatrix&d,const ConstGeneralMatrix&ZZ, + const ConstGeneralMatrix&HH,const ConstGeneralMatrix&TT, + const ConstGeneralMatrix&RR,const ConstGeneralMatrix&QQ, + const StateInit&init_state) + : // ssf(Z,H,T,R,Q), +data(d), Zt(ZZ), Ht(HH), Tt(TT), Rt(RR), Qt(QQ),init(init_state) + { + TS_RAISE_IF(d.numRows()!=Zt.numRows(), + "Data not compatible with BasicKalmanTask constructor"); + // TS_RAISE_IF(ssf.m!=init.getM(), + // "State initialization not compatible with SSF in KalmanTask constructor"); + } + + +BasicKalmanTask::~BasicKalmanTask() + { + if (&Zt); + delete &Zt; + if (&Ht); + delete &Ht; + if (&Tt); + delete &Tt; + if (&Rt); + delete &Rt; + if (&Qt); + delete &Qt; + } + KalmanTask::KalmanTask(const GeneralMatrix&d,const GeneralMatrix&Z, const GeneralMatrix&H,const GeneralMatrix&T, const GeneralMatrix&R,const GeneralMatrix&Q, @@ -496,7 +542,7 @@ KalmanTask::KalmanTask(const GeneralMatrix&d,const TMatrix&Z, "State initialization not compatible with SSF in KalmanTask constructor"); } -/***************** + /***************** This is a public interface to mechanism that filters data and returns the data loglikelihood. In addition, it returns the number of periods |per| successfully filtered. If the @@ -528,7 +574,7 @@ KalmanTask::filter(int&per,int&d)const if(fres.hasFinished()) return fres.getLogLikelihood(); } - return 0.0; + return 0.0; } double @@ -553,10 +599,17 @@ KalmanTask::filter(int&per,int&d, int start, std::vector* vll)const if(fres.hasFinished()) return fres.getLogLikelihood(start, vll); } - return 0.0; + return 0.0; } -/***************** +double +BasicKalmanTask::filter(int&per,int&d, int start, std::vector* vll)const + { + d= 0; + per= vll->size() ; + return filterNonDiffuse(init.getA(),init.getPstar(), start, vll); + } + /***************** This is public interface that runs a filter followed by a smoother. In addition to things returned by |KalmanTask::filter|, it fills also |SmootherResults|, which must be initialized to the number of columns @@ -564,7 +617,7 @@ KalmanTask::filter(int&per,int&d, int start, std::vector* vll)const *****************/ double KalmanTask::filter_and_smooth(SmootherResults&sres, - int&per,int&d)const + int&per,int&d)const { if(!init.isDiffuse()) { @@ -591,53 +644,59 @@ KalmanTask::filter_and_smooth(SmootherResults&sres, return fres.getLogLikelihood(); } } - return 0.0; + return 0.0; } -/***************** -This runs a non-diffuse filter with the given $t$, $a_t$ and -$P_t$. It fills the |FilterResults|. -First we check that the passed $P_t$ is positive definite by checking -that it has not negative diagonal and is symmetric diagonally -dominant. This is not equivalent to positive definitness but it excludes -``much'' of indefinite matrices. This check is important since this -routine is called from a diffuse filter and it is possible due to a -wrong guess/calculation of a number of diffuse periods that numerical -instability and roundoff errors make the matrix $P_*$ broken. - -Then we cycle until the end of period and perform a classical Kalman -filter operations. + /***************** + This runs a Basic non-diffuse filter with the given $t$, $a_t$ and + $P_t$. It fills the |FilterResults|. + + First we check that the passed $P_t$ is positive definite by checking + that it has not negative diagonal and is symmetric diagonally + dominant. This is not equivalent to positive definitness but it excludes + ``much'' of indefinite matrices. This check is important since this + routine is called from a diffuse filter and it is possible due to a + wrong guess/calculation of a number of diffuse periods that numerical + instability and roundoff errors make the matrix $P_*$ broken. + + Then we cycle until the end of period and perform a classical Kalman + filter operations. *****************/ - -void -KalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P, - int first,FilterResults&fres)const +double +BasicKalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P, + int start, std::vector* vll)const { + double loglik=0; Vector at(a); GeneralMatrix Pt(P); if(TSUtils::hasNegativeDiagonal(Pt)||!TSUtils::isSymDiagDominant(Pt)) - return; - for(int t= first;t<=data.numCols();t++) + return 0.0; + /* + ConstGeneralMatrix Zt(Z); + ConstGeneralMatrix Ht(H); + ConstGeneralMatrix Tt(T); + ConstGeneralMatrix Qt(Q); + ConstGeneralMatrix Rt(R); + */ + bool isTunit=0;// Tt->isUnit(); + bool isQzero= Qt.isZero(); + bool isRzero= Rt.isZero(); + + int t= 1; + int nonsteady=1; + for(;t<=data.numCols()&&nonsteady;++t) { ConstVector yt(data,t-1); - ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); - ConstGeneralMatrix Ht(((const TMatrix&)*ssf.H)[t]); - ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); - ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); - ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); - bool isTunit= ssf.T->isUnit(t); - bool isQzero= ssf.Q->isZero(t); - bool isRzero= ssf.R->isZero(t); /***************** - This calculates $$v_t = y_t - Z_t*a_t.$$ + This calculates $$v_t = y_t - Z_t*a_t.$$ *****************/ Vector vt(yt); Zt.multsVec(vt,at); /***************** - This calculates $$F_t = Z_tP_tZ_t^T+H_t.$$ + This calculates $$F_t = Z_tP_tZ_t^T+H_t.$$ *****************/ GeneralMatrix Mt(Pt,Zt,"trans"); GeneralMatrix Ft(Ht); @@ -646,31 +705,33 @@ KalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P, PLUFact Ftinv(Ft); if(!Ftinv.isRegular()) - return; + return 0.0; /***************** - This calculates $$K_t = T_tP_tZ_t^TF_t^{-1}.$$ + This calculates $$K_t = T_tP_tZ_t^TF_t^{-1}.$$ *****************/ GeneralMatrix Kt(Tt,Mt); Ftinv.multInvRight(Kt); /***************** - This calculates $$L_t = T_t-K_tZ_t.$$ + This calculates $$L_t = T_t-K_tZ_t.$$ *****************/ GeneralMatrix Lt(Tt); Lt.multAndAdd(ConstGeneralMatrix(Kt),Zt,-1.0); /***************** - Here we calc likelihood and store results. + Here we calc likelihood and store results. *****************/ double ll= calcStepLogLik(Ftinv,vt); - fres.set(t,Ftinv,vt,Lt,at,Pt,ll); + // fres.set(t,Ftinv,vt,Lt,at,Pt,ll); + (*vll)[t-1]=ll; + if (t>start) loglik+=ll; if(too $, then we switch to |KalmanTask::filterNonDiffuse|. - -The switching has two reasons: -The first is that the non-diffuse filter is computationally more efficient -(since it avoids multiplications of zero matrices). The second reason -is much more important. As $P_\infty$ approaches to zero, then -$F_\infty=Z P_\infty Z^T$ approaches to zero and might contain severe -roundoffs. All the operations employing its inverse, $F_\infty^{-1}$, -will commit very bad roundoff errors, and the results will become -unusable. That is why it is important to not only switch to -non-diffuse filter, but also to switch at the right period. - -In theory, the period $d$ of switching is equal to a number of -(univariate) observations for which $F_\infty$ is regular. This is -because the regular $F_\infty=ZP_\infty Z^T$ conveys some information -to $P=P_*+\kappa P_\infty$. However, it is only a theoretical result; -in real floating point world it is difficult to recognize a regular -matrix in this process. Moreover, the $F_\infty$ might be singular and -still convey some information for the diffuse elements since it might -have non-zero rank. - -In this implementation, we use the above idea with the following test -for regularity of $F_\infty$. $F_\infty$ is considered to be regular, -if its PLU factorization yields a condition number estimate less than -$10^10$. During the process it might happen that $P_\infty$ is -indefinite. In this case we correct it by setting its negative -eigenvalues to zero. So $F_\infty=ZP_\infty Z$ is always positive -semidefinite, so no tests for a sign of its determinant are -necessary. Further, the test for $F_\infty=0$ here is equivalent to an -exact match. This can be done since the roundoff errors are believed -to be eliminated during correcting the $P_\infty$ matrix, where not -only negative eigenvalues but also very small positive eigenvalues are -corrected to zeros. In neither case, this is if $F_\infty$ is regular -and still is non-zero, we raise end the filter. This error can be -recognized by |FilterResults::per| less than a number of periods. - -This is just one of many ways, how to implement this non-continuous -algorithm. It is theoretically continuous (since the non-diffuse -periods having $P_\infty$ zero are covered by the branch where -$F_\infty=0$). However, it is computationally discontinuous, since the -calcs depend on when we switch to non-diffuse filter. Because of the -roundoff errors we are uncertain about the switch. An experience shows -that if we switch late, the results can be very bad due to roundoff -errors implied by late switch, if we switch too early, the results -might be wrong since we neglect some uncertainity. - -Main decision point is |ndiff|. Whenever |ndiff<=0|, we consider -$P_\infty$ as zero and carry on as in non-diffuse filter. -*****************/ - -void -KalmanTask::filterDiffuse(const Vector&a,const GeneralMatrix&Pstar, - const GeneralMatrix&Pinf,int first, - DiffuseFilterResults&fres)const - { - Vector at(a); - GeneralMatrix Ptstar(Pstar); - GeneralMatrix Ptinf(Pinf); - int ndiff= init.getNDiff(); - for(int t= first;t<=data.numCols();t++) + + + double + BasicKalmanTask::calcStepLogLik(const PLUFact&Finv,const Vector&v) { - + int p= Finv.numRows(); + Vector Finvv(v); + Finv.multInvLeft(Finvv); + double vFinvv= v.dot(Finvv); + return-0.5*(p*log(2*M_PI)+Finv.getLogDeterminant()+vFinvv); + } + + /***************** - If $P_\infty$ is exactly zero, then we run the non-diffuse - filter. The $P_\infty$ might become exactly zero by negative or zero - |ndiff|, or by $P\infty$ definitness correction. - *****************/ - if(TSUtils::isZero(Ptinf)) - { - filterNonDiffuse(at,Ptstar,t,fres); + This runs a non-diffuse filter with the given $t$, $a_t$ and + $P_t$. It fills the |FilterResults|. + + First we check that the passed $P_t$ is positive definite by checking + that it has not negative diagonal and is symmetric diagonally + dominant. This is not equivalent to positive definitness but it excludes + ``much'' of indefinite matrices. This check is important since this + routine is called from a diffuse filter and it is possible due to a + wrong guess/calculation of a number of diffuse periods that numerical + instability and roundoff errors make the matrix $P_*$ broken. + + Then we cycle until the end of period and perform a classical Kalman + filter operations. + *****************/ + void + KalmanTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P, + int first,FilterResults&fres)const + { + Vector at(a); + GeneralMatrix Pt(P); + if(TSUtils::hasNegativeDiagonal(Pt)||!TSUtils::isSymDiagDominant(Pt)) return; - } - ConstVector yt(data,t-1); - ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); - ConstGeneralMatrix Ht(((const TMatrix&)*ssf.H)[t]); - ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); - ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); - ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); - bool isTunit= ssf.T->isUnit(t); - bool isQzero= ssf.Q->isZero(t); - bool isRzero= ssf.R->isZero(t); - - /***************** - This calculates $$v_t = y_t - Z_t*a_t.$$ - *****************/ - Vector vt(yt); - Zt.multsVec(vt,at); - - - /***************** - This calculates $$M_{*,t} = P_{*,t}Z_t^T.$$ - *****************/ - GeneralMatrix Mtstar(Ptstar,Zt,"trans"); - - - - /***************** - This calculates $$F_{*,t} = Z_tP_{*,t}^T+H_t.$$ - *****************/ - GeneralMatrix Ftstar(Ht); - Ftstar.multAndAdd(Zt,ConstGeneralMatrix(Mtstar)); - - - - /***************** - This calculates $$M_{\infty,t} = P_{\infty,t}Z_t^T.$$ - *****************/ - GeneralMatrix Mtinf(Ptinf,Zt,"trans"); - - - - /***************** - This calculates $$F_{\infty,t} = Z_tP_{\infty,t}Z_t^T.$$ - *****************/ - GeneralMatrix Ftinf(Zt,ConstGeneralMatrix(Mtinf)); - - - PLUFact Ftinfinv(Ftinf); - if(Ftinfinv.isRegular()&&Ftinfinv.getRcond()> 1.e-10) + for(int t= first;t<=data.numCols();t++) { - ndiff-= ssf.p; - - /***************** - We calculate all other matrices, and if we have not come to the end, - also $a_{t+1}$, $P_{*,t+1}$ and $P_{\infty,t+1}$. If |ndiff<=0|, we - set $P_{\infty,t+1}=0$. The matrix can be set to zero even if it is - not positive semidefinite in the code correcting definitness of - $P_\infty$. - *****************/ - - /***************** - This calculates $$F_t^{(2)} = -F_{\infty,t}^{-1}F_{*,t}F_{\infty,t}^{-1}.$$ - *****************/ - GeneralMatrix Ft_2(Ftstar); - Ftinfinv.multInvRight(Ft_2); - Ftinfinv.multInvLeft(Ft_2); - Ft_2.mult(-1.0); - - /***************** - This calculates $$K_t^{(0)} = T_tM_{\infty,t}F_t^{(1)}.$$ - *****************/ - GeneralMatrix Kt_0(Tt,Mtinf); - Ftinfinv.multInvRight(Kt_0); - - /***************** - This calculates $$K_t^{(1)} = T_t(M_{\infty,t}F_t^{(2)}+M_{*,t}F_t^{(1)}).$$ - *****************/ - GeneralMatrix Kt_1(Mtstar); - Ftinfinv.multInvRight(Kt_1); - Kt_1.multAndAdd(Mtinf,Ft_2); - if(!isTunit) - Kt_1.multLeft(Tt); - - /***************** - This calculates $$L_t^{(0)} = T_t-K_t^{(0)}Z_t.$$ - *****************/ - GeneralMatrix Lt_0(Tt); - Lt_0.multAndAdd(ConstGeneralMatrix(Kt_0),Zt,-1.0); - - /***************** - This calculates $$L_t^{(1)} = -K_t^{(1)}Z_t.$$ - *****************/ - GeneralMatrix Lt_1(Kt_1,Zt); - Lt_1.mult(-1.0); - - /***************** - This calculates log likelihood and store results - *****************/ - double ll= -0.5*(ssf.p*log(2*M_PI)+Ftinfinv.getLogDeterminant()); - fres.set(t,Ftinfinv,Ft_2,vt,Lt_0,Lt_1,at,Pstar,Pinf,ll); - - if(tisZero(t); - bool isRzero= ssf.R->isZero(t); - - - /***************** - Calculate $$\eta_t = Q_tR_t^Tr_t.$$ - *****************/ - etat.zeros(); - if(!isQzero&&!isRzero){ - Rt.multVecTrans(0.0,etat,1.0,rt); - Vector etatsav((const Vector&)etat); - Qt.multVec(0.0,etat,1.0,etatsav); - } - - - - /***************** - This calculates $$r_{t-1} = Z^T_tF_t^{-1}v_t + L^T_tr_t.$$ - *****************/ - Vector rtsav((const Vector&)rt); - Lt.multVecTrans(0.0,rt,1.0,rtsav); - Vector Ftinvvt(vt); - Ftinv.multInvLeft(Ftinvvt); - Zt.multVecTrans(1.0,rt,1.0,Ftinvvt); - - - - /***************** - This calculates $$N_{t-1} = Z^T_tF_t^{-1}Z_t+L^T_tN_tL_t.$$ - *****************/ - GeneralMatrix NtLt(Nt,Lt); - Nt.zeros(); - Nt.multAndAdd(Lt,"trans",NtLt); - GeneralMatrix FtinvZt(Zt); - Ftinv.multInvLeft(FtinvZt); - Nt.multAndAdd(Zt,"trans",ConstGeneralMatrix(FtinvZt)); - - - - /***************** - This calculates $$\alpha_t = a_t + P_tr_{t-1}.$$ - *****************/ - alphat= (const Vector&)at; - Pt.multVec(1.0,alphat,1.0,rt); - - - /***************** - This calculates $$V_t = P_t - P_tN_{t-1}P_t.$$ - *****************/ - Vt= (const GeneralMatrix&)Pt; - GeneralMatrix NtPt(Nt,Pt); - Vt.multAndAdd(Pt,NtPt,-1.0); - - } - -/***************** - The non-diffuse smoother just performs a series of - |KalmanTask::smootherNonDiffuseStep|. -*****************/ -void KalmanTask::smootherNonDiffuse(const FilterResults&fres, - SmootherResults&sres)const - { - Vector rt(ssf.m); - rt.zeros(); - GeneralMatrix Nt(ssf.m,ssf.m); - Nt.zeros(); - for(int t= data.numCols();t>=1;t--) - { - Vector alphat(ssf.m); - GeneralMatrix Vt(ssf.m,ssf.m); - Vector etat(ssf.r); - smootherNonDiffuseStep(t,fres,rt,Nt,alphat,Vt,etat); - sres.set(t,alphat,etat,Vt); - } - } - -/***************** - Here we cycle from $t=T,\ldots, 1$. Whenever $P_\infty$ is zero, we - perform the non-diffuse step. Otherwise we permorn a common code to - diffuse smoothing and then fork according to regularity of $F_\infty$. -*****************/ -void KalmanTask::smootherDiffuse(const DiffuseFilterResults&fres, - SmootherResults&sres)const - { - Vector rt_0(ssf.m); - Vector rt_1(ssf.m); - GeneralMatrix Nt_0(ssf.m,ssf.m); - GeneralMatrix Nt_1(ssf.m,ssf.m); - GeneralMatrix Nt_2(ssf.m,ssf.m); - rt_0.zeros(); - rt_1.zeros(); - Nt_0.zeros(); - Nt_1.zeros(); - Nt_2.zeros(); - - for(int t= data.numCols();t>=1;t--) - { - Vector alphat(ssf.m); - GeneralMatrix Vt(ssf.m,ssf.m); - Vector etat(ssf.r); - if(fres.isPinfZero(t)) - { - smootherNonDiffuseStep(t,fres,rt_0,Nt_0,alphat,Vt,etat); - } - else - { - const Vector&vt= fres.getV(t); - const GeneralMatrix&Lt_0= fres.getL0(t); - const Vector&at= fres.getA(t); - const GeneralMatrix&Ptstar= fres.getPstar(t); - const GeneralMatrix&Ptinf= fres.getPinf(t); + ConstVector yt(data,t-1); ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); - ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); + ConstGeneralMatrix Ht(((const TMatrix&)*ssf.H)[t]); ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); + ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); bool isTunit= ssf.T->isUnit(t); bool isQzero= ssf.Q->isZero(t); bool isRzero= ssf.R->isZero(t); /***************** - Calculate $$\eta_t =Q_tR_tr_t^{(0)}.$$ - *****************/ - etat.zeros(); - if(!isQzero&&!isRzero) - { - Rt.multVecTrans(0.0,etat,1.0,rt_0); - Vector etatsav((const Vector&)etat); - Qt.multVec(0.0,etat,1.0,etatsav); - } + This calculates $$v_t = y_t - Z_t*a_t.$$ + *****************/ + Vector vt(yt); + Zt.multsVec(vt,at); - if(!fres.isFinfRegular(t)) + /***************** + This calculates $$F_t = Z_tP_tZ_t^T+H_t.$$ + *****************/ + GeneralMatrix Mt(Pt,Zt,"trans"); + GeneralMatrix Ft(Ht); + Ft.multAndAdd(Zt,ConstGeneralMatrix(Mt)); + + + PLUFact Ftinv(Ft); + if(!Ftinv.isRegular()) + return; + + /***************** + This calculates $$K_t = T_tP_tZ_t^TF_t^{-1}.$$ + *****************/ + GeneralMatrix Kt(Tt,Mt); + Ftinv.multInvRight(Kt); + + /***************** + This calculates $$L_t = T_t-K_tZ_t.$$ + *****************/ + GeneralMatrix Lt(Tt); + Lt.multAndAdd(ConstGeneralMatrix(Kt),Zt,-1.0); + + + /***************** + Here we calc likelihood and store results. + *****************/ + double ll= calcStepLogLik(Ftinv,vt); + fres.set(t,Ftinv,vt,Lt,at,Pt,ll); + + if(too $, then we switch to |KalmanTask::filterNonDiffuse|. + + The switching has two reasons: + The first is that the non-diffuse filter is computationally more efficient + (since it avoids multiplications of zero matrices). The second reason + is much more important. As $P_\infty$ approaches to zero, then + $F_\infty=Z P_\infty Z^T$ approaches to zero and might contain severe + roundoffs. All the operations employing its inverse, $F_\infty^{-1}$, + will commit very bad roundoff errors, and the results will become + unusable. That is why it is important to not only switch to + non-diffuse filter, but also to switch at the right period. + + In theory, the period $d$ of switching is equal to a number of + (univariate) observations for which $F_\infty$ is regular. This is + because the regular $F_\infty=ZP_\infty Z^T$ conveys some information + to $P=P_*+\kappa P_\infty$. However, it is only a theoretical result; + in real floating point world it is difficult to recognize a regular + matrix in this process. Moreover, the $F_\infty$ might be singular and + still convey some information for the diffuse elements since it might + have non-zero rank. + In this implementation, we use the above idea with the following test + for regularity of $F_\infty$. $F_\infty$ is considered to be regular, + if its PLU factorization yields a condition number estimate less than + $10^10$. During the process it might happen that $P_\infty$ is + indefinite. In this case we correct it by setting its negative + eigenvalues to zero. So $F_\infty=ZP_\infty Z$ is always positive + semidefinite, so no tests for a sign of its determinant are + necessary. Further, the test for $F_\infty=0$ here is equivalent to an + exact match. This can be done since the roundoff errors are believed + to be eliminated during correcting the $P_\infty$ matrix, where not + only negative eigenvalues but also very small positive eigenvalues are + corrected to zeros. In neither case, this is if $F_\infty$ is regular + and still is non-zero, we raise end the filter. This error can be + recognized by |FilterResults::per| less than a number of periods. + + This is just one of many ways, how to implement this non-continuous + algorithm. It is theoretically continuous (since the non-diffuse + periods having $P_\infty$ zero are covered by the branch where + $F_\infty=0$). However, it is computationally discontinuous, since the + calcs depend on when we switch to non-diffuse filter. Because of the + roundoff errors we are uncertain about the switch. An experience shows + that if we switch late, the results can be very bad due to roundoff + errors implied by late switch, if we switch too early, the results + might be wrong since we neglect some uncertainity. + + Main decision point is |ndiff|. Whenever |ndiff<=0|, we consider + $P_\infty$ as zero and carry on as in non-diffuse filter. + *****************/ + + void + KalmanTask::filterDiffuse(const Vector&a,const GeneralMatrix&Pstar, + const GeneralMatrix&Pinf,int first, + DiffuseFilterResults&fres)const + { + Vector at(a); + GeneralMatrix Ptstar(Pstar); + GeneralMatrix Ptinf(Pinf); + int ndiff= init.getNDiff(); + for(int t= first;t<=data.numCols();t++) + { + + /***************** + If $P_\infty$ is exactly zero, then we run the non-diffuse + filter. The $P_\infty$ might become exactly zero by negative or zero + |ndiff|, or by $P\infty$ definitness correction. + *****************/ + if(TSUtils::isZero(Ptinf)) + { + filterNonDiffuse(at,Ptstar,t,fres); + return; + } + + ConstVector yt(data,t-1); + ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); + ConstGeneralMatrix Ht(((const TMatrix&)*ssf.H)[t]); + ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); + ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); + ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); + bool isTunit= ssf.T->isUnit(t); + bool isQzero= ssf.Q->isZero(t); + bool isRzero= ssf.R->isZero(t); + + /***************** + This calculates $$v_t = y_t - Z_t*a_t.$$ + *****************/ + Vector vt(yt); + Zt.multsVec(vt,at); + + + /***************** + This calculates $$M_{*,t} = P_{*,t}Z_t^T.$$ + *****************/ + GeneralMatrix Mtstar(Ptstar,Zt,"trans"); + + + + /***************** + This calculates $$F_{*,t} = Z_tP_{*,t}^T+H_t.$$ + *****************/ + GeneralMatrix Ftstar(Ht); + Ftstar.multAndAdd(Zt,ConstGeneralMatrix(Mtstar)); + + + + /***************** + This calculates $$M_{\infty,t} = P_{\infty,t}Z_t^T.$$ + *****************/ + GeneralMatrix Mtinf(Ptinf,Zt,"trans"); + + + + /***************** + This calculates $$F_{\infty,t} = Z_tP_{\infty,t}Z_t^T.$$ + *****************/ + GeneralMatrix Ftinf(Zt,ConstGeneralMatrix(Mtinf)); + + + PLUFact Ftinfinv(Ftinf); + if(Ftinfinv.isRegular()&&Ftinfinv.getRcond()> 1.e-10) + { + ndiff-= ssf.p; + + /***************** + We calculate all other matrices, and if we have not come to the end, + also $a_{t+1}$, $P_{*,t+1}$ and $P_{\infty,t+1}$. If |ndiff<=0|, we + set $P_{\infty,t+1}=0$. The matrix can be set to zero even if it is + not positive semidefinite in the code correcting definitness of + $P_\infty$. + *****************/ + + /***************** + This calculates $$F_t^{(2)} = -F_{\infty,t}^{-1}F_{*,t}F_{\infty,t}^{-1}.$$ + *****************/ + GeneralMatrix Ft_2(Ftstar); + Ftinfinv.multInvRight(Ft_2); + Ftinfinv.multInvLeft(Ft_2); + Ft_2.mult(-1.0); + + /***************** + This calculates $$K_t^{(0)} = T_tM_{\infty,t}F_t^{(1)}.$$ + *****************/ + GeneralMatrix Kt_0(Tt,Mtinf); + Ftinfinv.multInvRight(Kt_0); + + /***************** + This calculates $$K_t^{(1)} = T_t(M_{\infty,t}F_t^{(2)}+M_{*,t}F_t^{(1)}).$$ + *****************/ + GeneralMatrix Kt_1(Mtstar); + Ftinfinv.multInvRight(Kt_1); + Kt_1.multAndAdd(Mtinf,Ft_2); + if(!isTunit) + Kt_1.multLeft(Tt); + + /***************** + This calculates $$L_t^{(0)} = T_t-K_t^{(0)}Z_t.$$ + *****************/ + GeneralMatrix Lt_0(Tt); + Lt_0.multAndAdd(ConstGeneralMatrix(Kt_0),Zt,-1.0); + + /***************** + This calculates $$L_t^{(1)} = -K_t^{(1)}Z_t.$$ + *****************/ + GeneralMatrix Lt_1(Kt_1,Zt); + Lt_1.mult(-1.0); + + /***************** + This calculates log likelihood and store results + *****************/ + double ll= -0.5*(ssf.p*log(2*M_PI)+Ftinfinv.getLogDeterminant()); + fres.set(t,Ftinfinv,Ft_2,vt,Lt_0,Lt_1,at,Pstar,Pinf,ll); + + if(t|. - *****************/ - Nt_1.zeros(); - GeneralMatrix FtinfinvZt(Zt); - Ftinfinv.multInvLeft(FtinfinvZt); - Nt_1.multAndAdd(Zt,"trans",ConstGeneralMatrix(FtinfinvZt)); - Nt_1.multAndAdd(Lt_0,"trans",Nt_1Lt_0); - GeneralMatrix Nt_0Lt_0(Nt_0,Lt_0); - Nt_1.multAndAdd(Lt_1,"trans",Nt_0Lt_0); - - /***************** - This calculates $$N_{t-1}^{(0)} = L_t^{(0)T}N_t^{(0)}L_t^{(0)}.$$ - |Nt_0Lt_0| was set in |@|. - *****************/ - Nt_0.zeros(); - Nt_0.multAndAdd(Lt_0,"trans",Nt_0Lt_0); - - /***************** - This calculates $$\alpha_t = a_t^{(0)} + P_{*,t}r_{t-1}^{(0)} + P_{\infty,t}r_{t-1}^{(1)}.$$ - for diffuse smoother and regular $F_{\infty,t} - *****************/ - alphat= (const Vector&)at; - Ptstar.multVec(1.0,alphat,1.0,rt_0); - Ptinf.multVec(1.0,alphat,1.0,rt_1); + return; } - + } + } + + + /***************** + This executes only one step of smoother non-diffuse step. It takes + $r_t$, and $N_t$ and outputs $\alpha_t$, $V_t$ and $\eta_t$. The code is clear. + *****************/ + void + KalmanTask::smootherNonDiffuseStep(int t,const FilterResults&fres, + Vector&rt,GeneralMatrix&Nt, + Vector&alphat,GeneralMatrix&Vt, + Vector&etat)const + { + const PLUFact&Ftinv= fres.getFInverse(t); + const Vector&vt= fres.getV(t); + const GeneralMatrix&Lt= fres.getL(t); + const Vector&at= fres.getA(t); + const GeneralMatrix&Pt= fres.getP(t); + ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); + ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); + ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); + bool isQzero= ssf.Q->isZero(t); + bool isRzero= ssf.R->isZero(t); + + + /***************** + Calculate $$\eta_t = Q_tR_t^Tr_t.$$ + *****************/ + etat.zeros(); + if(!isQzero&&!isRzero){ + Rt.multVecTrans(0.0,etat,1.0,rt); + Vector etatsav((const Vector&)etat); + Qt.multVec(0.0,etat,1.0,etatsav); + } + + + /***************** - This calculates $$V_t = P_{*,t} - P_{*,t}N_{t-1}^{(0)}P_{*,t} - - P_{\infty,t}N_{t-1}^{(1)}P_{*,t} -(P_{\infty,t}N_{t-1}^{(1)}P_{*,t})^T - - P_{\infty,t}N_{t-1}^{(2)}P_{\infty,t}.$$ - *****************/ - Vt= (const GeneralMatrix&)Ptstar; - GeneralMatrix Nt_0Ptstar(Nt_0,Ptstar); - Vt.multAndAdd(Ptstar,Nt_0Ptstar,-1.0); - GeneralMatrix Nt_2Ptinf(Nt_2,Ptinf); - Vt.multAndAdd(Ptinf,Nt_2Ptinf,-1.0); - GeneralMatrix Nt_1Ptstar(Nt_1,Ptstar); - GeneralMatrix PtinfNt_1Ptstar(Ptinf,Nt_1Ptstar); - Vt.add(-1.0,PtinfNt_1Ptstar); - Vt.add(-1.0,PtinfNt_1Ptstar,"trans"); + This calculates $$r_{t-1} = Z^T_tF_t^{-1}v_t + L^T_tr_t.$$ + *****************/ + Vector rtsav((const Vector&)rt); + Lt.multVecTrans(0.0,rt,1.0,rtsav); + Vector Ftinvvt(vt); + Ftinv.multInvLeft(Ftinvvt); + Zt.multVecTrans(1.0,rt,1.0,Ftinvvt); + + + + /***************** + This calculates $$N_{t-1} = Z^T_tF_t^{-1}Z_t+L^T_tN_tL_t.$$ + *****************/ + GeneralMatrix NtLt(Nt,Lt); + Nt.zeros(); + Nt.multAndAdd(Lt,"trans",NtLt); + GeneralMatrix FtinvZt(Zt); + Ftinv.multInvLeft(FtinvZt); + Nt.multAndAdd(Zt,"trans",ConstGeneralMatrix(FtinvZt)); + + + + /***************** + This calculates $$\alpha_t = a_t + P_tr_{t-1}.$$ + *****************/ + alphat= (const Vector&)at; + Pt.multVec(1.0,alphat,1.0,rt); + + + /***************** + This calculates $$V_t = P_t - P_tN_{t-1}P_t.$$ + *****************/ + Vt= (const GeneralMatrix&)Pt; + GeneralMatrix NtPt(Nt,Pt); + Vt.multAndAdd(Pt,NtPt,-1.0); + + } + + /***************** + The non-diffuse smoother just performs a series of + |KalmanTask::smootherNonDiffuseStep|. + *****************/ + void KalmanTask::smootherNonDiffuse(const FilterResults&fres, + SmootherResults&sres)const + { + Vector rt(ssf.m); + rt.zeros(); + GeneralMatrix Nt(ssf.m,ssf.m); + Nt.zeros(); + for(int t= data.numCols();t>=1;t--) + { + Vector alphat(ssf.m); + GeneralMatrix Vt(ssf.m,ssf.m); + Vector etat(ssf.r); + smootherNonDiffuseStep(t,fres,rt,Nt,alphat,Vt,etat); + sres.set(t,alphat,etat,Vt); + } + } + + /***************** + Here we cycle from $t=T,\ldots, 1$. Whenever $P_\infty$ is zero, we + perform the non-diffuse step. Otherwise we permorn a common code to + diffuse smoothing and then fork according to regularity of $F_\infty$. + *****************/ + void KalmanTask::smootherDiffuse(const DiffuseFilterResults&fres, + SmootherResults&sres)const + { + Vector rt_0(ssf.m); + Vector rt_1(ssf.m); + GeneralMatrix Nt_0(ssf.m,ssf.m); + GeneralMatrix Nt_1(ssf.m,ssf.m); + GeneralMatrix Nt_2(ssf.m,ssf.m); + rt_0.zeros(); + rt_1.zeros(); + Nt_0.zeros(); + Nt_1.zeros(); + Nt_2.zeros(); + + for(int t= data.numCols();t>=1;t--) + { + Vector alphat(ssf.m); + GeneralMatrix Vt(ssf.m,ssf.m); + Vector etat(ssf.r); + if(fres.isPinfZero(t)) + { + smootherNonDiffuseStep(t,fres,rt_0,Nt_0,alphat,Vt,etat); + } + else + { + const Vector&vt= fres.getV(t); + const GeneralMatrix&Lt_0= fres.getL0(t); + const Vector&at= fres.getA(t); + const GeneralMatrix&Ptstar= fres.getPstar(t); + const GeneralMatrix&Ptinf= fres.getPinf(t); + ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); + ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); + ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); + ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); + bool isTunit= ssf.T->isUnit(t); + bool isQzero= ssf.Q->isZero(t); + bool isRzero= ssf.R->isZero(t); + + /***************** + Calculate $$\eta_t =Q_tR_tr_t^{(0)}.$$ + *****************/ + etat.zeros(); + if(!isQzero&&!isRzero) + { + Rt.multVecTrans(0.0,etat,1.0,rt_0); + Vector etatsav((const Vector&)etat); + Qt.multVec(0.0,etat,1.0,etatsav); + } + + if(!fres.isFinfRegular(t)) + { + /***************** + We call here |smootherNonDiffuseStep| and calculate $r_{t-1}^{(1)}$, + $N_{t-1}^{(1)}$, $N_{t-1}^{(2)}$ and correct for $\alpha_t$. + *****************/ + smootherNonDiffuseStep(t,fres,rt_0,Nt_0,alphat,Vt,etat); + + /***************** + This calculates $$r_{t-1}^{(1)} = T^T_tr_t^{(1)}.$$ + *****************/ + if(!isTunit) + { + Vector rt_1sav((const Vector&)rt_1); + rt_1.zeros(); + Tt.multVecTrans(0.0,rt_1,1.0,rt_1sav); + } + + /***************** + This corrects $\alpha_t$ after|KalmanTask::smootherNonDiffuseStep|. + This adds $P_{\infty,t}r_{t-1}^{(1)}$ to $\alpha_t$. + *****************/ + Ptinf.multVec(1.0,alphat,1.0,rt_1); + + /***************** + This calculates $$N_{t-1}^{(1)} = T_t^TN_t^{(1)}L_t^{(0)}.$$ + *****************/ + if(!isTunit) + { + GeneralMatrix Nt_1Lt_0(Nt_1,Lt_0); + Nt_1.zeros(); + Nt_1.multAndAdd(Tt,"trans",ConstGeneralMatrix(Nt_1Lt_0)); + } + else + Nt_1.mult(Nt_1,Lt_0); + + /***************** + This calculates $$N_{t-1}^{(2)} = T_t^TN_t^{(2)}T_t.$$ + *****************/ + if(!isTunit) + { + GeneralMatrix Nt_2Tt(Nt_2,Tt); + Nt_2.zeros(); + Nt_2.multAndAdd(Tt,"trans",ConstGeneralMatrix(Nt_2Tt)); + } + + } + else + { + const GeneralMatrix&Lt_1= fres.getL1(t); + const GeneralMatrix&Ft_2= fres.getF2(t); + const PLUFact&Ftinfinv= fres.getFinfInverse(t); + + /***************** + This calculates $$r_{t-1}^{(1)} = Z^T_tF_{\infty,t}^{-1}v_t^{(0)} + + L_t^{(0)T}r_t^{(1)} + L_t^{(1)T}r_t^{(0)}.$$ + *****************/ + Vector rt_1sav((const Vector&)rt_1); + Lt_0.multVecTrans(0.0,rt_1,1.0,rt_1sav); + Lt_1.multVecTrans(1.0,rt_1,1.0,rt_0); + Vector Ftinfinvvt(vt); + Ftinfinv.multInvLeft(Ftinfinvvt); + Zt.multVecTrans(1.0,rt_1,1.0,Ftinfinvvt); + + /***************** + This calculates $$r_{t-1}^{(0)} = L_t^{(0)}r_t^{(0)}.$$ + *****************/ + Vector rt_0sav((const Vector&)rt_0); + Lt_0.multVecTrans(0.0,rt_0,1.0,rt_0sav); + + /***************** + This calculates + $$N_{t-1}^{(2)} = Z_t^TF_t^{(2)}Z_t + L_t^{(0)T}N_t^{(2)}L_t^{(0)}+ + L_t^{(0)T}N_t^{(1)}L_t^{(1)} + L_t^{(1)T}N_t^{(1)}L_t^{(0)} + + L_t^{(1)T}N_t^{(0)}L_t^{(1)}. + $$ + *****************/ + GeneralMatrix Nt_2sav((const GeneralMatrix&)Nt_2); + Nt_2.zeros(); + GeneralMatrix Ft_2Zt(Ft_2,Zt); + Nt_2.multAndAdd(Zt,"trans",ConstGeneralMatrix(Ft_2Zt)); + GeneralMatrix Nt_2Lt_0(Nt_2sav,Lt_0); + Nt_2.multAndAdd(Lt_0,"trans",Nt_2Lt_0); + GeneralMatrix Nt_1Lt_1(Nt_1,Lt_1); + Nt_2.multAndAdd(Lt_0,"trans",Nt_1Lt_1); + GeneralMatrix Nt_1Lt_0(Nt_1,Lt_0); + Nt_2.multAndAdd(Lt_1,"trans",Nt_1Lt_0); + GeneralMatrix Nt_0Lt_1(Nt_0,Lt_1); + Nt_2.multAndAdd(Lt_1,"trans",Nt_0Lt_1); + + /***************** + This calculates + $$N_{t-1}^{(1)} = Z_t^TF_t^{(1)}Z_t + L_t^{(0)T}N_t^{(1)}L_t^{(0)}+ + L_t^{(1)T}N_t^{(0)}L_t^{(0)}.$$ |Nt_1Lt_0| was set in |@|. + *****************/ + Nt_1.zeros(); + GeneralMatrix FtinfinvZt(Zt); + Ftinfinv.multInvLeft(FtinfinvZt); + Nt_1.multAndAdd(Zt,"trans",ConstGeneralMatrix(FtinfinvZt)); + Nt_1.multAndAdd(Lt_0,"trans",Nt_1Lt_0); + GeneralMatrix Nt_0Lt_0(Nt_0,Lt_0); + Nt_1.multAndAdd(Lt_1,"trans",Nt_0Lt_0); + + /***************** + This calculates $$N_{t-1}^{(0)} = L_t^{(0)T}N_t^{(0)}L_t^{(0)}.$$ + |Nt_0Lt_0| was set in |@|. + *****************/ + Nt_0.zeros(); + Nt_0.multAndAdd(Lt_0,"trans",Nt_0Lt_0); + + /***************** + This calculates $$\alpha_t = a_t^{(0)} + P_{*,t}r_{t-1}^{(0)} + P_{\infty,t}r_{t-1}^{(1)}.$$ + for diffuse smoother and regular $F_{\infty,t} + *****************/ + alphat= (const Vector&)at; + Ptstar.multVec(1.0,alphat,1.0,rt_0); + Ptinf.multVec(1.0,alphat,1.0,rt_1); + } + + /***************** + This calculates $$V_t = P_{*,t} - P_{*,t}N_{t-1}^{(0)}P_{*,t} + - P_{\infty,t}N_{t-1}^{(1)}P_{*,t} -(P_{\infty,t}N_{t-1}^{(1)}P_{*,t})^T + - P_{\infty,t}N_{t-1}^{(2)}P_{\infty,t}.$$ + *****************/ + Vt= (const GeneralMatrix&)Ptstar; + GeneralMatrix Nt_0Ptstar(Nt_0,Ptstar); + Vt.multAndAdd(Ptstar,Nt_0Ptstar,-1.0); + GeneralMatrix Nt_2Ptinf(Nt_2,Ptinf); + Vt.multAndAdd(Ptinf,Nt_2Ptinf,-1.0); + GeneralMatrix Nt_1Ptstar(Nt_1,Ptstar); + GeneralMatrix PtinfNt_1Ptstar(Ptinf,Nt_1Ptstar); + Vt.add(-1.0,PtinfNt_1Ptstar); + Vt.add(-1.0,PtinfNt_1Ptstar,"trans"); }// end if/else sres.set(t,alphat,etat,Vt); }// end for } - - -/***************** -This evaluates a step loglikelihood as + + + /***************** + This evaluates a step loglikelihood as \log p(y_t\vert Y_{t-1})=-{1\over 2}\left[p\log(2\pi)+\log\vert F_t\vert+ v_t^TF_t^{-1}v_t\right]$$ -This is a static method. -*****************/ -double -KalmanTask::calcStepLogLik(const PLUFact&Finv,const Vector&v) - { - int p= Finv.numRows(); - Vector Finvv(v); - Finv.multInvLeft(Finvv); - double vFinvv= v.dot(Finvv); - return-0.5*(p*log(2*M_PI)+Finv.getLogDeterminant()+vFinvv); - } - - -FilterUniResults::~FilterUniResults() - { - for(unsigned int i= 0;i (int)L.size()+1, - "Wrong time for FilterUniResults::set"); - int tm= t-1; - if(L[tm]) - delete L[tm]; - if(a[tm]) - delete a[tm]; - if(P[tm]) - delete P[tm]; - if(t> maxt) - maxt= t; - - F[tm]= FF; - v[tm]= vv; - L[tm]= new GeneralMatrix(LL); - a[tm]= new Vector(aa); - P[tm]= new GeneralMatrix(PP); - loglik[tm]= ll; - } - -; - -double FilterUniResults::getF(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getF"); - return F[t-1]; - } - -; - -double FilterUniResults::getV(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getV"); - return v[t-1]; - } - -; - -const GeneralMatrix&FilterUniResults::getL(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getL"); - return*(L[t-1]); - } - -; - -const Vector&FilterUniResults::getA(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getA"); - return*(a[t-1]); - } - -; - -const GeneralMatrix&FilterUniResults::getP(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getP"); - return*(P[t-1]); - } - -/***************** -This adds all the log likelihoods for all periods. If some periods -in the results have not been set, these are initialized to zeros and -thus this method is pretty safe but may not be if the likelihood tends to -be far lower or higher than 0. -*****************/ -double -FilterUniResults::getLogLikelihood()const - { - double res= 0.0; - for(unsigned int i= 0;i* vloglik)const - { - double res= 0.0; - for(unsigned int i= 0;i* vloglik)const - { - double res= 0.0; - for(unsigned int i= start;i maxt, - "Wrong time for DiffuseFilterUniResults::isFinfRegular"); - return Finf_reg[t-1]; - } - -; - -bool -DiffuseFilterUniResults::isPinfZero(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for DiffuseFilterUniResults::isPinfZero"); - return Pinf_zero[t-1]; - } - -; - -double -DiffuseFilterUniResults::getFinf(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for DiffuseFilterUniResults::getFinf"); - TS_RAISE_IF(!isFinfRegular(t), - "Finf not regular in the period in DiffuseFilterUniResults::getFinf"); - return getF(t); - } - -; - -double -DiffuseFilterUniResults::getFstar(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for DiffuseFilterUniResults::getFstar"); - TS_RAISE_IF(isFinfRegular(t), - "Finf not zero in the period in DiffuseFilterUniResults::getFstar"); - return getF(t); - } - - -; - -double -DiffuseFilterUniResults::getF2(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for DiffuseFilterUniResults::getF2"); - TS_RAISE_IF(!isFinfRegular(t), - "Finf not regular in the period in DiffuseFilterUniResults::getF2"); - return F_2[t-1]; - } - -; - -const -GeneralMatrix&DiffuseFilterUniResults::getL1(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getL1"); - TS_RAISE_IF(!isFinfRegular(t), - "Finf not regular in the period in DiffuseFilterUniResults::getL1"); - return*(L_1[t-1]); - } - -; - -const -GeneralMatrix&DiffuseFilterUniResults::getPinf(int t)const - { - TS_RAISE_IF(t<1||t> maxt, - "Wrong time for FilterUniResults::getPinf"); - TS_RAISE_IF(isPinfZero(t), - "Pinf is zero in the period in DiffuseFilterUniResults::getPinf"); - return*(Pinf[t-1]); - } - -; - -void -DiffuseFilterUniResults::set(int t,double FF,double FF_2, - double vv,const GeneralMatrix&LL_0, - const GeneralMatrix&LL_1,const Vector&aa, - const GeneralMatrix&PPstar,const GeneralMatrix&PPinf, - double ll) - { - FilterUniResults::set(t,FF,vv,LL_0,aa,PPstar,ll); - int tm= t-1; - if(L_1[tm]) - delete L_1[tm]; - if(Pinf[tm]) - delete Pinf[tm]; + ; - L_1[tm]= new GeneralMatrix(LL_1); - Pinf[tm]= new GeneralMatrix(PPinf); - F_2[tm]= FF_2; - Finf_reg[tm]= true; - Pinf_zero[tm]= false; - } - -; - -void -DiffuseFilterUniResults::set(int t,double FFstar,double vv, - const GeneralMatrix&LL_0,const Vector&aa, - const GeneralMatrix&PPstar,const GeneralMatrix&PPinf, - double ll) - { - FilterUniResults::set(t,FFstar,vv,LL_0,aa,PPstar,ll); + void + FilterUniResults::set(int t,double FF,double vv, + const GeneralMatrix&LL,const Vector&aa, + const GeneralMatrix&PP,double ll) + { + TS_RAISE_IF(t<1||t> (int)L.size()+1, + "Wrong time for FilterUniResults::set"); + + int tm= t-1; + if(L[tm]) + delete L[tm]; + if(a[tm]) + delete a[tm]; + if(P[tm]) + delete P[tm]; + + if(t> maxt) + maxt= t; + + F[tm]= FF; + v[tm]= vv; + L[tm]= new GeneralMatrix(LL); + a[tm]= new Vector(aa); + P[tm]= new GeneralMatrix(PP); + loglik[tm]= ll; + } - int tm= t-1; - if(Pinf[tm]) - delete Pinf[tm]; - Pinf[tm]= new GeneralMatrix(PPinf); + ; - Finf_reg[tm]= false; - Pinf_zero[tm]= false; - } - -; - -int -DiffuseFilterUniResults::getDiffusePeriods()const - { - int d= maxt; - while(d> 1&&isPinfZero(d)) - d--; - return d; - } - -/***************** -@ This converts a multivariate |KalmanTask| to univariate -|KalmanUniTask|. It unfolds time dimension so that at each time only -one univariate observation comes. The measurment equation is -transformed so that the measurment errors would not be correlated. -*****************/ -KalmanUniTask::KalmanUniTask(const KalmanTask&kt) - :me(kt.data,*(kt.ssf.Z),*(kt.ssf.H)), - ssf(TMatrixCycle(*(me.Z),"rows"),TScalarCycle(*(me.H)), + double FilterUniResults::getF(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getF"); + return F[t-1]; + } + + ; + + double FilterUniResults::getV(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getV"); + return v[t-1]; + } + + ; + + const GeneralMatrix&FilterUniResults::getL(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getL"); + return*(L[t-1]); + } + + ; + + const Vector&FilterUniResults::getA(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getA"); + return*(a[t-1]); + } + + ; + + const GeneralMatrix&FilterUniResults::getP(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getP"); + return*(P[t-1]); + } + + /***************** + This adds all the log likelihoods for all periods. If some periods + in the results have not been set, these are initialized to zeros and + thus this method is pretty safe but may not be if the likelihood tends to + be far lower or higher than 0. + *****************/ + double + FilterUniResults::getLogLikelihood()const + { + double res= 0.0; + for(unsigned int i= 0;i* vloglik)const + { + double res= 0.0; + for(unsigned int i= 0;i* vloglik)const + { + double res= 0.0; + for(unsigned int i= start;i maxt, + "Wrong time for DiffuseFilterUniResults::isFinfRegular"); + return Finf_reg[t-1]; + } + + ; + + bool + DiffuseFilterUniResults::isPinfZero(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for DiffuseFilterUniResults::isPinfZero"); + return Pinf_zero[t-1]; + } + + ; + + double + DiffuseFilterUniResults::getFinf(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for DiffuseFilterUniResults::getFinf"); + TS_RAISE_IF(!isFinfRegular(t), + "Finf not regular in the period in DiffuseFilterUniResults::getFinf"); + return getF(t); + } + + ; + + double + DiffuseFilterUniResults::getFstar(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for DiffuseFilterUniResults::getFstar"); + TS_RAISE_IF(isFinfRegular(t), + "Finf not zero in the period in DiffuseFilterUniResults::getFstar"); + return getF(t); + } + + + ; + + double + DiffuseFilterUniResults::getF2(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for DiffuseFilterUniResults::getF2"); + TS_RAISE_IF(!isFinfRegular(t), + "Finf not regular in the period in DiffuseFilterUniResults::getF2"); + return F_2[t-1]; + } + + ; + + const + GeneralMatrix&DiffuseFilterUniResults::getL1(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getL1"); + TS_RAISE_IF(!isFinfRegular(t), + "Finf not regular in the period in DiffuseFilterUniResults::getL1"); + return*(L_1[t-1]); + } + + ; + + const + GeneralMatrix&DiffuseFilterUniResults::getPinf(int t)const + { + TS_RAISE_IF(t<1||t> maxt, + "Wrong time for FilterUniResults::getPinf"); + TS_RAISE_IF(isPinfZero(t), + "Pinf is zero in the period in DiffuseFilterUniResults::getPinf"); + return*(Pinf[t-1]); + } + + ; + + void + DiffuseFilterUniResults::set(int t,double FF,double FF_2, + double vv,const GeneralMatrix&LL_0, + const GeneralMatrix&LL_1,const Vector&aa, + const GeneralMatrix&PPstar,const GeneralMatrix&PPinf, + double ll) + { + FilterUniResults::set(t,FF,vv,LL_0,aa,PPstar,ll); + + int tm= t-1; + if(L_1[tm]) + delete L_1[tm]; + if(Pinf[tm]) + delete Pinf[tm]; + + L_1[tm]= new GeneralMatrix(LL_1); + Pinf[tm]= new GeneralMatrix(PPinf); + F_2[tm]= FF_2; + Finf_reg[tm]= true; + Pinf_zero[tm]= false; + } + + ; + + void + DiffuseFilterUniResults::set(int t,double FFstar,double vv, + const GeneralMatrix&LL_0,const Vector&aa, + const GeneralMatrix&PPstar,const GeneralMatrix&PPinf, + double ll) + { + FilterUniResults::set(t,FFstar,vv,LL_0,aa,PPstar,ll); + + int tm= t-1; + if(Pinf[tm]) + delete Pinf[tm]; + Pinf[tm]= new GeneralMatrix(PPinf); + + Finf_reg[tm]= false; + Pinf_zero[tm]= false; + } + + ; + + int + DiffuseFilterUniResults::getDiffusePeriods()const + { + int d= maxt; + while(d> 1&&isPinfZero(d)) + d--; + return d; + } + + /***************** + @ This converts a multivariate |KalmanTask| to univariate + |KalmanUniTask|. It unfolds time dimension so that at each time only + one univariate observation comes. The measurment equation is + transformed so that the measurment errors would not be correlated. + *****************/ + KalmanUniTask::KalmanUniTask(const KalmanTask&kt) + :me(kt.data,*(kt.ssf.Z),*(kt.ssf.H)), + ssf(TMatrixCycle(*(me.Z),"rows"),TScalarCycle(*(me.H)), TMatrixPadUnit(*(kt.ssf.T),kt.data.numRows()), TMatrixPadZero(*(kt.ssf.R),kt.data.numRows()), TMatrixPadZero(*(kt.ssf.Q),kt.data.numRows())), data(me.y.base(),1,me.y.numRows()*me.y.numCols()), init(kt.init) - { - } - - -double -KalmanUniTask::filter(int&per,int&d)const - { - if(!init.isDiffuse()) { - FilterUniResults fres(data.numCols()); - filterNonDiffuse(init.getA(),init.getPstar(),1,fres); - d= 0; - per= fres.getMaxT(); - if(fres.hasFinished()) - return fres.getLogLikelihood(); } - else + + + double + KalmanUniTask::filter(int&per,int&d)const { - DiffuseFilterUniResults fres(data.numCols()); - filterDiffuse(init.getA(),init.getPstar(),init.getPinf(), - 1,fres); - d= fres.getDiffusePeriods(); - per= fres.getMaxT(); - if(fres.hasFinished()) - return fres.getLogLikelihood(); - } - return 0.0; - } - -double -KalmanUniTask::filter(int&per,int&d, int start, std::vector* vll)const - { - if(!init.isDiffuse()) - { - FilterUniResults fres(data.numCols()); - filterNonDiffuse(init.getA(),init.getPstar(),1,fres); - d= 0; - per= fres.getMaxT(); - if(fres.hasFinished()) - return fres.getLogLikelihood(start,vll); - } - else - { - DiffuseFilterUniResults fres(data.numCols()); - filterDiffuse(init.getA(),init.getPstar(),init.getPinf(), - 1,fres); - d= fres.getDiffusePeriods(); - per= fres.getMaxT(); - if(fres.hasFinished()) - return fres.getLogLikelihood(start,vll); - } - return 0.0; - } - -double -KalmanUniTask::filter_and_smooth(SmootherResults&sres, - int&per,int&d)const - { - if(!init.isDiffuse()) - { - FilterUniResults fres(data.numCols()); - filterNonDiffuse(init.getA(),init.getPstar(),1,fres); - d= 0; - per= fres.getMaxT(); - if(fres.hasFinished()) + if(!init.isDiffuse()) { - smootherNonDiffuse(fres,sres); - return fres.getLogLikelihood(); + FilterUniResults fres(data.numCols()); + filterNonDiffuse(init.getA(),init.getPstar(),1,fres); + d= 0; + per= fres.getMaxT(); + if(fres.hasFinished()) + return fres.getLogLikelihood(); } - } - else - { - DiffuseFilterUniResults fres(data.numCols()); - filterDiffuse(init.getA(),init.getPstar(),init.getPinf(), - 1,fres); - d= fres.getDiffusePeriods(); - per= fres.getMaxT(); - if(fres.hasFinished()) - { - smootherDiffuse(fres,sres); - return fres.getLogLikelihood(); - } - } - return 0.0; - } - -/***************** - This filters univariate data starting at given $t$, $a_t$ and -$P_t$. If at some period $F_t\leq 0$, than we end and the filter -results are not finished. -*****************/ -void -KalmanUniTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P, - int first,FilterUniResults&fres)const - { - Vector at(a); - GeneralMatrix Pt(P); - for(int t= first;t<=data.numCols();t++){ - double yt= data.get(0,t-1); - ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); - double Ht= ((const TScalar&)*ssf.H)[t]; - ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); - ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); - ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); - bool isTunit= ssf.T->isUnit(t); - bool isQzero= ssf.Q->isZero(t); - bool isRzero= ssf.R->isZero(t); - - - double vt= at.dot(Zt.getData()); - vt= yt-vt; - - Vector Mt(ssf.m); - Mt.zeros(); - Pt.multVec(0.0,Mt,1.0,Zt.getData()); - double Ft= Mt.dot(Zt.getData()); - Ft+= Ht; - - if(Ft<=0.0) - return; - - Vector Kt(ssf.m); - Kt.zeros(); - if(isTunit) - Kt.add(1.0/Ft,Mt); else - Tt.multVec(0.0,Kt,1.0/Ft,Mt); - - GeneralMatrix Lt(Tt); - Lt.multAndAdd(ConstGeneralMatrix(Kt.base(),ssf.m,1),Zt,-1.0); - - double ll= calcStepLogLik(Ft,vt); - fres.set(t,Ft,vt,Lt,at,Pt,ll); - - if(t* vll)const + { + if(!init.isDiffuse()) + { + FilterUniResults fres(data.numCols()); + filterNonDiffuse(init.getA(),init.getPstar(),1,fres); + d= 0; + per= fres.getMaxT(); + if(fres.hasFinished()) + return fres.getLogLikelihood(start,vll); + } + else + { + DiffuseFilterUniResults fres(data.numCols()); + filterDiffuse(init.getA(),init.getPstar(),init.getPinf(), + 1,fres); + d= fres.getDiffusePeriods(); + per= fres.getMaxT(); + if(fres.hasFinished()) + return fres.getLogLikelihood(start,vll); + } + return 0.0; + } + + double + KalmanUniTask::filter_and_smooth(SmootherResults&sres, + int&per,int&d)const + { + if(!init.isDiffuse()) + { + FilterUniResults fres(data.numCols()); + filterNonDiffuse(init.getA(),init.getPstar(),1,fres); + d= 0; + per= fres.getMaxT(); + if(fres.hasFinished()) { - Vector atsave((const Vector&)at); - Tt.multVec(0.0,at,1.0,atsave); - } - at.add(vt,Kt); - - - GeneralMatrix PtLttrans(Pt,Lt,"trans"); - if(!isTunit) - { - Pt.zeros(); - Pt.multAndAdd(Tt,ConstGeneralMatrix(PtLttrans)); - } - else - { - Pt= (const GeneralMatrix&)PtLttrans; - } - if(!isRzero&&!isQzero) - { - GeneralMatrix QtRttrans(Qt,Rt,"trans"); - Pt.multAndAdd(Rt,ConstGeneralMatrix(QtRttrans)); + smootherNonDiffuse(fres,sres); + return fres.getLogLikelihood(); } } + else + { + DiffuseFilterUniResults fres(data.numCols()); + filterDiffuse(init.getA(),init.getPstar(),init.getPinf(), + 1,fres); + d= fres.getDiffusePeriods(); + per= fres.getMaxT(); + if(fres.hasFinished()) + { + smootherDiffuse(fres,sres); + return fres.getLogLikelihood(); + } + } + return 0.0; } - } - -/***************** - This is a univariate version of |KalmanTask::filterDiffuse|. The -decision whether $F_\infty$ is regular or zero is simpler here, yet -the algorithm is not numerically stable. We recognize $d$ in the same -way as in |KalmanTask::filterDiffuse|. It may still happen that small -non-zero $F_t$ implies a wrong $P_{\infty,t+1}$, or zero $F_t$ which -should be positive causes $d$ to be missed and numerical error is -committed in $P_{\infty,t+1}$. - -So, as in |KalmanTask::filterDiffuse| we use |ndiff| and cancel it by -one for periods for which $F_t$ is non-zero. If $P_\infty$ is not -positive definite, we set it to zero. -*****************/ -void -KalmanUniTask::filterDiffuse(const Vector&a,const GeneralMatrix&Pstar, - const GeneralMatrix&Pinf,int first, - DiffuseFilterUniResults&fres)const - { - Vector at(a); - GeneralMatrix Ptstar(Pstar); - GeneralMatrix Ptinf(Pinf); - int ndiff= init.getNDiff(); - for(int t= first;t<=data.numCols();t++){ - + /***************** - This is the same code as |@isUnit(t); - bool isQzero= ssf.Q->isZero(t); - bool isRzero= ssf.R->isZero(t); - - double vt= at.dot(Zt.getData()); - vt= yt-vt; - - Vector Mtstar(ssf.m); - Mtstar.zeros(); - Ptstar.multVec(0.0,Mtstar,1.0,Zt.getData()); - - double Ftstar= Mtstar.dot(Zt.getData()); - Ftstar+= Ht; - - Vector Mtinf(ssf.m); - Mtinf.zeros(); - Ptinf.multVec(0.0,Mtinf,1.0,Zt.getData()); - - double Ftinf= Mtinf.dot(Zt.getData()); - if(Ftinf<2*DBL_EPSILON) - Ftinf= 0.0; - - if(Ftinf> 0.0) - { - ndiff--; - - double Ft_2= -Ftstar/Ftinf/Ftinf; - - Vector Kt_0(ssf.m); - Kt_0.zeros(); - if(!isTunit) - Tt.multVec(0.0,Kt_0,1.0/Ftinf,Mtinf); - else - Kt_0.add(1.0/Ftinf,Mtinf); - - Vector Kt_1tmp(ssf.m); - Kt_1tmp.zeros(); - Kt_1tmp.add(Ft_2,Mtinf); - Kt_1tmp.add(1.0/Ftinf,Mtstar); - Vector Kt_1(ssf.m); - if(!isTunit) - { - Kt_1.zeros(); - Tt.multVec(0.0,Kt_1,1.0,Kt_1tmp); - } - else - { - Kt_1= (const Vector&)Kt_1tmp; - } - - GeneralMatrix Lt_0(Tt); - Lt_0.multAndAdd(ConstGeneralMatrix(Kt_0.base(),ssf.m,1),Zt,-1.0); - - - GeneralMatrix Lt_1(ConstGeneralMatrix(Kt_1.base(),ssf.m,1),Zt); - Lt_1.mult(-1.0); - - - double ll= -0.5*(log(2*M_PI)+log(Ftinf)); - fres.set(t,Ftinf,Ft_2,vt,Lt_0,Lt_1,at,Pstar,Pinf,ll); - - if(tisZero(t); - bool isRzero= ssf.R->isZero(t); - - - - etat.zeros(); - if(!isQzero&&!isRzero) + This filters univariate data starting at given $t$, $a_t$ and + $P_t$. If at some period $F_t\leq 0$, than we end and the filter + results are not finished. + *****************/ + void + KalmanUniTask::filterNonDiffuse(const Vector&a,const GeneralMatrix&P, + int first,FilterUniResults&fres)const { - Rt.multVecTrans(0.0,etat,1.0,rt); - Vector etatsav((const Vector&)etat); - Qt.multVec(0.0,etat,1.0,etatsav); - } - - Vector rtsav((const Vector&)rt); - Lt.multVecTrans(0.0,rt,1.0,rtsav); - rt.add(vt/Ft,Zt.getData()); - - GeneralMatrix NtLt(Nt,Lt); - Nt.zeros(); - Nt.multAndAdd(Lt,"trans",NtLt); - Nt.multAndAdd(Zt,"trans",Zt,1.0/Ft); - - alphat= (const Vector&)at; - Pt.multVec(1.0,alphat,1.0,rt); - - Vt= (const GeneralMatrix&)Pt; - GeneralMatrix NtPt(Nt,Pt); - Vt.multAndAdd(Pt,NtPt,-1.0); - } - - -void -KalmanUniTask::smootherNonDiffuse(const FilterUniResults&fres, - SmootherResults&sres)const - { - Vector rt(ssf.m); - rt.zeros(); - GeneralMatrix Nt(ssf.m,ssf.m); - Nt.zeros(); - for(int t= data.numCols();t>=1;t--){ - Vector alphat(ssf.m); - GeneralMatrix Vt(ssf.m,ssf.m); - Vector etat(ssf.r); - smootherNonDiffuseStep(t,fres,rt,Nt,alphat,Vt,etat); - sres.set(t,alphat,etat,Vt); - } - } - -void -KalmanUniTask::smootherDiffuse(const DiffuseFilterUniResults&fres, - SmootherResults&sres)const - { - - Vector rt_0(ssf.m); - Vector rt_1(ssf.m); - GeneralMatrix Nt_0(ssf.m,ssf.m); - GeneralMatrix Nt_1(ssf.m,ssf.m); - GeneralMatrix Nt_2(ssf.m,ssf.m); - rt_0.zeros(); - rt_1.zeros(); - Nt_0.zeros(); - Nt_1.zeros(); - Nt_2.zeros(); - - - for(int t= data.numCols();t>=1;t--) - { - Vector alphat(ssf.m); - GeneralMatrix Vt(ssf.m,ssf.m); - Vector etat(ssf.r); - if(fres.isPinfZero(t)) - { - smootherNonDiffuseStep(t,fres,rt_0,Nt_0,alphat,Vt,etat); - } - else - { - double vt= fres.getV(t); - const GeneralMatrix&Lt_0= fres.getL0(t); - const Vector&at= fres.getA(t); - const GeneralMatrix&Ptstar= fres.getPstar(t); - const GeneralMatrix&Ptinf= fres.getPinf(t); + Vector at(a); + GeneralMatrix Pt(P); + for(int t= first;t<=data.numCols();t++){ + double yt= data.get(0,t-1); ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); - ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); + double Ht= ((const TScalar&)*ssf.H)[t]; ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); + ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); bool isTunit= ssf.T->isUnit(t); bool isQzero= ssf.Q->isZero(t); bool isRzero= ssf.R->isZero(t); - etat.zeros(); - if(!isQzero&&!isRzero) - { - Rt.multVecTrans(0.0,etat,1.0,rt_0); - Vector etatsav((const Vector&)etat); - Qt.multVec(0.0,etat,1.0,etatsav); - } - if(!fres.isFinfRegular(t)) + double vt= at.dot(Zt.getData()); + vt= yt-vt; + + Vector Mt(ssf.m); + Mt.zeros(); + Pt.multVec(0.0,Mt,1.0,Zt.getData()); + double Ft= Mt.dot(Zt.getData()); + Ft+= Ht; + + if(Ft<=0.0) + return; + + Vector Kt(ssf.m); + Kt.zeros(); + if(isTunit) + Kt.add(1.0/Ft,Mt); + else + Tt.multVec(0.0,Kt,1.0/Ft,Mt); + + GeneralMatrix Lt(Tt); + Lt.multAndAdd(ConstGeneralMatrix(Kt.base(),ssf.m,1),Zt,-1.0); + + double ll= calcStepLogLik(Ft,vt); + fres.set(t,Ft,vt,Lt,at,Pt,ll); + + if(tisUnit(t); + bool isQzero= ssf.Q->isZero(t); + bool isRzero= ssf.R->isZero(t); + + double vt= at.dot(Zt.getData()); + vt= yt-vt; + + Vector Mtstar(ssf.m); + Mtstar.zeros(); + Ptstar.multVec(0.0,Mtstar,1.0,Zt.getData()); + + double Ftstar= Mtstar.dot(Zt.getData()); + Ftstar+= Ht; + + Vector Mtinf(ssf.m); + Mtinf.zeros(); + Ptinf.multVec(0.0,Mtinf,1.0,Zt.getData()); + + double Ftinf= Mtinf.dot(Zt.getData()); + if(Ftinf<2*DBL_EPSILON) + Ftinf= 0.0; + + if(Ftinf> 0.0) + { + ndiff--; + + double Ft_2= -Ftstar/Ftinf/Ftinf; + + Vector Kt_0(ssf.m); + Kt_0.zeros(); + if(!isTunit) + Tt.multVec(0.0,Kt_0,1.0/Ftinf,Mtinf); + else + Kt_0.add(1.0/Ftinf,Mtinf); + + Vector Kt_1tmp(ssf.m); + Kt_1tmp.zeros(); + Kt_1tmp.add(Ft_2,Mtinf); + Kt_1tmp.add(1.0/Ftinf,Mtstar); + Vector Kt_1(ssf.m); + if(!isTunit) + { + Kt_1.zeros(); + Tt.multVec(0.0,Kt_1,1.0,Kt_1tmp); + } + else + { + Kt_1= (const Vector&)Kt_1tmp; + } + + GeneralMatrix Lt_0(Tt); + Lt_0.multAndAdd(ConstGeneralMatrix(Kt_0.base(),ssf.m,1),Zt,-1.0); + + + GeneralMatrix Lt_1(ConstGeneralMatrix(Kt_1.base(),ssf.m,1),Zt); + Lt_1.mult(-1.0); + + + double ll= -0.5*(log(2*M_PI)+log(Ftinf)); + fres.set(t,Ftinf,Ft_2,vt,Lt_0,Lt_1,at,Pstar,Pinf,ll); + + if(tisZero(t); + bool isRzero= ssf.R->isZero(t); + + + + etat.zeros(); + if(!isQzero&&!isRzero) + { + Rt.multVecTrans(0.0,etat,1.0,rt); + Vector etatsav((const Vector&)etat); + Qt.multVec(0.0,etat,1.0,etatsav); + } + + Vector rtsav((const Vector&)rt); + Lt.multVecTrans(0.0,rt,1.0,rtsav); + rt.add(vt/Ft,Zt.getData()); + + GeneralMatrix NtLt(Nt,Lt); + Nt.zeros(); + Nt.multAndAdd(Lt,"trans",NtLt); + Nt.multAndAdd(Zt,"trans",Zt,1.0/Ft); + + alphat= (const Vector&)at; + Pt.multVec(1.0,alphat,1.0,rt); + + Vt= (const GeneralMatrix&)Pt; + GeneralMatrix NtPt(Nt,Pt); + Vt.multAndAdd(Pt,NtPt,-1.0); + } + + + void + KalmanUniTask::smootherNonDiffuse(const FilterUniResults&fres, + SmootherResults&sres)const + { + Vector rt(ssf.m); + rt.zeros(); + GeneralMatrix Nt(ssf.m,ssf.m); + Nt.zeros(); + for(int t= data.numCols();t>=1;t--){ + Vector alphat(ssf.m); + GeneralMatrix Vt(ssf.m,ssf.m); + Vector etat(ssf.r); + smootherNonDiffuseStep(t,fres,rt,Nt,alphat,Vt,etat); + sres.set(t,alphat,etat,Vt); + } + } + + void + KalmanUniTask::smootherDiffuse(const DiffuseFilterUniResults&fres, + SmootherResults&sres)const + { + + Vector rt_0(ssf.m); + Vector rt_1(ssf.m); + GeneralMatrix Nt_0(ssf.m,ssf.m); + GeneralMatrix Nt_1(ssf.m,ssf.m); + GeneralMatrix Nt_2(ssf.m,ssf.m); + rt_0.zeros(); + rt_1.zeros(); + Nt_0.zeros(); + Nt_1.zeros(); + Nt_2.zeros(); + + + for(int t= data.numCols();t>=1;t--) + { + Vector alphat(ssf.m); + GeneralMatrix Vt(ssf.m,ssf.m); + Vector etat(ssf.r); + if(fres.isPinfZero(t)) + { + smootherNonDiffuseStep(t,fres,rt_0,Nt_0,alphat,Vt,etat); + } + else + { + double vt= fres.getV(t); + const GeneralMatrix&Lt_0= fres.getL0(t); + const Vector&at= fres.getA(t); + const GeneralMatrix&Ptstar= fres.getPstar(t); + const GeneralMatrix&Ptinf= fres.getPinf(t); + ConstGeneralMatrix Zt(((const TMatrix&)*ssf.Z)[t]); + ConstGeneralMatrix Qt(((const TMatrix&)*ssf.Q)[t]); + ConstGeneralMatrix Tt(((const TMatrix&)*ssf.T)[t]); + ConstGeneralMatrix Rt(((const TMatrix&)*ssf.R)[t]); + bool isTunit= ssf.T->isUnit(t); + bool isQzero= ssf.Q->isZero(t); + bool isRzero= ssf.R->isZero(t); + + etat.zeros(); + if(!isQzero&&!isRzero) + { + Rt.multVecTrans(0.0,etat,1.0,rt_0); + Vector etatsav((const Vector&)etat); + Qt.multVec(0.0,etat,1.0,etatsav); + } + + if(!fres.isFinfRegular(t)) + { + smootherNonDiffuseStep(t,fres,rt_0,Nt_0,alphat,Vt,etat); + if(!isTunit) + { + Vector rt_1sav((const Vector&)rt_1); + rt_1.zeros(); + Tt.multVecTrans(0.0,rt_1,1.0,rt_1sav); + } + + Ptinf.multVec(1.0,alphat,1.0,rt_1); + + if(!isTunit) + { + GeneralMatrix Nt_1Lt_0(Nt_1,Lt_0); + Nt_1.zeros(); + Nt_1.multAndAdd(Tt,"trans",ConstGeneralMatrix(Nt_1Lt_0)); + } + else Nt_1.mult(Nt_1,Lt_0); @@ -2123,73 +2310,73 @@ KalmanUniTask::smootherDiffuse(const DiffuseFilterUniResults&fres, Nt_2.multAndAdd(Tt,"trans",ConstGeneralMatrix(Nt_2Tt)); } - } - else - { - const GeneralMatrix&Lt_1= fres.getL1(t); - double Ft_2= fres.getF2(t); - double Ftinf= fres.getFinf(t); + } + else + { + const GeneralMatrix&Lt_1= fres.getL1(t); + double Ft_2= fres.getF2(t); + double Ftinf= fres.getFinf(t); + + + Vector rt_1sav((const Vector&)rt_1); + Lt_0.multVecTrans(0.0,rt_1,1.0,rt_1sav); + Lt_1.multVecTrans(1.0,rt_1,1.0,rt_0); + rt_1.add(vt/Ftinf,Zt.getData()); + + + Vector rt_0sav((const Vector&)rt_0); + Lt_0.multVecTrans(0.0,rt_0,1.0,rt_0sav); + + GeneralMatrix Nt_2sav((const GeneralMatrix&)Nt_2); + Nt_2.zeros(); + Nt_2.multAndAdd(Zt,"trans",Zt,Ft_2); + GeneralMatrix Nt_2Lt_0(Nt_2sav,Lt_0); + Nt_2.multAndAdd(Lt_0,"trans",Nt_2Lt_0); + GeneralMatrix Nt_1Lt_1(Nt_1,Lt_1); + Nt_2.multAndAdd(Lt_0,"trans",Nt_1Lt_1); + GeneralMatrix Nt_1Lt_0(Nt_1,Lt_0); + Nt_2.multAndAdd(Lt_1,"trans",Nt_1Lt_0); + GeneralMatrix Nt_0Lt_1(Nt_0,Lt_1); + Nt_2.multAndAdd(Lt_1,"trans",Nt_0Lt_1); + + + + Nt_1.zeros(); + Nt_1.multAndAdd(Zt,"trans",Zt,1.0/Ftinf); + Nt_1.multAndAdd(Lt_0,"trans",Nt_1Lt_0); + GeneralMatrix Nt_0Lt_0(Nt_0,Lt_0); + Nt_1.multAndAdd(Lt_1,"trans",Nt_0Lt_0); + + Nt_0.zeros(); + Nt_0.multAndAdd(Lt_0,"trans",Nt_0Lt_0); + + alphat= (const Vector&)at; + Ptstar.multVec(1.0,alphat,1.0,rt_0); + Ptinf.multVec(1.0,alphat,1.0,rt_1); + } - - Vector rt_1sav((const Vector&)rt_1); - Lt_0.multVecTrans(0.0,rt_1,1.0,rt_1sav); - Lt_1.multVecTrans(1.0,rt_1,1.0,rt_0); - rt_1.add(vt/Ftinf,Zt.getData()); - - - Vector rt_0sav((const Vector&)rt_0); - Lt_0.multVecTrans(0.0,rt_0,1.0,rt_0sav); - - GeneralMatrix Nt_2sav((const GeneralMatrix&)Nt_2); - Nt_2.zeros(); - Nt_2.multAndAdd(Zt,"trans",Zt,Ft_2); - GeneralMatrix Nt_2Lt_0(Nt_2sav,Lt_0); - Nt_2.multAndAdd(Lt_0,"trans",Nt_2Lt_0); - GeneralMatrix Nt_1Lt_1(Nt_1,Lt_1); - Nt_2.multAndAdd(Lt_0,"trans",Nt_1Lt_1); - GeneralMatrix Nt_1Lt_0(Nt_1,Lt_0); - Nt_2.multAndAdd(Lt_1,"trans",Nt_1Lt_0); - GeneralMatrix Nt_0Lt_1(Nt_0,Lt_1); - Nt_2.multAndAdd(Lt_1,"trans",Nt_0Lt_1); - - - - Nt_1.zeros(); - Nt_1.multAndAdd(Zt,"trans",Zt,1.0/Ftinf); - Nt_1.multAndAdd(Lt_0,"trans",Nt_1Lt_0); - GeneralMatrix Nt_0Lt_0(Nt_0,Lt_0); - Nt_1.multAndAdd(Lt_1,"trans",Nt_0Lt_0); - - Nt_0.zeros(); - Nt_0.multAndAdd(Lt_0,"trans",Nt_0Lt_0); - - alphat= (const Vector&)at; - Ptstar.multVec(1.0,alphat,1.0,rt_0); - Ptinf.multVec(1.0,alphat,1.0,rt_1); - } - - Vt= (const GeneralMatrix&)Ptstar; - GeneralMatrix Nt_0Ptstar(Nt_0,Ptstar); - Vt.multAndAdd(Ptstar,Nt_0Ptstar,-1.0); - GeneralMatrix Nt_2Ptinf(Nt_2,Ptinf); - Vt.multAndAdd(Ptinf,Nt_2Ptinf,-1.0); - GeneralMatrix Nt_1Ptstar(Nt_1,Ptstar); - GeneralMatrix PtinfNt_1Ptstar(Ptinf,Nt_1Ptstar); - Vt.add(-1.0,PtinfNt_1Ptstar); - Vt.add(-1.0,PtinfNt_1Ptstar,"trans"); + Vt= (const GeneralMatrix&)Ptstar; + GeneralMatrix Nt_0Ptstar(Nt_0,Ptstar); + Vt.multAndAdd(Ptstar,Nt_0Ptstar,-1.0); + GeneralMatrix Nt_2Ptinf(Nt_2,Ptinf); + Vt.multAndAdd(Ptinf,Nt_2Ptinf,-1.0); + GeneralMatrix Nt_1Ptstar(Nt_1,Ptstar); + GeneralMatrix PtinfNt_1Ptstar(Ptinf,Nt_1Ptstar); + Vt.add(-1.0,PtinfNt_1Ptstar); + Vt.add(-1.0,PtinfNt_1Ptstar,"trans"); } sres.set(t,alphat,etat,Vt); } } - - -double -KalmanUniTask::calcStepLogLik(double F,double v) - { - return-0.5*(log(2*M_PI)+log(F)+v*v/F); - } - - - + + + double + KalmanUniTask::calcStepLogLik(double F,double v) + { + return-0.5*(log(2*M_PI)+log(F)+v*v/F); + } + + + diff --git a/mex/sources/kalman/cc/kalman.h b/mex/sources/kalman/cc/kalman.h index 8deef681a..ae034b0a1 100644 --- a/mex/sources/kalman/cc/kalman.h +++ b/mex/sources/kalman/cc/kalman.h @@ -154,6 +154,50 @@ class SmootherResults{ }; +class BasicKalmanTask{ +// friend class KalmanUniTask; +// SSForm ssf; + const GeneralMatrix &data; + const ConstGeneralMatrix &Zt; + const ConstGeneralMatrix &Ht; + const ConstGeneralMatrix &Tt; + const ConstGeneralMatrix &Rt; + const ConstGeneralMatrix &Qt; + const StateInit&init; + public: + BasicKalmanTask(const GeneralMatrix&d,const GeneralMatrix&ZZ, + const GeneralMatrix&HH,const GeneralMatrix&TT, + const GeneralMatrix&RR,const GeneralMatrix&QQ, + const StateInit&init_state); +// BasicKalmanTask(const GeneralMatrix&d,const TMatrix&Z, +// const TMatrix&H,const TMatrix&T, +// const TMatrix&R,const TMatrix&Q, +// const StateInit&init_state); + BasicKalmanTask(const GeneralMatrix&d,const ConstGeneralMatrix&ZZ, + const ConstGeneralMatrix&HH,const ConstGeneralMatrix&TT, + const ConstGeneralMatrix&RR,const ConstGeneralMatrix&QQ, + const StateInit&init_state); + virtual ~BasicKalmanTask(); +// double filter(int&per,int&d)const; +// double filter(int&per,int&d, int start, std::vector* vll)const; + double filter(int&per,int&d,int start, std::vector* vll)const; +// double filter_and_smooth(SmootherResults&sres,int&per,int&d)const; + protected: + double filterNonDiffuse(const Vector&a,const GeneralMatrix&Pstar, + int start, std::vector* vll) const; //int first,FilterResults&fres)const; +// void filterDiffuse(const Vector&a,const GeneralMatrix&Pstar, +// const GeneralMatrix&Pinf,int first, +// DiffuseFilterResults&fres)const; +// void smootherNonDiffuse(const FilterResults&fres,SmootherResults&sres)const; +// void smootherDiffuse(const DiffuseFilterResults&fres,SmootherResults&sres)const; +// void smootherNonDiffuseStep(int t,const FilterResults&fres, +// Vector&rt,GeneralMatrix&Nt, +// Vector&alphat,GeneralMatrix&Vt, +// Vector&etat)const; + static double calcStepLogLik(const PLUFact&Finv,const Vector&v); + }; + + class KalmanUniTask; class KalmanTask{ friend class KalmanUniTask; diff --git a/mex/sources/kalman/matlab/Makefile b/mex/sources/kalman/matlab/Makefile index a955f8f4e..c77281ef9 100644 --- a/mex/sources/kalman/matlab/Makefile +++ b/mex/sources/kalman/matlab/Makefile @@ -1,16 +1,16 @@ # $Id: Makefile 531 2005-11-30 13:49:48Z kamenik $ # Copyright 2005, Ondra Kamenik -DEBUG = yes +#DEBUG = yes #LD_LIBS := -llapack -lcblas -lf77blas -latlas -lg2c -CC_FLAGS := -DMATLAB -DWINDOWS -DNO_BLAS_H -DNO_LAPACK_H \ - -Wall -I../../gensylv/cc -I../cc \ +CC_FLAGS := -DMATLAB -DWINDOWS -DNO_BLAS_H -DNO_LAPACK_H \ + -Wall -I../sylv/cc -I../cc \ -Ic:/"Program Files"/MATLAB_SV71/extern/include ifeq ($(DEBUG),yes) # CC_FLAGS := -DDEBUG $(CC_FLAGS) -g - CC_FLAGS := -DTIMING_LOOP -DDEBUG $(CC_FLAGS) -g -pg #-Wl,-pg + CC_FLAGS := -DTIMING_LOOP -DDEBUG $(CC_FLAGS) -g #-pg #-Wl,-pg KALMANLIB := kalmanlib_dbg.a else # CC_FLAGS := $(CC_FLAGS) -O2 @@ -34,9 +34,9 @@ endif # end add matrix_interface := GeneralMatrix Vector SylvException -matobjs := $(patsubst %, ../../gensylv/cc/%.o, $(matrix_interface)) -mathsource := $(patsubst %, ../../gensylv/cc/%.h, $(matrix_interface)) -matcppsource := $(patsubst %, ../../gensylv/cc/%.cpp, $(matrix_interface)) +matobjs := $(patsubst %, ../sylv/cc/%.o, $(matrix_interface)) +mathsource := $(patsubst %, ../sylv/cc/%.h, $(matrix_interface)) +matcppsource := $(patsubst %, ../sylv/cc/%.cpp, $(matrix_interface)) # cppsource := $(patsubst %.cweb,%.cpp,$(cwebsource)) kalmancppsource := $(wildcard ../cc/*.cpp) kalmanhsource := $(wildcard ../cc/*.h) @@ -62,27 +62,27 @@ dummy.ch: c++ $(CC_FLAGS) -c $*.cpp -kalmanlib.a: $(objects) # $(matobjs) #$(kalmanobjects) - ar cr kalmanlib.a $(kalmanobjects) $(matobjs) # $(objects) - ranlib kalmanlib.a +$(KALMANLIB): $(objects) # $(matobjs) #$(kalmanobjects) + ar cr $(KALMANLIB) $(kalmanobjects) $(matobjs) # $(objects) + ranlib $(KALMANLIB) -kalman_smoother_dll.dll: kalman_smoother.o kalmanlib.a #$(hsource) $(cppsource) +kalman_smoother_dll.dll: kalman_smoother.o $(KALMANLIB) #$(hsource) $(cppsource) gcc -shared $(CC_FLAGS) -o kalman_smoother_dll.dll kalman_smoother.o \ kalmanlib.a $(LD_LIBS) -minv.dll: minv.o kalmanlib.a # $(hsource) $(cppsource) +minv.dll: minv.o $(KALMANLIB) # $(hsource) $(cppsource) gcc -shared $(CC_FLAGS) -o minv.dll minv.o \ kalmanlib.a $(LD_LIBS) -kalman_filter_dll.dll: kalman_filters.o kalmanlib.a # $(hsource) $(cppsource) +kalman_filter_dll.dll: kalman_filters.o $(KALMANLIB) # $(hsource) $(cppsource) gcc -shared $(CC_FLAGS) -o kalman_filter_dll.dll kalman_filters.o \ $(KALMANLIB) $(LD_LIBS) -kalman_filters_testx.exe: kalman_filters_testx.o kalmanlib.a # $(hsource) $(cppsource) +kalman_filters_testx.exe: kalman_filters_testx.o $(KALMANLIB) # $(hsource) $(cppsource) gcc $(CC_FLAGS) -pg -o kalman_filters_testx.exe kalman_filters_testx.o \ $(KALMANLIB) $(LD_LIBS) -all: $(objects) kalmanlib.a kalman_smoother_dll.dll kalman_filter_dll.dll # $(cppsource) $(hsource) $(kalmanhsource) $(kalmancppsource) +all: $(objects) $(KALMANLIB) kalman_smoother_dll.dll kalman_filter_dll.dll # $(cppsource) $(hsource) $(kalmanhsource) $(kalmancppsource) kalman_filter_loop.o: kalman_filters.cpp c++ -DTIMING_LOOP $(CC_FLAGS) -o kalman_filter_loop.o kalman_filters.cpp diff --git a/mex/sources/kalman/matlab/kalman_filters.cpp b/mex/sources/kalman/matlab/kalman_filters.cpp index 07ad00133..169b52423 100644 --- a/mex/sources/kalman/matlab/kalman_filters.cpp +++ b/mex/sources/kalman/matlab/kalman_filters.cpp @@ -115,34 +115,56 @@ extern "C" { // create state init StateInit* init = NULL; std::vector* vll=new std::vector (nper); - if (diffuse) { - GeneralMatrix Pinf(mxGetPr(prhs[9]), mxGetM(prhs[9]), mxGetN(prhs[9])); - init = new StateInit(P, Pinf, a.getData()); - } - else - { - init = new StateInit(P, a.getData()); - } - // fork, create objects and do filtering - KalmanTask kt(Y, Z, H, T, R, Q, *init); - if (uni) - { - KalmanUniTask kut(kt); - loglik = kut.filter(per, d, (start-1), vll); - per = per / Y.numRows(); - d = d / Y.numRows(); - } - else - { + if (diffuse||uni) + { + if (diffuse) + { + GeneralMatrix Pinf(mxGetPr(prhs[9]), mxGetM(prhs[9]), mxGetN(prhs[9])); + init = new StateInit(P, Pinf, a.getData()); + } + else + { + init = new StateInit(P, a.getData()); + } + // fork, create objects and do filtering + KalmanTask kt(Y, Z, H, T, R, Q, *init); + if (uni) + { + KalmanUniTask kut(kt); + loglik = kut.filter(per, d, (start-1), vll); + per = per / Y.numRows(); + d = d / Y.numRows(); + } + else + { #ifdef TIMING_LOOP for (int tt=0;tt<1000;++tt) { #endif - loglik = kt.filter(per, d, (start-1), vll); + loglik = kt.filter(per, d, (start-1), vll); #ifdef TIMING_LOOP } - //mexPrintf("kalman_filter: finished 10,000 loops"); + mexPrintf("kalman_filter: finished 1000 loops"); #endif + } + } + else // basic Kalman + { + init = new StateInit(P, a.getData()); + BasicKalmanTask bkt(Y, Z, H, T, R, Q, *init); +#ifdef TIMING_LOOP + for (int tt=0;tt<1000;++tt) + { +#endif + loglik = bkt.filter( per, d, (start-1), vll); +#ifdef DEBUG + mexPrintf("Basickalman_filter: loglik=%d \n", loglik); +#endif +#ifdef TIMING_LOOP + } + mexPrintf("Basickalman_filter: finished 1000 loops"); +#endif + } // destroy init delete init; diff --git a/mex/sources/kalman/matlab/kalman_filters_testx.cpp b/mex/sources/kalman/matlab/kalman_filters_testx.cpp index 4f19ec856..717ea27a8 100644 --- a/mex/sources/kalman/matlab/kalman_filters_testx.cpp +++ b/mex/sources/kalman/matlab/kalman_filters_testx.cpp @@ -218,38 +218,64 @@ int main(int argc, char* argv[]) int d; // create state init StateInit* init = NULL; - std::vector* vll=new std::vector (nper); - if (diffuse) { - GeneralMatrix Pinf(P.numRows(),P.numCols()); - Pinf.zeros(); - init = new StateInit(P, Pinf, a.getData()); - } - else - { - init = new StateInit(P, a.getData()); - } - // fork, create objects and do filtering -#ifdef TIMING_LOOP - for (int tt=0;tt<10000;++tt) - { -#endif - KalmanTask kt(Y, Z, H, T, R, Q, *init); - if (uni) - { - KalmanUniTask kut(kt); - loglik = kut.filter(per, d, (start-1), vll); - per = per / Y.numRows(); - d = d / Y.numRows(); - } - else - { - loglik = kt.filter(per, d, (start-1), vll); - } -#ifdef TIMING_LOOP - // mexPrintf("kalman_filter: finished %d loops", tt); - } - mexPrintf("kalman_filter: finished 10,000 loops"); -#endif + std::vector* vll=new std::vector (nper); + + if (diffuse||uni) + { + if (diffuse) + { + GeneralMatrix Pinf(P.numRows(),P.numCols()); + Pinf.zeros(); + init = new StateInit(P, Pinf, a.getData()); + } + else + { + init = new StateInit(P, a.getData()); + } + // fork, create objects and do filtering + #ifdef TIMING_LOOP + for (int tt=0;tt<10000;++tt) + { + #endif + KalmanTask kt(Y, Z, H, T, R, Q, *init); + if (uni) + { + KalmanUniTask kut(kt); + loglik = kut.filter(per, d, (start-1), vll); + per = per / Y.numRows(); + d = d / Y.numRows(); + } + else + { + loglik = kt.filter(per, d, (start-1), vll); + } + #ifdef TIMING_LOOP + // mexPrintf("kalman_filter: finished %d loops", tt); + } + mexPrintf("kalman_filter: finished 10,000 loops"); + #endif + + } + else // basic Kalman + { + init = new StateInit(P, a.getData()); + BasicKalmanTask bkt(Y, Z, H, T, R, Q, *init); +#ifdef TIMING_LOOP + for (int tt=0;tt<10000;++tt) + { +#endif + loglik = bkt.filter( per, d, (start-1), vll); +#ifdef DEBUG + mexPrintf("Basickalman_filter: loglik=%d \n", loglik); +#endif +#ifdef TIMING_LOOP + } + mexPrintf("Basickalman_filter: finished 10,000 loops"); +#endif + + } + + // destroy init delete init; mexPrintf("logLik = %f \n", loglik);