2011-09-20 14:18:31 +02:00
/*
2020-01-10 17:55:57 +01:00
* Copyright © 2007 - 2020 Dynare Team
2011-09-20 14:18:31 +02:00
*
* 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 < http : //www.gnu.org/licenses/>.
*/
2019-04-30 12:59:43 +02:00
2011-09-20 14:18:31 +02:00
# include <cmath>
2019-04-30 12:59:43 +02:00
# include <algorithm>
2019-06-25 15:42:32 +02:00
# include <omp.h>
2019-04-18 09:46:03 +02:00
# include "block_kalman_filter.hh"
2019-04-30 12:59:43 +02:00
2013-03-22 15:47:57 +01:00
# define BLAS
//#define CUBLAS
2011-09-20 14:18:31 +02:00
2013-03-22 15:47:57 +01:00
# ifdef CUBLAS
2017-05-16 16:30:27 +02:00
# include <cuda_runtime.h>
# include <cublas_v2.h>
2013-03-22 15:47:57 +01:00
# endif
2019-04-30 12:59:43 +02:00
2011-09-20 14:18:31 +02:00
void
2019-04-30 12:59:43 +02:00
mexDisp ( const mxArray * P )
2011-09-20 14:18:31 +02:00
{
2019-04-30 12:59:43 +02:00
size_t n = mxGetN ( P ) ;
size_t m = mxGetM ( P ) ;
const double * M = mxGetPr ( P ) ;
2011-09-20 14:18:31 +02:00
mexPrintf ( " %d x %d \n " , m , n ) ;
mexEvalString ( " drawnow; " ) ;
2019-04-30 12:59:43 +02:00
for ( size_t i = 0 ; i < m ; i + + )
2011-09-20 14:18:31 +02:00
{
2019-04-30 12:59:43 +02:00
for ( size_t j = 0 ; j < n ; j + + )
2017-05-16 16:30:27 +02:00
mexPrintf ( " %9.4f " , M [ i + j * m ] ) ;
2011-09-20 14:18:31 +02:00
mexPrintf ( " \n " ) ;
}
2012-06-11 10:39:25 +02:00
mexEvalString ( " drawnow; " ) ;
2011-09-20 14:18:31 +02:00
}
2012-06-11 10:39:25 +02:00
void
2019-04-30 12:59:43 +02:00
mexDisp ( const double * M , int m , int n )
2012-06-11 10:39:25 +02:00
{
mexPrintf ( " %d x %d \n " , m , n ) ;
mexEvalString ( " drawnow; " ) ;
for ( int i = 0 ; i < m ; i + + )
{
for ( int j = 0 ; j < n ; j + + )
2017-05-16 16:30:27 +02:00
mexPrintf ( " %9.4f " , M [ i + j * m ] ) ;
2012-06-11 10:39:25 +02:00
mexPrintf ( " \n " ) ;
}
mexEvalString ( " drawnow; " ) ;
}
2019-04-30 12:59:43 +02:00
2012-06-11 10:39:25 +02:00
/*if block
2017-05-16 16:30:27 +02:00
% nz_state_var = M_ . nz_state_var ;
while notsteady & & t < smpl
t = t + 1 ;
v = Y ( : , t ) - a ( mf ) ;
F = P ( mf , mf ) + H ;
if rcond ( F ) < kalman_tol
if ~ all ( abs ( F ( : ) ) < kalman_tol )
return
else
a = T * a ;
P = T * P * transpose ( T ) + QQ ;
end
else
F_singular = 0 ;
dF = det ( F ) ;
iF = inv ( F ) ;
lik ( t ) = log ( dF ) + transpose ( v ) * iF * v ;
K = P ( : , mf ) * iF ;
a = T * ( a + K * v ) ;
P = block_pred_Vcov_KF ( mf , P , K , T , QQ ) ;
% P = T * ( P - K * P ( mf , : ) ) * transpose ( T ) + QQ ;
notsteady = max ( abs ( K ( : ) - oldK ) ) > riccati_tol ;
oldK = K ( : ) ;
end
end ;
else
while notsteady & & t < smpl
t = t + 1 ;
v = Y ( : , t ) - a ( mf ) ;
F = P ( mf , mf ) + H ;
if rcond ( F ) < kalman_tol
if ~ all ( abs ( F ( : ) ) < kalman_tol )
return
else
a = T * a ;
P = T * P * transpose ( T ) + QQ ;
end
else
F_singular = 0 ;
dF = det ( F ) ;
iF = inv ( F ) ;
lik ( t ) = log ( dF ) + transpose ( v ) * iF * v ;
K = P ( : , mf ) * iF ;
a = T * ( a + K * v ) ;
P = T * ( P - K * P ( mf , : ) ) * transpose ( T ) + QQ ;
notsteady = max ( abs ( K ( : ) - oldK ) ) > riccati_tol ;
oldK = K ( : ) ;
end
end
end
2012-06-11 10:39:25 +02:00
*/
2011-09-20 14:18:31 +02:00
bool
2019-04-30 12:59:43 +02:00
not_all_abs_F_bellow_crit ( const double * F , int size , double crit )
2011-09-20 14:18:31 +02:00
{
2017-05-16 16:30:27 +02:00
int i = 0 ;
while ( i < size & & abs ( F [ i ] ) < crit )
2019-04-30 12:59:43 +02:00
i + + ;
2017-05-16 16:30:27 +02:00
if ( i < size )
return false ;
else
return true ;
2011-09-20 14:18:31 +02:00
}
double
2019-04-30 12:59:43 +02:00
det ( const double * F , int dim , const lapack_int * ipiv )
2011-09-20 14:18:31 +02:00
{
double det = 1.0 ;
for ( int i = 0 ; i < dim ; i + + )
2011-11-25 14:45:54 +01:00
if ( ipiv [ i ] - 1 = = i )
det * = F [ i + i * dim ] ;
else
det * = - F [ i + i * dim ] ;
2011-09-20 14:18:31 +02:00
return det ;
}
2012-06-11 10:39:25 +02:00
2019-04-30 12:59:43 +02:00
BlockKalmanFilter : : BlockKalmanFilter ( int nlhs , mxArray * plhs [ ] , int nrhs , const mxArray * prhs [ ] )
2011-09-20 14:18:31 +02:00
{
2020-01-10 17:55:57 +01:00
if ( nlhs > 2 )
mexErrMsgTxt ( " block_kalman_filter provides at most 2 output argument. " ) ;
2012-06-11 10:39:25 +02:00
if ( nrhs ! = 13 & & nrhs ! = 16 )
2020-01-10 17:55:57 +01:00
mexErrMsgTxt ( " block_kalman_filter requires exactly \n 13 input arguments for standard Kalman filter \n or \n 16 input arguments for missing observations Kalman filter. " ) ;
2017-05-16 16:30:27 +02:00
if ( nrhs = = 16 )
2012-06-11 10:39:25 +02:00
missing_observations = true ;
else
missing_observations = false ;
if ( missing_observations )
{
2017-05-16 16:30:27 +02:00
if ( ! mxIsCell ( prhs [ 0 ] ) )
2020-01-10 17:55:57 +01:00
mexErrMsgTxt ( " the first input argument of block_missing_observations_kalman_filter must be a Cell Array. " ) ;
2012-06-11 10:39:25 +02:00
pdata_index = prhs [ 0 ] ;
2017-05-16 16:30:27 +02:00
if ( ! mxIsDouble ( prhs [ 1 ] ) )
2020-01-10 17:55:57 +01:00
mexErrMsgTxt ( " the second input argument of block_missing_observations_kalman_filter must be a scalar. " ) ;
2012-06-11 10:39:25 +02:00
number_of_observations = ceil ( mxGetScalar ( prhs [ 1 ] ) ) ;
2017-05-16 16:30:27 +02:00
if ( ! mxIsDouble ( prhs [ 2 ] ) )
2020-01-10 17:55:57 +01:00
mexErrMsgTxt ( " the third input argument of block_missing_observations_kalman_filter must be a scalar. " ) ;
2012-06-11 10:39:25 +02:00
no_more_missing_observations = ceil ( mxGetScalar ( prhs [ 2 ] ) ) ;
pT = mxDuplicateArray ( prhs [ 3 ] ) ;
pR = mxDuplicateArray ( prhs [ 4 ] ) ;
pQ = mxDuplicateArray ( prhs [ 5 ] ) ;
pH = mxDuplicateArray ( prhs [ 6 ] ) ;
pP = mxDuplicateArray ( prhs [ 7 ] ) ;
pY = mxDuplicateArray ( prhs [ 8 ] ) ;
start = mxGetScalar ( prhs [ 9 ] ) ;
2019-07-09 11:51:23 +02:00
mfd = mxGetPr ( prhs [ 10 ] ) ;
2012-06-11 10:39:25 +02:00
kalman_tol = mxGetScalar ( prhs [ 11 ] ) ;
riccati_tol = mxGetScalar ( prhs [ 12 ] ) ;
2019-07-09 11:51:23 +02:00
nz_state_var = mxGetPr ( prhs [ 13 ] ) ;
2012-06-11 10:39:25 +02:00
n_diag = mxGetScalar ( prhs [ 14 ] ) ;
pure_obs = mxGetScalar ( prhs [ 15 ] ) ;
}
else
{
no_more_missing_observations = 0 ;
pT = mxDuplicateArray ( prhs [ 0 ] ) ;
pR = mxDuplicateArray ( prhs [ 1 ] ) ;
pQ = mxDuplicateArray ( prhs [ 2 ] ) ;
pH = mxDuplicateArray ( prhs [ 3 ] ) ;
pP = mxDuplicateArray ( prhs [ 4 ] ) ;
pY = mxDuplicateArray ( prhs [ 5 ] ) ;
start = mxGetScalar ( prhs [ 6 ] ) ;
/*Defining the initials values*/
2019-12-20 14:50:19 +01:00
n = mxGetN ( pT ) ; // Number of state variables.
pp = mxGetM ( pY ) ; // Maximum number of observed variables.
smpl = mxGetN ( pY ) ; // Sample size. ;
2019-07-09 11:51:23 +02:00
mfd = mxGetPr ( prhs [ 7 ] ) ;
2012-06-11 10:39:25 +02:00
kalman_tol = mxGetScalar ( prhs [ 8 ] ) ;
riccati_tol = mxGetScalar ( prhs [ 9 ] ) ;
2019-07-09 11:51:23 +02:00
nz_state_var = mxGetPr ( prhs [ 10 ] ) ;
2012-06-11 10:39:25 +02:00
n_diag = mxGetScalar ( prhs [ 11 ] ) ;
pure_obs = mxGetScalar ( prhs [ 12 ] ) ;
}
T = mxGetPr ( pT ) ;
R = mxGetPr ( pR ) ;
Q = mxGetPr ( pQ ) ;
H = mxGetPr ( pH ) ;
2019-04-30 12:59:43 +02:00
P = mxGetPr ( pP ) ;
2012-06-11 10:39:25 +02:00
Y = mxGetPr ( pY ) ;
2017-05-16 16:30:27 +02:00
2019-12-20 14:50:19 +01:00
n = mxGetN ( pT ) ; // Number of state variables.
pp = mxGetM ( pY ) ; // Maximum number of observed variables.
smpl = mxGetN ( pY ) ; // Sample size. ;
2012-06-11 10:39:25 +02:00
n_state = n - pure_obs ;
/*mexPrintf("T\n");
2017-05-16 16:30:27 +02:00
mexDisp ( pT ) ; */
2011-10-28 22:25:05 +02:00
2012-06-11 10:39:25 +02:00
H_size = mxGetN ( pH ) * mxGetM ( pH ) ;
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
n_shocks = mxGetM ( pQ ) ;
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
if ( missing_observations )
2019-04-23 12:58:38 +02:00
if ( mxGetNumberOfElements ( pdata_index ) ! = static_cast < unsigned int > ( smpl ) )
2020-01-10 17:55:57 +01:00
mexErrMsgTxt ( " the number of element in the cell array passed to block_missing_observation_kalman_filter as first argument has to be equal to the smpl size " ) ;
2017-05-16 16:30:27 +02:00
2019-04-30 12:59:43 +02:00
i_nz_state_var = std : : make_unique < int [ ] > ( n ) ;
2012-06-11 10:39:25 +02:00
for ( int i = 0 ; i < n ; i + + )
i_nz_state_var [ i ] = nz_state_var [ i ] ;
2011-09-20 14:18:31 +02:00
2019-12-20 14:50:19 +01:00
pa = mxCreateDoubleMatrix ( n , 1 , mxREAL ) ; // State vector.
2019-04-30 12:59:43 +02:00
a = mxGetPr ( pa ) ;
tmp_a = std : : make_unique < double [ ] > ( n ) ;
2019-12-20 14:50:19 +01:00
dF = 0.0 ; // det(F).
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
p_tmp1 = mxCreateDoubleMatrix ( n , n_shocks , mxREAL ) ;
tmp1 = mxGetPr ( p_tmp1 ) ;
2019-12-20 14:50:19 +01:00
t = 0 ; // Initialization of the time index.
plik = mxCreateDoubleMatrix ( smpl , 1 , mxREAL ) ;
2012-06-11 10:39:25 +02:00
lik = mxGetPr ( plik ) ;
2019-12-20 14:50:19 +01:00
Inf = mxGetInf ( ) ;
LIK = 0.0 ; // Default value of the log likelihood.
notsteady = true ; // Steady state flag.
F_singular = true ;
2019-04-30 12:59:43 +02:00
v_pp = std : : make_unique < double [ ] > ( pp ) ;
v_n = std : : make_unique < double [ ] > ( n ) ;
mf = std : : make_unique < int [ ] > ( pp ) ;
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < pp ; i + + )
mf [ i ] = mfd [ i ] - 1 ;
2017-05-16 16:30:27 +02:00
2019-12-20 14:50:19 +01:00
/*compute QQ = R*Q*transpose(R)*/ // Variance of R times the vector of structural innovations.;
2011-09-20 14:18:31 +02:00
// tmp = R * Q;
for ( int i = 0 ; i < n ; i + + )
2011-11-25 14:27:25 +01:00
for ( int j = 0 ; j < n_shocks ; j + + )
2011-09-20 14:18:31 +02:00
{
double res = 0.0 ;
2011-10-28 22:40:05 +02:00
for ( int k = 0 ; k < n_shocks ; k + + )
2011-11-25 14:27:25 +01:00
res + = R [ i + k * n ] * Q [ j * n_shocks + k ] ;
2011-09-20 14:18:31 +02:00
tmp1 [ i + j * n ] = res ;
}
// QQ = tmp * transpose(R)
2012-06-11 10:39:25 +02:00
pQQ = mxCreateDoubleMatrix ( n , n , mxREAL ) ;
QQ = mxGetPr ( pQQ ) ;
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < n ; i + + )
for ( int j = i ; j < n ; j + + )
{
double res = 0.0 ;
2011-10-28 22:40:05 +02:00
for ( int k = 0 ; k < n_shocks ; k + + )
2011-09-20 14:18:31 +02:00
res + = tmp1 [ i + k * n ] * R [ k * n + j ] ;
QQ [ i + j * n ] = QQ [ j + i * n ] = res ;
}
mxDestroyArray ( p_tmp1 ) ;
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
pv = mxCreateDoubleMatrix ( pp , 1 , mxREAL ) ;
v = mxGetPr ( pv ) ;
2019-12-20 14:50:19 +01:00
pF = mxCreateDoubleMatrix ( pp , pp , mxREAL ) ;
2012-06-11 10:39:25 +02:00
F = mxGetPr ( pF ) ;
2019-12-20 14:50:19 +01:00
piF = mxCreateDoubleMatrix ( pp , pp , mxREAL ) ;
2012-06-11 10:39:25 +02:00
iF = mxGetPr ( piF ) ;
lw = pp * 4 ;
2019-04-30 12:59:43 +02:00
w = std : : make_unique < double [ ] > ( lw ) ;
iw = std : : make_unique < lapack_int [ ] > ( pp ) ;
ipiv = std : : make_unique < lapack_int [ ] > ( pp ) ;
2012-06-11 10:39:25 +02:00
info = 0 ;
2013-06-12 16:05:10 +02:00
# if defined(BLAS) || defined(CUBLAS)
2013-03-22 15:47:57 +01:00
p_tmp = mxCreateDoubleMatrix ( n , n , mxREAL ) ;
2019-04-30 12:59:43 +02:00
tmp = mxGetPr ( p_tmp ) ;
2013-03-22 15:47:57 +01:00
p_P_t_t1 = mxCreateDoubleMatrix ( n , n , mxREAL ) ;
2019-04-30 12:59:43 +02:00
P_t_t1 = mxGetPr ( p_P_t_t1 ) ;
2013-03-22 15:47:57 +01:00
pK = mxCreateDoubleMatrix ( n , n , mxREAL ) ;
2019-04-30 12:59:43 +02:00
K = mxGetPr ( pK ) ;
2013-03-22 15:47:57 +01:00
p_K_P = mxCreateDoubleMatrix ( n , n , mxREAL ) ;
2019-04-30 12:59:43 +02:00
K_P = mxGetPr ( p_K_P ) ;
oldK = std : : make_unique < double [ ] > ( n * n ) ;
P_mf = std : : make_unique < double [ ] > ( n * n ) ;
2013-03-22 15:47:57 +01:00
for ( int i = 0 ; i < n * n ; i + + )
oldK [ i ] = Inf ;
# else
p_tmp = mxCreateDoubleMatrix ( n , n_state , mxREAL ) ;
2019-04-30 12:59:43 +02:00
tmp = mxGetPr ( p_tmp ) ;
2012-06-11 10:39:25 +02:00
p_P_t_t1 = mxCreateDoubleMatrix ( n_state , n_state , mxREAL ) ;
2019-04-30 12:59:43 +02:00
P_t_t1 = mxGetPr ( p_P_t_t1 ) ;
2012-06-11 10:39:25 +02:00
pK = mxCreateDoubleMatrix ( n , pp , mxREAL ) ;
2019-04-30 12:59:43 +02:00
K = mxGetPr ( pK ) ;
2012-06-11 10:39:25 +02:00
p_K_P = mxCreateDoubleMatrix ( n_state , n_state , mxREAL ) ;
2019-04-30 12:59:43 +02:00
K_P = mxGetPr ( p_K_P ) ;
oldK = std : : make_unique < double [ ] > ( n * pp ) ;
P_mf = std : : make_unique < double [ ] > ( n * pp ) ;
2012-06-11 10:39:25 +02:00
for ( int i = 0 ; i < n * pp ; i + + )
oldK [ i ] = Inf ;
2013-03-22 15:47:57 +01:00
# endif
2012-06-11 10:39:25 +02:00
}
2017-05-16 16:30:27 +02:00
void
2019-04-30 12:59:43 +02:00
BlockKalmanFilter : : block_kalman_filter_ss ( )
2012-06-11 10:39:25 +02:00
{
if ( t + 1 < smpl )
2019-04-30 12:59:43 +02:00
while ( t < smpl )
{
//v = Y(:,t)-a(mf);
for ( int i = 0 ; i < pp ; i + + )
2019-12-20 14:50:19 +01:00
v [ i ] = Y [ i + t * pp ] - a [ mf [ i ] ] ;
2017-05-16 16:30:27 +02:00
2019-04-30 12:59:43 +02:00
//a = T*(a+K*v);
for ( int i = pure_obs ; i < n ; i + + )
{
double res = 0.0 ;
for ( int j = 0 ; j < pp ; j + + )
res + = K [ j * n + i ] * v [ j ] ;
v_n [ i ] = res + a [ i ] ;
}
for ( int i = 0 ; i < n ; i + + )
{
double res = 0.0 ;
for ( int j = pure_obs ; j < n ; j + + )
res + = T [ j * n + i ] * v_n [ j ] ;
a [ i ] = res ;
}
2017-05-16 16:30:27 +02:00
2019-04-30 12:59:43 +02:00
//lik(t) = transpose(v)*iF*v;
for ( int i = 0 ; i < pp ; i + + )
{
double res = 0.0 ;
for ( int j = 0 ; j < pp ; j + + )
res + = v [ j ] * iF [ j * pp + i ] ;
v_pp [ i ] = res ;
}
double res = 0.0 ;
for ( int i = 0 ; i < pp ; i + + )
res + = v_pp [ i ] * v [ i ] ;
2012-06-11 10:39:25 +02:00
2019-04-30 12:59:43 +02:00
lik [ t ] = ( log ( dF ) + res + pp * log ( 2.0 * M_PI ) ) / 2 ;
if ( t + 1 > start )
LIK + = lik [ t ] ;
2012-06-11 10:39:25 +02:00
2019-04-30 12:59:43 +02:00
t + + ;
}
2012-06-11 10:39:25 +02:00
}
bool
2019-04-30 12:59:43 +02:00
BlockKalmanFilter : : block_kalman_filter ( int nlhs , mxArray * plhs [ ] )
2012-06-11 10:39:25 +02:00
{
2011-09-20 14:18:31 +02:00
while ( notsteady & & t < smpl )
{
2017-05-16 16:30:27 +02:00
if ( missing_observations )
2012-06-11 10:39:25 +02:00
{
// retrieve the d_index
2017-05-16 16:30:27 +02:00
pd_index = mxGetCell ( pdata_index , t ) ;
2019-07-09 11:51:23 +02:00
dd_index = mxGetPr ( pd_index ) ;
2012-06-11 10:39:25 +02:00
size_d_index = mxGetM ( pd_index ) ;
d_index . resize ( size_d_index ) ;
for ( int i = 0 ; i < size_d_index ; i + + )
2019-04-30 12:59:43 +02:00
d_index [ i ] = ceil ( dd_index [ i ] ) - 1 ;
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
//v = Y(:,t) - a(mf)
int i_i = 0 ;
2019-06-25 15:42:32 +02:00
//#pragma omp parallel for shared(v, i_i, d_index)
2019-04-30 12:59:43 +02:00
for ( auto i = d_index . begin ( ) ; i ! = d_index . end ( ) ; i + + )
2012-06-11 10:39:25 +02:00
{
//mexPrintf("i_i=%d, omp_get_max_threads()=%d\n",i_i,omp_get_max_threads());
2019-12-20 14:50:19 +01:00
v [ i_i ] = Y [ * i + t * pp ] - a [ mf [ * i ] ] ;
2012-06-11 10:39:25 +02:00
i_i + + ;
}
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
//F = P(mf,mf) + H;
i_i = 0 ;
if ( H_size = = 1 )
2019-06-25 15:42:32 +02:00
//#pragma omp parallel for shared(iF, F, i_i)
2019-04-30 12:59:43 +02:00
for ( auto i = d_index . begin ( ) ; i ! = d_index . end ( ) ; i + + , i_i + + )
{
int j_j = 0 ;
for ( auto j = d_index . begin ( ) ; j ! = d_index . end ( ) ; j + + , j_j + + )
iF [ i_i + j_j * size_d_index ] = F [ i_i + j_j * size_d_index ] = P [ mf [ * i ] + mf [ * j ] * n ] + H [ 0 ] ;
}
2012-06-11 10:39:25 +02:00
else
2019-06-25 15:42:32 +02:00
//#pragma omp parallel for shared(iF, F, P, H, mf, i_i)
2019-04-30 12:59:43 +02:00
for ( auto i = d_index . begin ( ) ; i ! = d_index . end ( ) ; i + + , i_i + + )
{
int j_j = 0 ;
for ( auto j = d_index . begin ( ) ; j ! = d_index . end ( ) ; j + + , j_j + + )
iF [ i_i + j_j * size_d_index ] = F [ i_i + j_j * size_d_index ] = P [ mf [ * i ] + mf [ * j ] * n ] + H [ * i + * j * pp ] ;
}
2012-06-11 10:39:25 +02:00
}
else
{
size_d_index = pp ;
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
//v = Y(:,t) - a(mf)
for ( int i = 0 ; i < pp ; i + + )
2019-12-20 14:50:19 +01:00
v [ i ] = Y [ i + t * pp ] - a [ mf [ i ] ] ;
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
//F = P(mf,mf) + H;
if ( H_size = = 1 )
2019-04-30 12:59:43 +02:00
for ( int i = 0 ; i < pp ; i + + )
for ( int j = 0 ; j < pp ; j + + )
iF [ i + j * pp ] = F [ i + j * pp ] = P [ mf [ i ] + mf [ j ] * n ] + H [ 0 ] ;
2012-06-11 10:39:25 +02:00
else
2019-04-30 12:59:43 +02:00
for ( int i = 0 ; i < pp ; i + + )
for ( int j = 0 ; j < pp ; j + + )
iF [ i + j * pp ] = F [ i + j * pp ] = P [ mf [ i ] + mf [ j ] * n ] + H [ i + j * pp ] ;
2012-06-11 10:39:25 +02:00
}
2017-05-16 16:30:27 +02:00
2013-03-22 15:47:57 +01:00
/* Computes the norm of iF */
2019-04-30 12:59:43 +02:00
double anorm = dlange ( " 1 " , & size_d_index , & size_d_index , iF , & size_d_index , w . get ( ) ) ;
2012-06-11 10:39:25 +02:00
//mexPrintf("anorm = %f\n",anorm);
2011-09-20 14:18:31 +02:00
2013-03-22 15:47:57 +01:00
/* Modifies F in place with a LU decomposition */
2019-04-30 12:59:43 +02:00
dgetrf ( & size_d_index , & size_d_index , iF , & size_d_index , ipiv . get ( ) , & info ) ;
2017-05-16 16:30:27 +02:00
if ( info ! = 0 )
2019-04-23 12:58:38 +02:00
mexPrintf ( " dgetrf failure with error %d \n " , static_cast < int > ( info ) ) ;
2017-05-16 16:30:27 +02:00
2013-03-22 15:47:57 +01:00
/* Computes the reciprocal norm */
2019-04-30 12:59:43 +02:00
dgecon ( " 1 " , & size_d_index , iF , & size_d_index , & anorm , & rcond , w . get ( ) , iw . get ( ) , & info ) ;
2017-05-16 16:30:27 +02:00
if ( info ! = 0 )
2019-04-23 12:58:38 +02:00
mexPrintf ( " dgecon failure with error %d \n " , static_cast < int > ( info ) ) ;
2017-05-16 16:30:27 +02:00
2011-09-20 14:18:31 +02:00
if ( rcond < kalman_tol )
2019-12-20 14:50:19 +01:00
if ( not_all_abs_F_bellow_crit ( F , size_d_index * size_d_index , kalman_tol ) ) //~all(abs(F(:))<kalman_tol)
2011-09-20 14:18:31 +02:00
{
mexPrintf ( " error: F singular \n " ) ;
2019-12-20 14:50:19 +01:00
LIK = Inf ;
2020-01-10 17:55:57 +01:00
if ( nlhs = = 2 )
2019-04-30 12:59:43 +02:00
for ( int i = t ; i < smpl ; i + + )
lik [ i ] = Inf ;
2012-06-11 10:39:25 +02:00
// info = 0
2019-04-30 12:59:43 +02:00
return_results_and_clean ( nlhs , plhs ) ;
2012-06-11 10:39:25 +02:00
return false ;
2011-09-20 14:18:31 +02:00
}
else
{
mexPrintf ( " F singular \n " ) ;
2012-06-11 10:39:25 +02:00
2011-09-20 14:18:31 +02:00
//a = T*a;
for ( int i = 0 ; i < n ; i + + )
{
double res = 0.0 ;
2011-10-28 22:25:05 +02:00
for ( int j = pure_obs ; j < n ; j + + )
2011-09-20 14:18:31 +02:00
res + = T [ i + j * n ] * a [ j ] ;
tmp_a [ i ] = res ;
}
2019-04-30 12:59:43 +02:00
std : : copy_n ( tmp_a . get ( ) , n , a ) ;
2011-09-20 14:18:31 +02:00
2017-05-16 16:30:27 +02:00
//P = T*P*transpose(T)+QQ;
2019-04-30 12:59:43 +02:00
std : : fill_n ( tmp , 0 , n * n_state ) ;
2012-06-11 10:39:25 +02:00
2017-05-16 16:30:27 +02:00
for ( int i = 0 ; i < n ; i + + )
for ( int j = pure_obs ; j < n ; j + + )
{
int j1 = j - pure_obs ;
int j1_n_state = j1 * n_state - pure_obs ;
for ( int k = pure_obs ; k < i_nz_state_var [ i ] ; k + + )
2019-12-20 14:50:19 +01:00
tmp [ i + j1 * n ] + = T [ i + k * n ] * P [ k + j1_n_state ] ;
2017-05-16 16:30:27 +02:00
}
2012-06-11 10:39:25 +02:00
2019-04-30 12:59:43 +02:00
std : : fill_n ( P , 0 , n * n ) ;
2017-05-16 16:30:27 +02:00
int n_n_obs = n * pure_obs ;
for ( int i = 0 ; i < n ; i + + )
for ( int j = i ; j < n ; j + + )
2019-04-30 12:59:43 +02:00
for ( int k = pure_obs ; k < i_nz_state_var [ j ] ; k + + )
{
int k_n = k * n ;
P [ i * n + j ] + = tmp [ i + k_n - n_n_obs ] * T [ j + k_n ] ;
}
2017-05-16 16:30:27 +02:00
for ( int i = 0 ; i < n ; i + + )
2011-09-20 14:18:31 +02:00
{
2017-05-16 16:30:27 +02:00
for ( int j = i ; j < n ; j + + )
P [ j + i * n ] + = QQ [ j + i * n ] ;
for ( int j = i + 1 ; j < n ; j + + )
P [ i + j * n ] = P [ j + i * n ] ;
2011-09-20 14:18:31 +02:00
}
2017-05-16 16:30:27 +02:00
}
2011-09-20 14:18:31 +02:00
else
{
F_singular = false ;
2017-05-16 16:30:27 +02:00
2011-09-20 14:18:31 +02:00
//dF = det(F);
2019-12-20 14:50:19 +01:00
dF = det ( iF , size_d_index , ipiv . get ( ) ) ;
2011-09-20 14:18:31 +02:00
//iF = inv(F);
//int lwork = 4/*2*/* pp;
2019-04-30 12:59:43 +02:00
dgetri ( & size_d_index , iF , & size_d_index , ipiv . get ( ) , w . get ( ) , & lw , & info ) ;
2017-05-16 16:30:27 +02:00
if ( info ! = 0 )
2019-04-23 12:58:38 +02:00
mexPrintf ( " dgetri failure with error %d \n " , static_cast < int > ( info ) ) ;
2011-09-20 14:18:31 +02:00
//lik(t) = log(dF)+transpose(v)*iF*v;
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(v_pp)
2012-06-11 10:39:25 +02:00
for ( int i = 0 ; i < size_d_index ; i + + )
2011-09-20 14:18:31 +02:00
{
double res = 0.0 ;
2012-06-11 10:39:25 +02:00
for ( int j = 0 ; j < size_d_index ; j + + )
res + = v [ j ] * iF [ j * size_d_index + i ] ;
2011-09-20 14:18:31 +02:00
v_pp [ i ] = res ;
}
double res = 0.0 ;
2012-06-11 10:39:25 +02:00
for ( int i = 0 ; i < size_d_index ; i + + )
2011-09-20 14:18:31 +02:00
res + = v_pp [ i ] * v [ i ] ;
2019-04-30 12:59:43 +02:00
lik [ t ] = ( log ( dF ) + res + size_d_index * log ( 2.0 * M_PI ) ) / 2 ;
2011-09-20 14:18:31 +02:00
if ( t + 1 > = start )
LIK + = lik [ t ] ;
2012-06-11 10:39:25 +02:00
if ( missing_observations )
2019-12-20 14:50:19 +01:00
//K = P(:,mf)*iF;
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(P_mf)
2019-04-30 12:59:43 +02:00
for ( int i = 0 ; i < n ; i + + )
{
int j_j = 0 ;
//for (int j = 0; j < pp; j++)
for ( auto j = d_index . begin ( ) ; j ! = d_index . end ( ) ; j + + , j_j + + )
P_mf [ i + j_j * n ] = P [ i + mf [ * j ] * n ] ;
}
2012-06-11 10:39:25 +02:00
else
2019-04-30 12:59:43 +02:00
//K = P(:,mf)*iF;
for ( int i = 0 ; i < n ; i + + )
for ( int j = 0 ; j < pp ; j + + )
P_mf [ i + j * n ] = P [ i + mf [ j ] * n ] ;
2017-05-16 16:30:27 +02:00
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(K)
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < n ; i + + )
2012-06-11 10:39:25 +02:00
for ( int j = 0 ; j < size_d_index ; j + + )
2011-09-20 14:18:31 +02:00
{
double res = 0.0 ;
2012-06-11 10:39:25 +02:00
int j_pp = j * size_d_index ;
for ( int k = 0 ; k < size_d_index ; k + + )
2011-09-20 14:18:31 +02:00
res + = P_mf [ i + k * n ] * iF [ j_pp + k ] ;
K [ i + j * n ] = res ;
}
2017-05-16 16:30:27 +02:00
2011-09-20 14:18:31 +02:00
//a = T*(a+K*v);
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(v_n)
2011-10-28 22:25:05 +02:00
for ( int i = pure_obs ; i < n ; i + + )
2011-09-20 14:18:31 +02:00
{
double res = 0.0 ;
2012-06-11 10:39:25 +02:00
for ( int j = 0 ; j < size_d_index ; j + + )
2011-09-20 14:18:31 +02:00
res + = K [ j * n + i ] * v [ j ] ;
v_n [ i ] = res + a [ i ] ;
}
2017-05-16 16:30:27 +02:00
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(a)
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < n ; i + + )
{
double res = 0.0 ;
2011-10-28 22:25:05 +02:00
for ( int j = pure_obs ; j < n ; j + + )
2011-09-20 14:18:31 +02:00
res + = T [ j * n + i ] * v_n [ j ] ;
a [ i ] = res ;
}
2017-05-16 16:30:27 +02:00
2012-06-11 10:39:25 +02:00
if ( missing_observations )
{
//P = T*(P-K*P(mf,:))*transpose(T)+QQ;
int i_i = 0 ;
2019-06-25 15:42:32 +02:00
//#pragma omp parallel for shared(P_mf)
2019-04-30 12:59:43 +02:00
for ( auto i = d_index . begin ( ) ; i ! = d_index . end ( ) ; i + + , i_i + + )
2012-06-11 10:39:25 +02:00
for ( int j = pure_obs ; j < n ; j + + )
P_mf [ i_i + j * size_d_index ] = P [ mf [ * i ] + j * n ] ;
}
else
2019-04-30 12:59:43 +02:00
//P = T*(P-K*P(mf,:))*transpose(T)+QQ;
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(P_mf)
2019-04-30 12:59:43 +02:00
for ( int i = 0 ; i < pp ; i + + )
for ( int j = pure_obs ; j < n ; j + + )
P_mf [ i + j * pp ] = P [ mf [ i ] + j * n ] ;
2017-05-16 16:30:27 +02:00
2011-09-20 14:18:31 +02:00
# ifdef BLAS
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(K_P)
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < n ; i + + )
for ( int j = i ; j < n ; j + + )
{
double res = 0.0 ;
//int j_pp = j * pp;
2012-06-11 10:39:25 +02:00
for ( int k = 0 ; k < size_d_index ; k + + )
res + = K [ i + k * n ] * P_mf [ k + j * size_d_index ] ;
2011-09-20 14:18:31 +02:00
K_P [ i * n + j ] = K_P [ j * n + i ] = res ;
}
//#pragma omp parallel for shared(P, K_P, P_t_t1)
2012-06-11 10:39:25 +02:00
for ( int i = size_d_index ; i < n ; i + + )
2011-09-20 14:18:31 +02:00
for ( int j = i ; j < n ; j + + )
{
unsigned int k = i * n + j ;
P_t_t1 [ j * n + i ] = P_t_t1 [ k ] = P [ k ] - K_P [ k ] ;
}
2019-12-20 14:50:19 +01:00
double one = 1.0 ;
2011-09-20 14:18:31 +02:00
double zero = 0.0 ;
2019-04-30 12:59:43 +02:00
std : : copy_n ( QQ , n * n , P ) ;
2013-03-22 15:47:57 +01:00
blas_int n_b = n ;
/*mexPrintf("sizeof(n_b)=%d, n_b=%d, sizeof(n)=%d, n=%d\n",sizeof(n_b),n_b,sizeof(n),n);
2017-05-16 16:30:27 +02:00
mexEvalString ( " drawnow; " ) ; */
2013-03-22 15:47:57 +01:00
dsymm ( " R " , " U " , & n_b , & n_b ,
& one , P_t_t1 , & n_b ,
T , & n_b , & zero ,
tmp , & n_b ) ;
dgemm ( " N " , " T " , & n_b , & n_b ,
& n_b , & one , tmp , & n_b ,
T , & n_b , & one ,
P , & n_b ) ;
# else
2017-05-16 16:30:27 +02:00
# ifdef CUBLAS
for ( int i = 0 ; i < n ; i + + )
2013-03-22 15:47:57 +01:00
for ( int j = i ; j < n ; j + + )
{
double res = 0.0 ;
//int j_pp = j * pp;
for ( int k = 0 ; k < size_d_index ; k + + )
res + = K [ i + k * n ] * P_mf [ k + j * size_d_index ] ;
K_P [ i * n + j ] = K_P [ j * n + i ] = res ;
}
//#pragma omp parallel for shared(P, K_P, P_t_t1)
for ( int i = size_d_index ; i < n ; i + + )
for ( int j = i ; j < n ; j + + )
{
unsigned int k = i * n + j ;
P_t_t1 [ j * n + i ] = P_t_t1 [ k ] = P [ k ] - K_P [ k ] ;
}
mexPrintf ( " CudaBLAS \n " ) ;
mexEvalString ( " drawnow; " ) ;
2019-12-20 14:50:19 +01:00
double one = 1.0 ;
2013-03-22 15:47:57 +01:00
double zero = 0.0 ;
cublasStatus_t status ;
cublasHandle_t handle ;
status = cublasCreate ( & handle ) ;
if ( status ! = CUBLAS_STATUS_SUCCESS )
{
mexPrintf ( " !!!! CUBLAS initialization error \n " ) ;
return false ;
}
/*int device;
2017-05-16 16:30:27 +02:00
cudaGetDevice ( & device ) ; */
2013-03-22 15:47:57 +01:00
int n2 = n * n ;
2019-04-30 12:59:43 +02:00
double * d_A = nullptr , * d_B = nullptr , * d_C = nullptr , * d_D = nullptr ;
2013-03-22 15:47:57 +01:00
// Allocate device memory for the matrices
2019-04-30 12:59:43 +02:00
if ( cudaMalloc ( static_cast < void * * > ( & d_A ) , n2 * sizeof ( double ) ) ! = cudaSuccess )
2013-03-22 15:47:57 +01:00
{
mexPrintf ( " !!!! device memory allocation error (allocate A) \n " ) ;
return false ;
}
2019-04-30 12:59:43 +02:00
if ( cudaMalloc ( static_cast < void * * > ( & d_B ) , n2 * sizeof ( d_B [ 0 ] ) ) ! = cudaSuccess )
2013-03-22 15:47:57 +01:00
{
mexPrintf ( " !!!! device memory allocation error (allocate B) \n " ) ;
return false ;
}
2019-04-30 12:59:43 +02:00
if ( cudaMalloc ( static_cast < void * * > ( & d_C ) , n2 * sizeof ( d_C [ 0 ] ) ) ! = cudaSuccess )
2013-03-22 15:47:57 +01:00
{
mexPrintf ( " !!!! device memory allocation error (allocate C) \n " ) ;
return false ;
}
2019-04-30 12:59:43 +02:00
if ( cudaMalloc ( static_cast < void * * > ( & d_D ) , n2 * sizeof ( d_D [ 0 ] ) ) ! = cudaSuccess )
2013-03-22 15:47:57 +01:00
{
mexPrintf ( " !!!! device memory allocation error (allocate D) \n " ) ;
return false ;
}
// Initialize the device matrices with the host matrices
status = cublasSetVector ( n2 , sizeof ( P_t_t1 [ 0 ] ) , P_t_t1 , 1 , d_A , 1 ) ;
if ( status ! = CUBLAS_STATUS_SUCCESS )
{
mexPrintf ( " !!!! device access error (write A) \n " ) ;
return false ;
}
status = cublasSetVector ( n2 , sizeof ( T [ 0 ] ) , T , 1 , d_B , 1 ) ;
if ( status ! = CUBLAS_STATUS_SUCCESS )
{
mexPrintf ( " !!!! device access error (write B) \n " ) ;
return false ;
}
status = cublasSetVector ( n2 , sizeof ( tmp [ 0 ] ) , tmp , 1 , d_C , 1 ) ;
if ( status ! = CUBLAS_STATUS_SUCCESS )
{
mexPrintf ( " !!!! device access error (write C) \n " ) ;
return false ;
2017-05-16 16:30:27 +02:00
}
2013-03-22 15:47:57 +01:00
mexPrintf ( " just before calling \n " ) ;
mexEvalString ( " drawnow; " ) ;
status = cublasSetVector ( n2 , sizeof ( QQ [ 0 ] ) , QQ , 1 , d_D , 1 ) ;
if ( status ! = CUBLAS_STATUS_SUCCESS )
{
mexPrintf ( " !!!! device access error (write D) \n " ) ;
return false ;
2017-05-16 16:30:27 +02:00
}
2013-03-22 15:47:57 +01:00
// Performs operation using plain C code
cublasDsymm ( handle , CUBLAS_SIDE_RIGHT , CUBLAS_FILL_MODE_UPPER , n , n ,
2017-05-16 16:30:27 +02:00
& one , d_A , n ,
d_B , n , & zero ,
d_C , n ) ;
2013-03-22 15:47:57 +01:00
/*dgemm("N", "T", &n_b, &n_b,
2017-05-16 16:30:27 +02:00
& n_b , & one , tmp , & n_b ,
T , & n_b , & one ,
P , & n_b ) ; */
2013-03-22 15:47:57 +01:00
cublasDgemm ( handle , CUBLAS_OP_N , CUBLAS_OP_T , n , n ,
2017-05-16 16:30:27 +02:00
n , & one , d_C , n ,
d_B , n , & one ,
d_D , n ) ;
2013-03-22 15:47:57 +01:00
//double_symm(n, &one, h_A, h_B, &zero, h_C);
status = cublasGetVector ( n2 , sizeof ( P [ 0 ] ) , d_D , 1 , P , 1 ) ;
if ( status ! = CUBLAS_STATUS_SUCCESS )
{
mexPrintf ( " !!!! device access error (read P) \n " ) ;
return false ;
}
2017-05-16 16:30:27 +02:00
# else
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(K_P)
2011-10-28 22:25:05 +02:00
for ( int i = pure_obs ; i < n ; i + + )
2011-09-20 14:18:31 +02:00
{
2011-10-28 22:25:05 +02:00
unsigned int i1 = i - pure_obs ;
2017-05-16 16:30:27 +02:00
for ( int j = i ; j < n ; j + + )
2011-09-20 14:18:31 +02:00
{
2011-10-28 22:25:05 +02:00
unsigned int j1 = j - pure_obs ;
2011-09-20 14:18:31 +02:00
double res = 0.0 ;
2012-06-11 10:39:25 +02:00
int j_pp = j * size_d_index ;
for ( int k = 0 ; k < size_d_index ; k + + )
2011-09-20 14:18:31 +02:00
res + = K [ i + k * n ] * P_mf [ k + j_pp ] ;
K_P [ i1 * n_state + j1 ] = K_P [ j1 * n_state + i1 ] = res ;
}
}
2012-06-11 10:39:25 +02:00
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(P_t_t1)
2011-10-28 22:25:05 +02:00
for ( int i = pure_obs ; i < n ; i + + )
2011-09-20 14:18:31 +02:00
{
2011-10-28 22:25:05 +02:00
unsigned int i1 = i - pure_obs ;
2011-09-20 14:18:31 +02:00
for ( int j = i ; j < n ; j + + )
{
2011-10-28 22:25:05 +02:00
unsigned int j1 = j - pure_obs ;
2011-09-20 14:18:31 +02:00
unsigned int k1 = i1 * n_state + j1 ;
P_t_t1 [ j1 * n_state + i1 ] = P_t_t1 [ k1 ] = P [ i * n + j ] - K_P [ k1 ] ;
}
}
2012-06-11 10:39:25 +02:00
2019-04-30 12:59:43 +02:00
fill_n ( tmp , 0 , n * n_state ) ;
2012-06-11 10:39:25 +02:00
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(tmp)
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < n ; i + + )
{
int max_k = i_nz_state_var [ i ] ;
2011-10-28 22:25:05 +02:00
for ( int j = pure_obs ; j < n ; j + + )
2011-09-20 14:18:31 +02:00
{
2011-10-28 22:25:05 +02:00
int j1 = j - pure_obs ;
int j1_n_state = j1 * n_state - pure_obs ;
2017-05-16 16:30:27 +02:00
int indx_tmp = i + j1 * n ;
2012-06-11 10:39:25 +02:00
for ( int k = pure_obs ; k < max_k ; k + + )
2011-09-20 14:18:31 +02:00
tmp [ indx_tmp ] + = T [ i + k * n ] * P_t_t1 [ k + j1_n_state ] ;
}
}
2012-06-11 10:39:25 +02:00
2019-04-30 12:59:43 +02:00
fill_n ( P , 0 , n * n ) ;
2012-06-11 10:39:25 +02:00
2017-05-16 16:30:27 +02:00
int n_n_obs = - n * pure_obs ;
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(P)
2011-09-20 14:18:31 +02:00
for ( int i = 0 ; i < n ; i + + )
{
for ( int j = i ; j < n ; j + + )
{
int max_k = i_nz_state_var [ j ] ;
int P_indx = i * n + j ;
2011-10-28 22:25:05 +02:00
for ( int k = pure_obs ; k < max_k ; k + + )
2011-09-20 14:18:31 +02:00
{
int k_n = k * n ;
P [ P_indx ] + = tmp [ i + k_n + n_n_obs ] * T [ j + k_n ] ;
}
}
}
2012-06-11 10:39:25 +02:00
2019-06-25 15:42:32 +02:00
# pragma omp parallel for shared(P)
2017-05-16 16:30:27 +02:00
for ( int i = 0 ; i < n ; i + + )
2011-09-20 14:18:31 +02:00
{
2017-05-16 16:30:27 +02:00
for ( int j = i ; j < n ; j + + )
2011-09-20 14:18:31 +02:00
P [ j + i * n ] + = QQ [ j + i * n ] ;
2017-05-16 16:30:27 +02:00
for ( int j = i + 1 ; j < n ; j + + )
2011-09-20 14:18:31 +02:00
P [ i + j * n ] = P [ j + i * n ] ;
}
2017-05-16 16:30:27 +02:00
# endif
2013-03-22 15:47:57 +01:00
# endif
2012-06-11 10:39:25 +02:00
if ( t > = no_more_missing_observations )
2011-09-20 14:18:31 +02:00
{
2012-06-11 10:39:25 +02:00
double max_abs = 0.0 ;
for ( int i = 0 ; i < n * size_d_index ; i + + )
{
double res = abs ( K [ i ] - oldK [ i ] ) ;
2019-04-30 12:59:43 +02:00
max_abs = std : : max ( res , max_abs ) ;
2012-06-11 10:39:25 +02:00
}
notsteady = max_abs > riccati_tol ;
2011-09-20 14:18:31 +02:00
2012-06-11 10:39:25 +02:00
//oldK = K(:);
2017-05-16 16:30:27 +02:00
2019-04-30 12:59:43 +02:00
std : : copy_n ( K , n * pp , oldK . get ( ) ) ;
2012-06-11 10:39:25 +02:00
}
2011-09-20 14:18:31 +02:00
}
t + + ;
}
if ( F_singular )
mexErrMsgTxt ( " The variance of the forecast error remains singular until the end of the sample \n " ) ;
2012-06-11 10:39:25 +02:00
if ( t < smpl )
2019-04-30 12:59:43 +02:00
block_kalman_filter_ss ( ) ;
2012-06-11 10:39:25 +02:00
return true ;
}
2011-09-20 14:18:31 +02:00
2017-05-16 16:30:27 +02:00
void
2019-04-30 12:59:43 +02:00
BlockKalmanFilter : : return_results_and_clean ( int nlhs , mxArray * plhs [ ] )
2012-06-11 10:39:25 +02:00
{
2020-01-10 17:55:57 +01:00
if ( nlhs > = 1 )
2011-09-20 14:18:31 +02:00
{
2020-01-10 17:55:57 +01:00
plhs [ 0 ] = mxCreateDoubleMatrix ( 1 , 1 , mxREAL ) ;
double * pind = mxGetPr ( plhs [ 0 ] ) ;
2011-09-20 14:18:31 +02:00
pind [ 0 ] = LIK ;
}
2020-01-10 17:55:57 +01:00
if ( nlhs = = 2 )
plhs [ 1 ] = plik ;
2011-09-23 18:22:00 +02:00
else
mxDestroyArray ( plik ) ;
2012-06-11 10:39:25 +02:00
2011-09-20 14:18:31 +02:00
mxDestroyArray ( pa ) ;
mxDestroyArray ( p_tmp ) ;
mxDestroyArray ( pQQ ) ;
mxDestroyArray ( pv ) ;
mxDestroyArray ( pF ) ;
mxDestroyArray ( piF ) ;
mxDestroyArray ( p_P_t_t1 ) ;
mxDestroyArray ( pK ) ;
mxDestroyArray ( p_K_P ) ;
}
2012-06-11 10:39:25 +02:00
void
mexFunction ( int nlhs , mxArray * plhs [ ] , int nrhs , const mxArray * prhs [ ] )
{
2019-04-30 12:59:43 +02:00
BlockKalmanFilter block_kalman_filter ( nlhs , plhs , nrhs , prhs ) ;
if ( block_kalman_filter . block_kalman_filter ( nlhs , plhs ) )
block_kalman_filter . return_results_and_clean ( nlhs , plhs ) ;
2012-06-11 10:39:25 +02:00
}