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