[Bayes++] Exception "S not PD in observe" using correlated noise

[Nicola B. <nb...@es...>]

Hi,
I've implemented an EKF, where I have a linearized observation model (Linrz_correlated_observe_model) with correlated noise. The covariance matrix of such a noise is the following:

//      | *  0  0  0  0  0  0  0  0 |
//      | 0  *  0  0  0  0  0  0  0 |
//      | 0  0  *  0  0  0  0  0  0 |
//      | 0  0  0  *  0  0  0  0  0 |
// Z =  | 0  0  0  0  *  0  0  0  0 |
//      | 0  0  0  0  0  *  *  0  0 |
//      | 0  0  0  0  0  *  *  0  0 |
//      | 0  0  0  0  0  0  0  *  * |
//      | 0  0  0  0  0  0  0  *  * |

   Z.clear();
   Z(0,0) = VAR_alpha;
   Z(1,1) = VAR_beta;
   Z(2,2) = VAR_r;
   Z(3,3) = VAR_v;
   Z(4,4) = VAR_omega;
   Z(5,5) = VAR_psi;
   Z(7,7) = VAR_theta;
   Z(5,6) = VAR_psi / 0.01;
   Z(6,5) = Z(5,6); // probably not necessary because symmetric
   Z(6,6) = 2 * VAR_psi / 0.01;
   Z(7,8) = VAR_theta / 0.01;
   Z(8,7) = Z(7,8); // probably not necessary because symmetric
   Z(8,8) = 2 * VAR_theta / 0.01;


Unfortunately, at runtime, when I call the "Covariance_scheme::observe(Linrz_correlated_observe_model &h, const FM::Vec &z)" function I have the following exception:

terminate called after throwing an instance of 'Bayesian_filter::Numeric_exception'
  what():  S not PD in observe

I've noticed that removing the off-diagonal elements of Z, Z(5,6) and Z(7,8), there are no exceptions, but of course this is not a solution.
I guess it's some mathematical error, but checking inside the Bayes++ code, I cannot understand what's wrong and how to fix it. Any suggestion?
Thanks,
Nicola

-- 
------------------------------------------
Nicola Bellotto
University of Essex
Department of Computer Science
Wivenhoe Park
Colchester CO4 3SQ
United Kingdom

Room: 1N1.2.8
Tel. +44 (0)1206 874094
E-Mail: nb...@es...
URL: http://privatewww.essex.ac.uk/~nbello
------------------------------------------