Exemple #1
0
        /// <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());
        }