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
def prediction(self, u, t):
"""
Predict the current state and covariance matrix using control input
"""
if self.UPDATED:
# Resample
self.resample()
self.UPDATED = False
trans_mat, sc_process_noise = self.trans_matrices(u, t)
self._x_ = blas.dgemv(np.array([trans_mat]), self.x)
awg_noise = np.random.multivariate_normal(np.zeros(self.ndims), sc_process_noise, self.nparticles)
self._x_ += awg_noise
self._P_ = np.dot(np.dot(trans_mat, self.P), trans_mat.T) + sc_process_noise
x_mean, self._P_ = misctools.sample_mn_cv(self.x, self.weights)
def getStateVector(self):
x_mean, x_cov = misctools.sample_mn_cv(self.x, self.weights)
self.P = x_cov
return x_mean
