| 510 | |
| 511 | |
| 512 | void SIR_kalman_scheme::roughen_correlated (ColMatrix& P, Float K) |
| 513 | /* Roughening |
| 514 | * Uses a roughening noise based on covariance of P |
| 515 | * This is a more sophisticated algorithm then Ref[1] as it takes |
| 516 | * into account the correlation of P |
| 517 | * K is scaling factor for roughening noise |
| 518 | * Numerical colapse of P |
| 519 | * Numerically when covariance of P semi definite (or close), X's UdU factorisation |
| 520 | * may be negative. |
| 521 | * Exceptions: |
| 522 | * Bayes_filter_exception due collapse of P |
| 523 | * unchanged: P |
| 524 | */ |
| 525 | { |
| 526 | using namespace std; |
| 527 | // Scale variance by constant and state dimensions |
| 528 | Float VarScale = sqr(K) * pow (Float(P.size2()), Float(-2.)/Float(x_size)); |
| 529 | |
| 530 | update_statistics(); // Estimate sample mean and covariance |
| 531 | |
| 532 | // Decorrelate states |
| 533 | Matrix UD(x_size,x_size); |
| 534 | Float rcond = UdUfactor (UD, X); |
| 535 | rclimit.check_PSD(rcond, "Roughening X not PSD"); |
| 536 | |
| 537 | // Sampled predict model for roughening |
| 538 | FM::identity (roughen_model.Fx); |
| 539 | // Roughening predict based on scaled variance |
| 540 | UdUseperate (roughen_model.G, roughen_model.q, UD); |
| 541 | roughen_model.q *= VarScale; |
| 542 | roughen_model.init_GqG(); |
| 543 | // Predict using roughening model |
| 544 | predict (roughen_model); |
| 545 | } |
| 546 | |
| 547 | |
| 548 | }//namespace |