def ukf_prediction(self, u, t): _alpha_ = 1e-3 _beta_ = 2.0 _kappa_ = 0 trans_mat, sc_process_noise = self.trans_matrices(u, t) P = self.P + sc_process_noise x = self.x # UKF prediction # Create the Sigma points (x_sigma, x_weight, P_weight) = girona500.createSigmaPoints(x, P, _alpha_, _beta_, _kappa_) # Predict Sigma points and multiply by weight x_sigma_predicted = blas.dgemv(np.array([trans_mat]), x_sigma, beta=0.0) blas.dscal(x_weight, x_sigma_predicted) # Take the weighted mean of the Sigma points to get the predicted mean pred_state = np.add.reduce(x_sigma_predicted) # Generate the weighted Sigma covariance and add Q to get predicted cov pred_cov = girona500.evalSigmaCovariance(P_weight, x_sigma_predicted, pred_state) # + sc_process_noise self._x_ = pred_state self._P_ = pred_cov
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)