| 91 | } |
| 92 | |
| 93 | Bayes_base::Float |
| 94 | Covariance_scheme::observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s) |
| 95 | /* Correlated innovation observe |
| 96 | */ |
| 97 | { |
| 98 | // Size consistency, z to model |
| 99 | if (s.size() != h.Z.size1()) |
| 100 | error (Logic_exception("observation and model size inconsistent")); |
| 101 | observe_size (s.size());// Dynamic sizing |
| 102 | |
| 103 | // Innovation covariance |
| 104 | Matrix temp_XZ (prod(X, trans(h.Hx))); |
| 105 | noalias(S) = prod(h.Hx, temp_XZ) + h.Z; |
| 106 | |
| 107 | // Inverse innovation covariance |
| 108 | Float rcond = UdUinversePD (SI, S); |
| 109 | rclimit.check_PD(rcond, "S not PD in observe"); |
| 110 | |
| 111 | // Kalman gain, X*Hx'*SI |
| 112 | noalias(W) = prod(temp_XZ, SI); |
| 113 | |
| 114 | // State update |
| 115 | noalias(x) += prod(W, s); |
| 116 | noalias(X) -= prod_SPD(W, S, temp_XZ); |
| 117 | |
| 118 | return rcond; |
| 119 | } |
| 120 | |
| 121 | |
| 122 | Bayes_base::Float |
nothing calls this directly
no test coverage detected