// Set the current innovation covariance S_i^{-1} for this // particle. Called from // Scene_Single::predict_partially_initialised_feature_measurements() public void set_S(MatrixFixed Si) { Cholesky local_Si_cholesky = new Cholesky(Si); m_SInv.Update(local_Si_cholesky.Inverse()); m_detS = SceneLib.Determinant(Si); }
public void update_filter_internal_measurement_slow(Scene_Single scene, uint i) { // Size of measurement vector uint size = ((Internal_Measurement)(scene.internal_measurement_vector[(int)i])).get_internal_measurement_model().MEASUREMENT_SIZE; uint size2 = scene.get_total_state_size(); // Size of state vector Vector x = new Vector(size2); MatrixFixed P = new MatrixFixed(size2, size2); scene.construct_total_state_and_covariance(ref x, ref P); // cout << "x:" << x; // cout << "P:" << P; // 1. Form nu and dh_by_dx Vector nu_tot = new Vector(size); MatrixFixed dh_by_dx_tot = new MatrixFixed(size, size2); MatrixFixed R_tot = new MatrixFixed(size, size); scene.construct_total_internal_measurement_stuff(nu_tot, dh_by_dx_tot, R_tot, i); // 2. Calculate S(k+1) MatrixFixed S = new MatrixFixed(size, size); //MatrixFixed Tempss2 = new MatrixFixed(size, size2); //MatrixFixed Temps2s = new MatrixFixed(size2, size); MatrixFixed dh_by_dx_totT = dh_by_dx_tot.Transpose(); S.Update(dh_by_dx_tot * P * dh_by_dx_totT); S += R_tot; // cout << "R_tot:" << R_tot; // cout << "S:" << S; // cout << "dh_by_dx_tot:" << dh_by_dx_tot; // cout << "dh_by_dx_totT:" << dh_by_dx_totT; // 3. Calculate W(k+1) Cholesky S_cholesky = new Cholesky(S); MatrixFixed W = P * dh_by_dx_totT * S_cholesky.Inverse(); // cout << "W:" << W; // 4. Calculate x(k+1|k+1) x += W * nu_tot; // 5. Calculate P(k+1|k+1) P -= W * S * W.Transpose(); scene.fill_state_and_covariance(x, P); // cout << "x after update:" << x; // cout << "P after update:" << P; }
/// <summary> /// Update the filter in a simple overall way /// </summary> /// <param name="scene"></param> public void total_update_filter_slow(Scene_Single scene) { // Steps to update the total filter: // 1. Form h and dh_by_dx and R(k+1) and z // 2. Calculate S(k+1) // 3. Calculate W(k+1) // 4. Calculate x(k+1|k+1) // 5. Calculate P(k+1|k+1) uint size = scene.get_successful_measurement_vector_size(); // Size of measurement vector uint size2 = scene.get_total_state_size(); // Size of state vector Vector x = new Vector(size2); MatrixFixed P = new MatrixFixed(size2, size2); scene.construct_total_state_and_covariance(ref x, ref P); // 1. Form nu and dh_by_dx Vector nu_tot = new Vector(size); MatrixFixed dh_by_dx_tot = new MatrixFixed(size, size2); MatrixFixed R_tot = new MatrixFixed(size, size); scene.construct_total_measurement_stuff(nu_tot, dh_by_dx_tot, R_tot); // 2. Calculate S(k+1) MatrixFixed temp_matrix = P * dh_by_dx_tot.Transpose(); //pre-calculate to speed up subsequent stuff MatrixFixed S = dh_by_dx_tot * temp_matrix; S += R_tot; // 3. Calculate W(k+1) Cholesky S_cholesky = new Cholesky(S); MatrixFixed W = temp_matrix * S_cholesky.Inverse(); // 4. Calculate x(k+1|k+1) x += W * nu_tot; // 5. Calculate P(k+1|k+1) P -= W * S * W.Transpose(); scene.fill_state_and_covariance(x, P); }
/// <summary> /// Make a measurement of a feature. This function calls elliptical_search() to /// find the best match within three standard deviations of the predicted location. /// </summary> /// <param name="patch">The identifier for this feature (in this case an image patch)</param> /// <param name="z">The best image location match for the feature, to be filled in by this function</param> /// <param name="h">The expected image location</param> /// <param name="S">The expected location covariance, used to specify the search region.</param> /// <returns></returns> public override bool measure_feature(byte[] patch, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { Cholesky S_cholesky = new Cholesky(S); MatrixFixed Sinv = S_cholesky.Inverse(); uint u_found = 0, v_found = 0; if (SceneLib.elliptical_search(image, image_width, image_height, patch, patchwidth, patchwidth, h, Sinv, ref u_found, ref v_found, vz, Camera_Constants.BOXSIZE, outputimage, outputimage_width, outputimage_height, show_ellipses, calibrating, rnd) != true) { // Feature not successfully matched return false; } z.Put(0, (float)u_found); z.Put(1, (float)v_found); return true; }