/// <summary>
/// For a single feature, work out the predicted feature measurement and the
/// Jacobians with respect to the vehicle position and the feature position.
/// This calls the appropriate member functions in Feature to set the measurement
/// \vct{h}, the feature location Jacobian \partfracv{h}{y}, robot
/// position Jacobian \partfrac{\vct{h}}{\vct{x}_v}, measurement covariance
/// \mat{R} and innovation covariance \mat{S} respectively.
/// </summary>
/// <param name="sfp"></param>
public void predict_single_feature_measurements(Feature sfp)
{
motion_model.func_xp(xv);
Vector xpRES = motion_model.get_xpRES();
sfp.get_fully_initialised_feature_measurement_model().func_hi_and_dhi_by_dxp_and_dhi_by_dyi(sfp.get_y(),xpRES);
sfp.set_h(sfp.get_fully_initialised_feature_measurement_model().get_hiRES());
sfp.set_dh_by_dy(sfp.get_fully_initialised_feature_measurement_model().get_dhi_by_dyiRES());
motion_model.func_dxp_by_dxv(xv);
sfp.set_dh_by_dxv(sfp.get_fully_initialised_feature_measurement_model().get_dhi_by_dxpRES() * motion_model.get_dxp_by_dxvRES());
sfp.get_fully_initialised_feature_measurement_model().func_Ri(sfp.get_h());
sfp.set_R(sfp.get_fully_initialised_feature_measurement_model().get_RiRES());
sfp.get_fully_initialised_feature_measurement_model().func_Si(Pxx, sfp.get_Pxy(), sfp.get_Pyy(),
sfp.get_dh_by_dxv(), sfp.get_dh_by_dy(), sfp.get_R());
sfp.set_S(sfp.get_fully_initialised_feature_measurement_model().get_SiRES());
}
