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