/*
* Copyright (C) 2009-2017 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"
#include "LapackBindings.hh"
KalmanFilter::~KalmanFilter()
{
}
KalmanFilter::KalmanFilter(const std::string &basename, 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,
double qz_criterium_arg, const std::vector &varobs_arg,
double riccati_tol_arg, double lyapunov_tol_arg,
bool noconstant_arg) :
zeta_varobs_back_mixed(compute_zeta_varobs_back_mixed(zeta_back_arg, zeta_mixed_arg, varobs_arg)),
Z(varobs_arg.size(), zeta_varobs_back_mixed.size()), Zt(Z.getCols(), Z.getRows()), T(zeta_varobs_back_mixed.size()), R(zeta_varobs_back_mixed.size(), n_exo),
Pstar(zeta_varobs_back_mixed.size(), zeta_varobs_back_mixed.size()), Pinf(zeta_varobs_back_mixed.size(), zeta_varobs_back_mixed.size()),
RQRt(zeta_varobs_back_mixed.size(), zeta_varobs_back_mixed.size()), Ptmp(zeta_varobs_back_mixed.size(), zeta_varobs_back_mixed.size()), F(varobs_arg.size(), varobs_arg.size()),
Finv(varobs_arg.size(), varobs_arg.size()), K(zeta_varobs_back_mixed.size(), varobs_arg.size()), KFinv(zeta_varobs_back_mixed.size(), varobs_arg.size()),
oldKFinv(zeta_varobs_back_mixed.size(), varobs_arg.size()), a_init(zeta_varobs_back_mixed.size()),
a_new(zeta_varobs_back_mixed.size()), vt(varobs_arg.size()), vtFinv(varobs_arg.size()), riccati_tol(riccati_tol_arg),
initKalmanFilter(basename, n_endo, n_exo, zeta_fwrd_arg, zeta_back_arg, zeta_mixed_arg,
zeta_static_arg, zeta_varobs_back_mixed, varobs_arg, qz_criterium_arg, lyapunov_tol_arg, noconstant_arg),
FUTP(varobs_arg.size()*(varobs_arg.size()+1)/2)
{
Z.setAll(0.0);
Zt.setAll(0.0);
for (size_t i = 0; i < varobs_arg.size(); ++i)
{
size_t j = find(zeta_varobs_back_mixed.begin(), zeta_varobs_back_mixed.end(),
varobs_arg[i]) - zeta_varobs_back_mixed.begin();
Z(i, j) = 1.0;
Zt(j, i) = 1.0;
}
}
std::vector
KalmanFilter::compute_zeta_varobs_back_mixed(const std::vector &zeta_back_arg, const std::vector &zeta_mixed_arg, const std::vector &varobs_arg)
{
std::vector varobs_sorted = varobs_arg;
sort(varobs_sorted.begin(), varobs_sorted.end());
std::vector zeta_back_mixed, zeta_varobs_back_mixed;
set_union(zeta_back_arg.begin(), zeta_back_arg.end(),
zeta_mixed_arg.begin(), zeta_mixed_arg.end(),
back_inserter(zeta_back_mixed));
set_union(zeta_back_mixed.begin(), zeta_back_mixed.end(),
varobs_sorted.begin(), varobs_sorted.end(),
back_inserter(zeta_varobs_back_mixed));
return zeta_varobs_back_mixed;
}
/**
* Multi-variate standard Kalman Filter
*/
double
KalmanFilter::filter(const MatrixView &detrendedDataView, const Matrix &H, VectorView &vll, size_t start)
{
double loglik = 0.0, ll, logFdet = 0.0, Fdet, dvtFinvVt;
size_t p = Finv.getRows();
bool nonstationary = true;
a_init.setAll(0.0);
int info;
for (size_t t = 0; t < detrendedDataView.getCols(); ++t)
{
if (nonstationary)
{
// K=PZ'
//blas::gemm("N", "T", 1.0, Pstar, Z, 0.0, K);
blas::symm("L", "U", 1.0, Pstar, Zt, 0.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);
// Pack F upper trinagle as vector
for (size_t i = 1; i <= p; ++i)
for (size_t j = i; j <= p; ++j)
FUTP(i + (j-1)*j/2 -1) = F(i-1, j-1);
info = lapack::choleskySolver(FUTP, Finv, "U"); // F now contains its Chol decomposition!
assert(info >= 0);
if (info > 0)
{
//enforce Pstar symmetry with P=(P+P')/2=0.5P+0.5P'
mat::transpose(Ptmp, Pstar);
mat::add(Pstar, Ptmp);
for (size_t i = 0; i < Pstar.getCols(); ++i)
for (size_t j = 0; j < Pstar.getCols(); ++j)
Pstar(i, j) *= 0.5;
// K=PZ'
//blas::gemm("N", "T", 1.0, Pstar, Z, 0.0, K);
blas::symm("L", "U", 1.0, Pstar, Zt, 0.0, K);
//F=ZPZ' +H = ZK+H
F = H;
blas::gemm("N", "N", 1.0, Z, K, 1.0, F);
// Finv=inv(F)
mat::set_identity(Finv);
// Pack F upper trinagle as vector
for (size_t i = 1; i <= p; ++i)
for (size_t j = i; j <= p; ++j)
FUTP(i + (j-1)*j/2 -1) = F(i-1, j-1);
info = lapack::choleskySolver(FUTP, Finv, "U"); // F now contains
// its Chol
// decomposition!
assert(info == 0);
}
// KFinv gain matrix
blas::symm("R", "U", 1.0, Finv, K, 0.0, KFinv);
// deteminant of F:
Fdet = 1;
for (size_t d = 1; d <= p; ++d)
Fdet *= FUTP(d + (d-1)*d/2 -1);
Fdet *= Fdet;
logFdet = log(fabs(Fdet));
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;
//blas::gemm("N", "N", 1.0, T, Pstar, 0.0, Ptmp);
blas::symm("R", "U", 1.0, Pstar, T, 0.0, Ptmp);
// 3) Pt+1= Ptmp*T' +RQR'
Pstar = RQRt;
blas::gemm("N", "T", 1.0, Ptmp, T, 1.0, Pstar);
if (t > 0)
nonstationary = mat::isDiff(KFinv, oldKFinv, riccati_tol);
oldKFinv = KFinv;
}
// err= Yt - Za
VectorConstView yt = mat::get_col(detrendedDataView, t);
vt = yt;
blas::gemv("N", -1.0, Z, a_init, 1.0, vt);
// at+1= T(at+ KFinv *err)
blas::gemv("N", 1.0, KFinv, vt, 1.0, a_init);
blas::gemv("N", 1.0, T, a_init, 0.0, a_new);
a_init = a_new;
/*****************
Here we calc likelihood and store results.
*****************/
blas::symv("U", 1.0, Finv, vt, 0.0, vtFinv);
dvtFinvVt = blas::dot(vtFinv, vt);
ll = -0.5*(p*log(2*M_PI)+logFdet+dvtFinvVt);
vll(t) = ll;
if (t >= start)
loglik += ll;
}
return loglik;
}