public override void func_hi_noisy(Vector yi_true, Vector xp_true)
{
func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi_true, xp_true);
hi_noisyRES.Put(0, SceneLib.SampleNormal(hiRES[0], ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).SD_IMAGE_SIMULATION, rnd));
hi_noisyRES.Put(1, SceneLib.SampleNormal(hiRES[1], ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).SD_IMAGE_SIMULATION, rnd));
}
/// <summary>
/// Noisy process equation for simulation
/// Simply perturb xv with Gaussian noise and send it through func_fv
/// </summary>
/// <param name="xv_true"></param>
/// <param name="u_true"></param>
/// <param name="delta_t"></param>
public override void func_fv_noisy(Vector xv_true, Vector u_true, float delta_t)
{
Vector xv_noisy = xv_true;
// Linear velocity
for (int row = 7; row < 10; row++)
{
xv_noisy[row] = SceneLib.SampleNormal(xv_true[row], SD_A_component * delta_t, rnd);
}
// Angular velocity
for (int row = 10; row < 13; row++)
{
xv_noisy[row] = SceneLib.SampleNormal(xv_true[row], SD_alpha_component * delta_t, rnd);
}
// Now send through normal process equaion
func_fv_and_dfv_by_dxv(xv_noisy, u_true, delta_t);
// And copy result
fv_noisyRES.Update(fvRES);
}