def kf_gpsUpdate(self, z): ### gps update with z = [north, east] wrt world frame ### R = self.gps_r upd_state, upd_covariance, kalman_info = \ gmphdfilter.blas_kf_update(np.array([self._x_]), np.array([self._P_]), np.array([self.gps_h]), np.array([R]), z, False) self.x = upd_state[0] self.P = upd_covariance[0]
def kf_dvlUpdate(self, z, velocity_respect_to = 'bottom'): if velocity_respect_to == 'bottom': R = self.dvl_bottom_r else: R = self.dvl_water_r try: upd_state, upd_covariance, kalman_info = \ gmphdfilter.blas_kf_update(np.array([self._x_]), np.array([self._P_]), np.array([self.dvl_h]), np.array([R]), z, False) self.x = upd_state[0] self.P = upd_covariance[0] except: pass