def pf_gpsUpdate(self, z): R = self.gps_r obs_from_state = blas.dgemv(np.array([self.gps_h]), self._x_) x_pdf = misctools.mvnpdf(np.array([z]), obs_from_state, np.array([R])) self.weights *= x_pdf self.weights /= self.weights.sum() self.x = self._x_ x_mean, self._P_ = misctools.sample_mn_cv(self.x, self.weights) self.P = self._P_ self.UPDATED = True
def pf_dvlUpdate(self, z, velocity_respect_to = 'bottom'): if velocity_respect_to == 'bottom': R = self.dvl_bottom_r else: R = self.dvl_water_r obs_from_state = blas.dgemv(np.array([self.dvl_h]), self._x_) x_pdf = misctools.mvnpdf(np.array([z]), obs_from_state, np.array([R])) self.weights *= x_pdf self.weights /= self.weights.sum() self.x = self._x_ x_mean, self._P_ = misctools.sample_mn_cv(self.x, self.weights) self.P = self._P_ self.UPDATED = True