| 46 | } |
| 47 | |
| 48 | void Unscented_scheme::unscented (FM::ColMatrix& XX, const FM::Vec& x, const FM::SymMatrix& X, Float scale) |
| 49 | /* |
| 50 | * Generate the Unscented point representing a distribution |
| 51 | * Fails if scale is negative |
| 52 | */ |
| 53 | { |
| 54 | UTriMatrix Sigma(x_size,x_size); |
| 55 | |
| 56 | // Get a upper Cholesky factorisation |
| 57 | Float rcond = UCfactor(Sigma, X); |
| 58 | rclimit.check_PSD(rcond, "X not PSD"); |
| 59 | Sigma *= std::sqrt(scale); |
| 60 | |
| 61 | // Generate XX with the same sample Mean and Covariance as before |
| 62 | column(XX,0) = x; |
| 63 | |
| 64 | for (std::size_t c = 0; c < x_size; ++c) { |
| 65 | UTriMatrix::Column SigmaCol = column(Sigma,c); |
| 66 | column(XX,c+1) .assign (x + SigmaCol); |
| 67 | column(XX,x_size+c+1) .assign (x - SigmaCol); |
| 68 | } |
| 69 | } |
| 70 | |
| 71 | Unscented_scheme::Float Unscented_scheme::predict_Kappa (std::size_t size) const |
| 72 | // Default Kappa for predict: state augmented with predict noise |