From f0fc974b9e6da48eb9bc65d5f873aa510808b493 Mon Sep 17 00:00:00 2001 From: George Perendia Date: Thu, 1 Apr 2010 13:36:58 +0100 Subject: [PATCH] !st draft Kalman Filter main, auxiliary an test routines for C++ estimation --- mex/sources/estimation/DetrendData.cc | 4 +- mex/sources/estimation/DetrendData.hh | 2 +- .../estimation/InitializeKalmanFilter.cc | 23 +-- .../estimation/InitializeKalmanFilter.hh | 8 +- mex/sources/estimation/KalmanFilter.cc | 154 ++++++++++++++++++ mex/sources/estimation/KalmanFilter.hh | 84 ++++++++++ .../estimation/tests/testInitKalman.cc | 12 +- 7 files changed, 265 insertions(+), 22 deletions(-) create mode 100644 mex/sources/estimation/KalmanFilter.cc create mode 100644 mex/sources/estimation/KalmanFilter.hh diff --git a/mex/sources/estimation/DetrendData.cc b/mex/sources/estimation/DetrendData.cc index 538faee83..ebd1d282b 100644 --- a/mex/sources/estimation/DetrendData.cc +++ b/mex/sources/estimation/DetrendData.cc @@ -31,7 +31,9 @@ DetrendData::DetrendData(const bool INlogLinear) //, Vector& INtrendCoeff) }; void -DetrendData::detrend(const Vector &SteadyState, const MatrixView &dataView, Matrix &Yiew) +DetrendData::detrend(const Vector &SteadyState, const MatrixView &dataView, Matrix &Y) { +// dummy +Y=dataView; }; diff --git a/mex/sources/estimation/DetrendData.hh b/mex/sources/estimation/DetrendData.hh index fe71873c7..cf1ee8681 100644 --- a/mex/sources/estimation/DetrendData.hh +++ b/mex/sources/estimation/DetrendData.hh @@ -33,7 +33,7 @@ class DetrendData public: virtual ~DetrendData(){}; DetrendData(const bool logLinear); // add later Vector& trendCoeff); - void detrend(const Vector &SteadyState, const MatrixView &dataView, Matrix &Yiew); + void detrend(const Vector &SteadyState, const MatrixView &dataView, Matrix &Y); private: const bool logLinear; diff --git a/mex/sources/estimation/InitializeKalmanFilter.cc b/mex/sources/estimation/InitializeKalmanFilter.cc index 3b65de4d9..fdd82650d 100644 --- a/mex/sources/estimation/InitializeKalmanFilter.cc +++ b/mex/sources/estimation/InitializeKalmanFilter.cc @@ -41,7 +41,7 @@ InitializeKalmanFilter::InitializeKalmanFilter(const std::string &modName, size_ ghx(n_endo_arg, zeta_back_arg.size() + zeta_mixed_arg.size()), ghx_raw(n_endo_arg, zeta_back_arg.size() + zeta_mixed_arg.size()), ghu(n_endo_arg, n_exo_arg), ghu_raw(n_endo_arg, n_exo_arg), - Rt(n_exo_arg, riv.size()), RQ(riv.size(), n_exo_arg), RQR(riv.size(), riv.size()) + Rt(n_exo_arg, riv.size()), RQ(riv.size(), n_exo_arg) { size_t n_pred = ghx.getCols(); size_t n_static = zeta_static_arg.size(); @@ -52,18 +52,19 @@ InitializeKalmanFilter::InitializeKalmanFilter(const std::string &modName, size_ } void -InitializeKalmanFilter::initialize(Vector &steadyState, Vector &deepParams, const Vector &xparams1, - Matrix &R, Matrix &Z, Matrix &Q, Matrix &T, Matrix &Pstar, Matrix &Pinf, - double &penalty, const MatrixView &dataView, MatrixView &yView, int &info) +InitializeKalmanFilter::initialize(Vector &steadyState, const Vector &deepParams, Matrix &R, const Matrix &Z, + const Matrix &Q, Matrix &RQRt, Matrix &T, Matrix &Pstar, Matrix &Pinf, + double &penalty, const MatrixView &dataView, Matrix &Y, int &info) { modelSolution.compute(steadyState, deepParams, ghx_raw, ghu_raw); + detrendData.detrend(steadyState, dataView, Y); mat::reorderRowsByVectors(ghx, mat::nullVec, ghx_raw, order_var); mat::reorderRowsByVectors(ghu, mat::nullVec, ghu_raw, order_var); setT(T, info); setR(R, info); - setPstar(Pstar, Pinf, T, R, Q, info); + setPstar(Pstar, Pinf, T, R, Q, RQRt, info); } void @@ -86,21 +87,21 @@ InitializeKalmanFilter::setR(Matrix &R, int &info) } void -InitializeKalmanFilter::setPstar(Matrix &Pstar, Matrix &Pinf, Matrix &T, Matrix &R, Matrix &Q, int &info) +InitializeKalmanFilter::setPstar(Matrix &Pstar, Matrix &Pinf, Matrix &T, Matrix &R, const Matrix &Q, Matrix &RQRt, int &info) { - // Matrix RQR=R*Q*R' + // Matrix RQRt=R*Q*R' RQ.setAll(0.0); blas::gemm("N", "N", 1.0, R, Q, 0.0, RQ); // R*Q - RQR.setAll(0.0); + RQRt.setAll(0.0); //mat::transpose(Rt, R); - blas::gemm("N", "T", 1.0, RQ, R, 0.0, RQR); // R*Q*R' + blas::gemm("N", "T", 1.0, RQ, R, 0.0, RQRt); // R*Q*R' //mat::transpose(RQR); try { - // disclyap_fast(T, RQR, Pstar, lyapunov_tol, 0); // 1 to check chol - discLyapFast.solve_lyap(T, RQR, Pstar, lyapunov_tol, 0); // 1 to check chol + // disclyap_fast(T, RQR, Pstar, lyapunov_tol, 0 or 1 to check chol) + discLyapFast.solve_lyap(T, RQRt, Pstar, lyapunov_tol, 0); Pinf.setAll(0.0); } diff --git a/mex/sources/estimation/InitializeKalmanFilter.hh b/mex/sources/estimation/InitializeKalmanFilter.hh index ff55e3b10..fac1ed90e 100644 --- a/mex/sources/estimation/InitializeKalmanFilter.hh +++ b/mex/sources/estimation/InitializeKalmanFilter.hh @@ -45,8 +45,8 @@ public: const Matrix &llincidence, double qz_criterium, const std::vector &order_var_arg, const std::vector &inv_order_var_arg, const std::vector &riv, const std::vector &ric, double lyapunov_tol, int &info); virtual ~InitializeKalmanFilter(); - void initialize(Vector &steadyState, Vector &deepParams, const Vector &xparams1, Matrix &R, Matrix &Z, Matrix &Q, - Matrix &T, Matrix &Pstar, Matrix &Pinf, double &penalty, const MatrixView &dataView, MatrixView &yView, int &info); + void initialize(Vector &steadyState, const Vector &deepParams, Matrix &R, const Matrix &Z, const Matrix &Q, Matrix &RQRt, + Matrix &T, Matrix &Pstar, Matrix &Pinf, double &penalty, const MatrixView &dataView, Matrix &Y, int &info); private: const std::vector riv; // restrict_var_list @@ -59,10 +59,10 @@ private: DiscLyapFast discLyapFast; //Lyapunov solver Matrix ghx, ghx_raw; Matrix ghu, ghu_raw; - Matrix Rt, RQ, RQR; + Matrix Rt, RQ; void setT(Matrix &T, int &info); void setR(Matrix &R, int &info); - void setPstar(Matrix &Pstar, Matrix &Pinf, Matrix &T, Matrix &R, Matrix &Q, int &info); + void setPstar(Matrix &Pstar, Matrix &Pinf, Matrix &T, Matrix &R, const Matrix &Q, Matrix &RQRt, int &info); }; diff --git a/mex/sources/estimation/KalmanFilter.cc b/mex/sources/estimation/KalmanFilter.cc new file mode 100644 index 000000000..aec3a6361 --- /dev/null +++ b/mex/sources/estimation/KalmanFilter.cc @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2009-2010 Dynare Team + * + * This file is part of Dynare. + * + * Dynare is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Dynare is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Dynare. If not, see . + */ + +/////////////////////////////////////////////////////////// +// KalmanFilter.cpp +// Implementation of the Class KalmanFilter +// Created on: 02-Feb-2010 12:44:41 +/////////////////////////////////////////////////////////// + +#include "KalmanFilter.hh" + +KalmanFilter::~KalmanFilter() +{ + +} + +KalmanFilter::KalmanFilter(const std::string &modName, size_t n_endo, size_t n_exo, + const std::vector &zeta_fwrd_arg, const std::vector &zeta_back_arg, + const std::vector &zeta_mixed_arg, const std::vector &zeta_static_arg, + const Matrix &ll_incidence, double qz_criterium, const std::vector &order_var, + const std::vector &inv_order_var, const std::vector &varobs_arg, + const std::vector &riv, const std::vector &ric, + double riccati_tol_in, double lyapunov_tol, int &info) : + Z(varobs_arg.size(), riv.size()), T(riv.size()), R(riv.size(), n_exo), + Pstar(riv.size(), riv.size()), Pinf(riv.size(), riv.size()), RQRt(riv.size(), riv.size()), + Pold(riv.size(), riv.size()), Ptmp(riv.size(), riv.size()), F(varobs_arg.size(), varobs_arg.size()), + Finv(varobs_arg.size(), varobs_arg.size()), K(riv.size(), varobs_arg.size()), + KFinv(riv.size(), varobs_arg.size()), Finverter(varobs_arg.size()), a_init(riv.size(), 1), + a_new(riv.size(), 1), vt(varobs_arg.size(), 1), vtFinv(1, varobs_arg.size()), vtFinvVt(1), riccati_tol(riccati_tol_in), + initKalmanFilter(modName, n_endo, n_exo, zeta_fwrd_arg, zeta_back_arg, zeta_mixed_arg, + zeta_static_arg, ll_incidence, qz_criterium, order_var, inv_order_var, riv, ric, lyapunov_tol, info) +{ + a_init.setAll(0.0); + Z.setAll(0.0); + for (size_t i = 0; i < varobs_arg.size(); ++i) + Z(i, varobs_arg[i]) = 1.0; + +} + +double +KalmanFilter::compute(const MatrixView &dataView, Vector &steadyState, + const Matrix &Q, const Matrix &H, const Vector &deepParams, + Vector &vll, size_t start, double &penalty, int &info) +{ + double ll; + Matrix Y(dataView.getRows(), dataView.getCols()); // data + + initKalmanFilter.initialize(steadyState, deepParams, R, Z, Q, RQRt, T, Pstar, Pinf, + penalty, dataView, Y, info); + + return ll = filter(Y, H, vll, start, info); + +}; + +/** + * 30:* + */ +double +KalmanFilter::filter(const Matrix &dataView, const Matrix &H, Vector &vll, size_t start, int &info) +{ + double loglik, ll, logFdet; + int p = Finv.getRows(); + + bool nonstationary = true; + for (size_t t = 0; t < dataView.getCols(); ++t) + { + if (nonstationary) + { + // K=PZ' + K.setAll(0.0); + blas::gemm("N", "T", 1.0, Pstar, Z, 1.0, K); + + //F=ZPZ' +H = ZK+H + F = H; + blas::gemm("N", "N", 1.0, Z, K, 1.0, F); + // logFdet=log|F| + + // Finv=inv(F) + mat::set_identity(Finv); + Finverter.invMult("N", F, Finv); // F now contains its LU decomposition! + // KFinv + KFinv.setAll(0.0); + blas::gemm("N", "N", 1.0, K, Finv, 1.0, KFinv); + // deteminant of F: + logFdet = 1; + for (int d = 0; d < p; ++d) + logFdet *= (-F(d, d)); + Pold = Pstar; + Ptmp = Pstar; + // Pt+1= T(Pt - KFinvK')T' +RQR' + // 1) Ptmp= Pt - K*FinvK' + blas::gemm("N", "T", -1.0, KFinv, K, 1.0, Ptmp); + // 2) Ptmp= T*Ptmp + Pstar = Ptmp; + Ptmp.setAll(0.0); + blas::gemm("N", "N", 1.0, T, Pstar, 1.0, Ptmp); + // 3) Pt+1= Ptmp*T' +RQR' + Pstar = RQRt; + blas::gemm("N", "T", 1.0, Ptmp, T, 1.0, Pstar); + nonstationary = mat::isDiffSym(Pstar, Pold, riccati_tol); + } + else + { + } + + // err= Yt - Za + //VectorView yt(dataView.getData()+t*dataView.getRows(),dataView.getRows(),1); // current observation vector + MatrixConstView yt(dataView, 0, t, dataView.getRows(), 1); // current observation vector + vt = yt; + blas::gemm("N", "N", -1.0, Z, a_init, 1.0, vt); + // at+1= T(at+ KFinv *err) + blas::gemm("N", "N", 1.0, KFinv, vt, 1.0, a_init); + blas::gemm("N", "N", 1.0, T, a_init, 0.0, a_new); + a_init = a_new; + + /***************** + Here we calc likelihood and store results. + *****************/ + blas::gemm("T", "N", 1.0, vt, Finv, 0.0, vtFinv); + blas::gemm("N", "N", 1.0, vtFinv, vt, 0.0, vtFinvVt); + + ll = -0.5*(p*log(2*M_PI)+logFdet+*(vtFinvVt.getData())); + + vll(t) = ll; + if (t > start) loglik += ll; + + } + + return loglik; +} + +/** + * 89:* + double KalmanFilter::calcStepLogLik(const PLUFact &Finv, const Vector &v){ + + } + */ + diff --git a/mex/sources/estimation/KalmanFilter.hh b/mex/sources/estimation/KalmanFilter.hh new file mode 100644 index 000000000..dbdc62528 --- /dev/null +++ b/mex/sources/estimation/KalmanFilter.hh @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2009-2010 Dynare Team + * + * This file is part of Dynare. + * + * Dynare is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Dynare is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Dynare. If not, see . + */ + +/////////////////////////////////////////////////////////// +// KalmanFilter.h +// Implementation of the Class KalmanFilter +// Created on: 02-Feb-2010 12:44:41 +/////////////////////////////////////////////////////////// + +#if !defined(KF_213B0417_532B_4027_9EDF_36C004CB4CD1__INCLUDED_) +#define KF_213B0417_532B_4027_9EDF_36C004CB4CD1__INCLUDED_ + +#include "InitializeKalmanFilter.hh" + +/** + * •vanilla Kalman filter without constant and with measurement error (use scalar + * 0 when no measurement error). + * If multivariate filter is faster, do as in Matlab: start with multivariate + * filter and switch to univariate filter only in case of singularity + * + * mamber functions: compute() and filter() + * OUTPUT + * LIK: likelihood + * + * REFERENCES + * See "Filtering and Smoothing of State Vector for Diffuse State Space + * Models", S.J. Koopman and J. Durbin (2003, in Journal of Time Series + * Analysis, vol. 24(1), pp. 85-98). + */ + +class KalmanFilter { + +public: + virtual ~KalmanFilter(); + KalmanFilter(const std::string &modName, size_t n_endo, size_t n_exo, const std::vector &zeta_fwrd_arg, + const std::vector &zeta_back_arg, const std::vector &zeta_mixed_arg, const std::vector &zeta_static_arg, + const Matrix &llincidence, double qz_criterium, const std::vector &order_var_arg, const std::vector &inv_order_var, + const std::vector &varobs_arg, const std::vector &riv, const std::vector &ric, + double riccati_tol, double lyapunov_tol, int &info); + + double compute(const MatrixView &dataView, Vector &steadyState, + const Matrix &Q, const Matrix &H, const Vector &deepParams, + Vector &vll, size_t start, double &penalty, int &info); + +protected: +// static double calcStepLogLik(const PLUFact &Finv, const Vector &v); + +private: + Matrix Z; //nob*mm matrix mapping endogeneous variables and observations + Matrix T; //mm*mm transition matrix of the state equation. + Matrix R; //mm*rr matrix, mapping structural innovations to state variables. + Matrix Pstar; //mm*mm variance-covariance matrix of stationary variables + Matrix Pinf; //mm*mm variance-covariance matrix of diffuse variables + // allocate space for intermediary matrices + Matrix RQRt, Pold, Ptmp; //mm*mm variance-covariance matrix of variable disturbances + Matrix F, Finv; // nob*nob F=ZPZt +H an inv(F) + Matrix K, KFinv; // mm*nobs K=PZt and nm*nm K*Finv + LUSolver Finverter; // matrix inversion algorithm + Matrix a_init, a_new; // state vector + Matrix vt; // current observation error vectors + Matrix vtFinv, vtFinvVt; // intermeiate observation error *Finv vector + double riccati_tol; + InitializeKalmanFilter initKalmanFilter; //Initialise KF matrices + double filter(const Matrix &dataView, const Matrix &H, Vector &vll, size_t start, int &info); + +}; + +#endif // !defined(213B0417_532B_4027_9EDF_36C004CB4CD1__INCLUDED_) diff --git a/mex/sources/estimation/tests/testInitKalman.cc b/mex/sources/estimation/tests/testInitKalman.cc index f2f77af68..576a42311 100644 --- a/mex/sources/estimation/tests/testInitKalman.cc +++ b/mex/sources/estimation/tests/testInitKalman.cc @@ -136,7 +136,8 @@ main(int argc, char **argv) varobs.push_back(svarobs[i]-1); Matrix - T(riv.size(), riv.size()), R(riv.size(), n_exo), Pstar(riv.size(), riv.size()), + T(riv.size(), riv.size()), R(riv.size(), n_exo), + RQRt(riv.size(), riv.size()), Pstar(riv.size(), riv.size()), Pinf(riv.size(), riv.size()), Z(varobs.size(), riv.size()), Q(n_exo); Z.setAll(0.0); for (size_t i = 0; i < varobs.size(); ++i) @@ -160,10 +161,10 @@ main(int argc, char **argv) double lyapunov_tol = 1e-16; int info = 0; - const MatrixView + const MatrixView dataView(&lyapunov_tol, 1, 1, 1); // dummy - MatrixView - yView(&lyapunov_tol, 1, 1, 1); // dummy + Matrix + yView(dataView.getRows(),dataView.getCols()); // dummy const Vector xparams1(0); // dummy double penalty = 1e8; @@ -176,11 +177,12 @@ main(int argc, char **argv) std::cout << "Initilise KF with Q: " << std::endl << Q << std::endl; std::cout << "and Z" << std::endl << Z << std::endl; - initializeKalmanFilter.initialize(steadyState, deepParams, xparams1, R, Z, Q, T, Pstar, Pinf, + initializeKalmanFilter.initialize(steadyState, deepParams, R, Z, Q, RQRt, T, Pstar, Pinf, penalty, dataView, yView, info); std::cout << "Matrix T: " << std::endl << T << std::endl; std::cout << "Matrix R: " << std::endl << R << std::endl; + std::cout << "Matrix RQRt: " << std::endl << RQRt << std::endl; std::cout << "Matrix Pstar: " << std::endl << Pstar << std::endl; std::cout << "Matrix Pinf: " << std::endl << Pinf << std::endl;