예제 #1
0
    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
예제 #2
0
 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
예제 #3
0
 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
예제 #4
0
 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)