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