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
