Properly terminate nonlinear_kalman_filter if non-positive definite matrices are encountered

Otherwise, crashes can happen during mode-finding
rm-particles^2
Johannes Pfeifer 2017-01-23 12:20:24 +01:00 committed by GitHub
parent 285d5c1711
commit ddc5b5b665
1 changed files with 12 additions and 2 deletions

View File

@ -152,8 +152,18 @@ for t=1:sample_size
KalmanFilterGain = PredictedStateAndObservedCovariance/PredictedObservedVariance;
StateVectorMean = PredictedStateMean + KalmanFilterGain*PredictionError;
StateVectorVariance = PredictedStateVariance - KalmanFilterGain*PredictedObservedVariance*KalmanFilterGain';
StateVectorVarianceSquareRoot = chol(StateVectorVariance)';
PredictedObservedVarianceSquareRoot = chol(PredictedObservedVariance)' ;
[StateVectorVarianceSquareRoot, p]= chol(StateVectorVariance,'lower');
if p
LIK=-Inf;
lik(t)=-Inf;
return
end
[PredictedObservedVarianceSquareRoot, p]= chol(PredictedObservedVariance,'lower');
if p
LIK=-Inf;
lik(t)=-Inf;
return
end
end
lik(t) = log( probability2(0,PredictedObservedVarianceSquareRoot,PredictionError) ) ;
end