commaai
/
rednose
mirror of https://github.com/commaai/rednose.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

forgot to write this

This commit is contained in:
Harald Schafer 2021-05-05 11:27:25 -07:00
parent d432d0b398
commit 7345677778
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
rednose/helpers/ekf_sym.py
View File

@ -455,6 +455,7 @@ class EKF_sym():
dt = t - self.filter_time dt = t - self.filter_time
assert dt >= 0 assert dt >= 0
self.x, self.P = self._predict(self.x, self.P, dt) self.x, self.P = self._predict(self.x, self.P, dt)
self.normalize_quaternions()
self.filter_time = t self.filter_time = t
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # pylint: disable=dangerous-default-value def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # pylint: disable=dangerous-default-value
@ -514,6 +515,7 @@ class EKF_sym():
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F') extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
# update # update
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i) self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
self.normalize_quaternions()
y.append(y_i) y.append(y_i)
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P) xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)