| 261 | } |
| 262 | |
| 263 | Bayes_base::Float |
| 264 | UD_scheme::observe (Linrz_uncorrelated_observe_model& h, const Vec& z) |
| 265 | /* Standard linrz observe |
| 266 | * Uncorrelated observations are applied sequentially in the order they appear in z |
| 267 | * The sequential observation updates state x |
| 268 | * Therefore the model of each observation needs to be computed sequentially. Generally this |
| 269 | * is inefficient and observe (UD_sequential_observe_model&) should be used instead |
| 270 | * Precond: |
| 271 | * UD |
| 272 | * Zv is PSD |
| 273 | * Postcond: |
| 274 | * UD is PSD |
| 275 | * Return: Minimum rcond of all sequential observe |
| 276 | */ |
| 277 | { |
| 278 | const std::size_t z_size = z.size(); |
| 279 | Float s, S; // Innovation and covariance |
| 280 | |
| 281 | // Dynamic sizing |
| 282 | observe_size (z_size); |
| 283 | // Apply observations sequentially as they are decorrelated |
| 284 | Float rcondmin = std::numeric_limits<Float>::max(); |
| 285 | for (std::size_t o = 0; o < z_size; ++o) |
| 286 | { |
| 287 | // Observation model, extracted for a single z element |
| 288 | const Vec& zp = h.h(x); |
| 289 | h.normalise(znorm = z, zp); |
| 290 | noalias(h1) = row(h.Hx, o); |
| 291 | // Check Z precondition |
| 292 | if (h.Zv[o] < 0) |
| 293 | error (Numeric_exception("Zv not PSD in observe")); |
| 294 | // Update UD and extract gain |
| 295 | Float rcond = observeUD (w, S, h1, h.Zv[o]); |
| 296 | rclimit.check_PSD(rcond, "S not PD in observe"); // -1 implies S singular |
| 297 | if (rcond < rcondmin) rcondmin = rcond; |
| 298 | // State update using normalised non-linear innovation |
| 299 | s = znorm[o] - zp[o]; |
| 300 | noalias(x) += w * s; |
| 301 | // Copy s and Sd |
| 302 | UD_scheme::s[o] = s; |
| 303 | UD_scheme::Sd[o] = S; |
| 304 | } |
| 305 | return rcondmin; |
| 306 | } |
| 307 | |
| 308 | Bayes_base::Float |
| 309 | UD_scheme::observe (Linrz_correlated_observe_model& /*h*/, const Vec& /*z*/) |
no test coverage detected