| 106 | |
| 107 | |
| 108 | Bayes_base::Float |
| 109 | General_LzUnAd_observe_model::Likelihood_uncorrelated::L(const Uncorrelated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const |
| 110 | /* Definition of likelihood given an additive Gaussian observation model: |
| 111 | * p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z)); |
| 112 | * L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size |
| 113 | * Precond: Observation Information: z,Zv_inv,detZterm |
| 114 | */ |
| 115 | { |
| 116 | if (!zset) |
| 117 | Bayes_base::error (Logic_exception("General_observe_model used without Lz set")); |
| 118 | // Normalised innovation |
| 119 | zInnov = z; |
| 120 | model.normalise (zInnov, zp); |
| 121 | FM::noalias(zInnov) -= zp; |
| 122 | |
| 123 | // Likelihood w of observation z given particular state xi is true state |
| 124 | // The state, xi, defines a predicted observation with a Gaussian |
| 125 | // distribution with variance Zd. Thus, the likelihood can be determined directly from the Gaussian |
| 126 | |
| 127 | FM::Vec::iterator zi = zInnov.begin(), zi_end = zInnov.end(); |
| 128 | for (; zi != zi_end; ++zi) { |
| 129 | *zi *= *zi; |
| 130 | } |
| 131 | Float logL = FM::inner_prod(zInnov, Zv_inv); |
| 132 | |
| 133 | using namespace std; |
| 134 | return exp(Float(-0.5)*(logL + logdetZ)); |
| 135 | } |
| 136 | |
| 137 | void General_LzUnAd_observe_model::Likelihood_uncorrelated::Lz (const Uncorrelated_additive_observe_model& model) |
| 138 | /* Set the observation zz and Zv about which to evaluate the Likelihood function |