diff --git a/doc/manual/source/the-model-file.rst b/doc/manual/source/the-model-file.rst
index c1b183d5227642369a704534e1563325b4392b96..d49c15e7bea4750c6c3d37def23dd3df0ae596c2 100644
--- a/doc/manual/source/the-model-file.rst
+++ b/doc/manual/source/the-model-file.rst
@@ -2663,12 +2663,10 @@ blocks.
         var e2;
         periods 86:87 88:97;
         values 0.04 0.01;
-        end;
 
         var e3;
         periods 86:87;
         values 0.04;
-        end;
 
         var e3;
         periods 88:97;
diff --git a/matlab/+occbin/match_function.m b/matlab/+occbin/match_function.m
index 959f3c3c2d7ad3fd0104da94a2d546e5b4b90007..ae26236201fa04ca4d6f3ce21b6f129421cb5064 100644
--- a/matlab/+occbin/match_function.m
+++ b/matlab/+occbin/match_function.m
@@ -37,19 +37,18 @@ options_.occbin.simul=opts_simul;
 options_.occbin.simul.full_output=1;
 options_.noprint = 1;
 [~, out, ss] = occbin.solver(M_,oo_,options_);
-state_out= out.piecewise(1,:)' - out.ys;
-
-E = ss.R(:,opts_simul.exo_pos);
-grad = ss.R(opts_simul.varobs_id,opts_simul.exo_pos);
 
 nobs = size(obs_list,1);
 resids = zeros(nobs,1);
 
 if ~out.error_flag
-    % -- add observation block in model ---%
-    %   % put in model file
+    state_out= out.piecewise(1,:)' - out.ys;
+    
+    E = ss.R(:,opts_simul.exo_pos);
+    grad = ss.R(opts_simul.varobs_id,opts_simul.exo_pos);
     resids = (out.piecewise(1,opts_simul.varobs_id)-current_obs)'; %-out.endo_ss.(obs_list{this_obs});
 else
+    grad = NaN(length(opts_simul.varobs_id),length(opts_simul.exo_pos));
     resids = resids+100;
 end
 
diff --git a/matlab/+occbin/solver.m b/matlab/+occbin/solver.m
index 125f28d909a06e20279d53f9c5614c36ff7af832..ec053af49dd643b1f05c8ee91177f0214a088062 100644
--- a/matlab/+occbin/solver.m
+++ b/matlab/+occbin/solver.m
@@ -68,7 +68,6 @@ end
 out.error_flag=error_flag;
 if error_flag
     print_info(error_flag, options_.noprint, options_)
-    out=[];
     return;
 end
 
diff --git a/matlab/dynare_estimation_init.m b/matlab/dynare_estimation_init.m
index c341d7a56967092f324a5ad78b6218217c5b0020..3cdd4c097ea47997ebba010eca4e26f00c55139e 100644
--- a/matlab/dynare_estimation_init.m
+++ b/matlab/dynare_estimation_init.m
@@ -671,11 +671,13 @@ if options_.heteroskedastic_filter
 
     for k=1:length(M_.heteroskedastic_shocks.Qvalue_orig)
         v = M_.heteroskedastic_shocks.Qvalue_orig(k);
-        M_.heteroskedastic_shocks.Qvalue(v.exo_id, v.periods) = v.value^2;
+        temp_periods=v.periods(v.periods<=options_.nobs+options_.first_obs);
+        M_.heteroskedastic_shocks.Qvalue(v.exo_id, temp_periods-(options_.first_obs-1)) = v.value^2;
     end
     for k=1:length(M_.heteroskedastic_shocks.Qscale_orig)
         v = M_.heteroskedastic_shocks.Qscale_orig(k);
-        M_.heteroskedastic_shocks.Qscale(v.exo_id, v.periods) = v.scale^2;
+        temp_periods=v.periods(v.periods<=options_.nobs+options_.first_obs);
+        M_.heteroskedastic_shocks.Qscale(v.exo_id, temp_periods-(options_.first_obs-1)) = v.scale^2;
     end
 
     if any(any(~isnan(M_.heteroskedastic_shocks.Qvalue) & ~isnan(M_.heteroskedastic_shocks.Qscale)))
diff --git a/matlab/kalman/get_Qvec_heteroskedastic_filter.m b/matlab/kalman/get_Qvec_heteroskedastic_filter.m
index b87d035e69c6fea8d396e3babbb02fec872b995b..004870a9033c641801bba03a594cba2a76208b00 100644
--- a/matlab/kalman/get_Qvec_heteroskedastic_filter.m
+++ b/matlab/kalman/get_Qvec_heteroskedastic_filter.m
@@ -1,4 +1,30 @@
 function Qvec=get_Qvec_heteroskedastic_filter(Q,smpl,Model)
+% function Qvec=get_Qvec_heteroskedastic_filter(Q,smpl,Model)
+% 
+% INPUTS
+%   Q:      baseline non-heteroskadastic covariance matrix of shocks
+%   smpl:   scalar storing end of sample
+%   Model:  structure storing the model information
+% Outputs:
+%   Qvec:   [n_exo by n_exo by smpl] array of covariance matrices
+
+% Copyright (C) 2020-21 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 <https://www.gnu.org/licenses/>.
+
 isqdiag = all(all(abs(Q-diag(diag(Q)))<1e-14)); % ie, the covariance matrix is diagonal...
 Qvec=repmat(Q,[1 1 smpl+1]);
 for k=1:smpl
diff --git a/matlab/optimization/dynare_minimize_objective.m b/matlab/optimization/dynare_minimize_objective.m
index 0311e91c5c11fd17d913e34e3d72193f6785d116..06ecb9be9c59e8e3ba1eff62f1fa399c834d6995 100644
--- a/matlab/optimization/dynare_minimize_objective.m
+++ b/matlab/optimization/dynare_minimize_objective.m
@@ -26,7 +26,7 @@ function [opt_par_values,fval,exitflag,hessian_mat,options_,Scale,new_rat_hess_i
 %   none.
 %
 %
-% Copyright (C) 2014-2019 Dynare Team
+% Copyright (C) 2014-2021 Dynare Team
 %
 % This file is part of Dynare.
 %
@@ -43,7 +43,9 @@ function [opt_par_values,fval,exitflag,hessian_mat,options_,Scale,new_rat_hess_i
 % You should have received a copy of the GNU General Public License
 % along with Dynare.  If not, see <https://www.gnu.org/licenses/>.
 
-
+if isfield(options_,'occbin') && ((options_.occbin.smoother.status && options_.occbin.smoother.inversion_filter) || (options_.occbin.likelihood.status && options_.occbin.likelihood.inversion_filter))
+    warning('off','MATLAB:nearlySingularMatrix')    
+end
 %% set bounds and parameter names if not already set
 n_params=size(start_par_value,1);
 if isempty(bounds)
@@ -650,6 +652,10 @@ switch minimizer_algorithm
     end
 end
 
+if isfield(options_,'occbin') && ((options_.occbin.smoother.status && options_.occbin.smoother.inversion_filter) || (options_.occbin.likelihood.status && options_.occbin.likelihood.inversion_filter))
+    warning('on','MATLAB:nearlySingularMatrix')
+end
+
 end
 
 function [LB, UB]=set_bounds_to_finite_values(bounds, huge_number)
diff --git a/tests/Makefile.am b/tests/Makefile.am
index 527d4cdd229b172a74fb121ce1e3a72d5d15e593..c186004b96e2af4fe447c395ffac618c91100b95 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -60,6 +60,7 @@ MODFILES = \
 	estimation/MH_recover/fs2000_recover_3.mod \
 	estimation/heteroskedastic_shocks/fs2000_het.mod \
 	estimation/heteroskedastic_shocks/fs2000_het_corr.mod \
+	estimation/heteroskedastic_shocks/fs2000_het_sample_restriction.mod \
 	estimation/t_proposal/fs2000_student.mod \
 	estimation/tune_mh_jscale/fs2000.mod \
 	estimation/method_of_moments/AnScho/AnScho_matched_moments.mod \
diff --git a/tests/estimation/heteroskedastic_shocks/fs2000_het_sample_restriction.mod b/tests/estimation/heteroskedastic_shocks/fs2000_het_sample_restriction.mod
new file mode 100644
index 0000000000000000000000000000000000000000..d0c6509b3050a6a06b4cbb347192e5dfdef4568f
--- /dev/null
+++ b/tests/estimation/heteroskedastic_shocks/fs2000_het_sample_restriction.mod
@@ -0,0 +1,34 @@
+// See fs2000.mod in the examples/ directory for details on the model
+// tests heteroskedastic filter/smoother
+// includes lagged exogenous variable introduced by preprocessor
+
+@#include "fs2000_het_model.inc" 
+
+shocks;
+var e_a; stderr 0.014;
+var e_m; stderr 0.005;
+end;
+
+steady;
+
+check;
+stoch_simul(order=1,loglinear);
+forecast;
+
+options_.solve_tolf = 1e-12;
+
+heteroskedastic_shocks;
+  var e_b;
+  periods 100:120;
+  values 0.01;
+
+  var e_a;
+  periods 100:120;
+  scales 0;
+end;
+
+estimation(order=1,datafile='../fsdat_simul',first_obs=10,nobs=182,mode_compute=5,loglinear,mh_replic=0,smoother,filtered_vars,forecast=8,filter_step_ahead=[1:8],consider_all_endogenous,heteroskedastic_filter);
+
+if M_.heteroskedastic_shocks.Qscale(strmatch('e_a',M_.exo_names,'exact'),91)~=0 && M_.heteroskedastic_shocks.Qscale(strmatch('e_b',M_.exo_names,'exact'),91)~=0.01
+    error('first_obs is incorrectly handled.')
+end
\ No newline at end of file