/// <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); }