Skip to content

Commit

Permalink
propogate ruff (#33)
Browse files Browse the repository at this point in the history
  • Loading branch information
jnewb1 authored Aug 17, 2023
1 parent 22f02dd commit 75f545a
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 24 deletions.
2 changes: 1 addition & 1 deletion examples/kinematic_kf.py
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ def __init__(self, generated_dir):
dim_state_err = self.initial_P_diag.shape[0]

# init filter
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
self.filter_func = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)


if __name__ == "__main__":
Expand Down
26 changes: 13 additions & 13 deletions examples/live_kf.py
Original file line number Diff line number Diff line change
Expand Up @@ -258,31 +258,31 @@ def __init__(self, generated_dir):
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}

# init filter
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
self.filter_func = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)

@property
def x(self):
return self.filter.state()
return self.filter_func.state()

@property
def t(self):
return self.filter.filter_time
return self.filter_func.filter_time

@property
def P(self):
return self.filter.covs()
return self.filter_func.covs()

def rts_smooth(self, estimates):
return self.filter.rts_smooth(estimates, norm_quats=True)
return self.filter_func.rts_smooth(estimates, norm_quats=True)

def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
if covs_diag is not None:
P = np.diag(covs_diag)
elif covs is not None:
P = covs
else:
P = self.filter.covs()
self.filter.init_state(state, P, filter_time)
P = self.filter_func.covs()
self.filter_func.init_state(state, P, filter_time)

def predict_and_observe(self, t, kind, data):
if len(data) > 0:
Expand All @@ -294,16 +294,16 @@ def predict_and_observe(self, t, kind, data):
elif kind == ObservationKind.ODOMETRIC_SPEED:
r = self.predict_and_update_odo_speed(data, t, kind)
else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
r = self.filter_func.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))

# Normalize quats
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
quat_norm = np.linalg.norm(self.filter_func.x[3:7, 0])

# Should not continue if the quats behave this weirdly
if not (0.1 < quat_norm < 10):
raise KalmanError("Kalman filter quaternions unstable")

self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
self.filter_func.x[States.ECEF_ORIENTATION, 0] = self.filter_func.x[States.ECEF_ORIENTATION, 0] / quat_norm

return r

Expand All @@ -320,21 +320,21 @@ def predict_and_update_odo_speed(self, speed, t, kind):
R = np.zeros((len(speed), 1, 1))
for i, _ in enumerate(z):
R[i, :, :] = np.diag([0.2**2])
return self.filter.predict_and_update_batch(t, kind, z, R)
return self.filter_func.predict_and_update_batch(t, kind, z, R)

def predict_and_update_odo_trans(self, trans, t, kind):
z = trans[:, :3]
R = np.zeros((len(trans), 3, 3))
for i, _ in enumerate(z):
R[i, :, :] = np.diag(trans[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
return self.filter_func.predict_and_update_batch(t, kind, z, R)

def predict_and_update_odo_rot(self, rot, t, kind):
z = rot[:, :3]
R = np.zeros((len(rot), 3, 3))
for i, _ in enumerate(z):
R[i, :, :] = np.diag(rot[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
return self.filter_func.predict_and_update_batch(t, kind, z, R)


if __name__ == "__main__":
Expand Down
7 changes: 4 additions & 3 deletions pyproject.toml
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml
[tool.ruff]
select = ["E", "F", "W"]
ignore = ["W292", "E741"]
select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"]
ignore = ["W292", "E741", "E402", "C408", "ISC003"]
line-length = 160
target-version="py311"
target-version="py311"
flake8-implicit-str-concat.allow-multiline=false
14 changes: 7 additions & 7 deletions rednose/helpers/kalmanfilter.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,28 +10,28 @@ class KalmanFilter:
Q = np.zeros((0, 0))
obs_noise: Dict[int, Any] = {}

filter = None # Should be initialized when initializating a KalmanFilter implementation
filter_func = None # Should be initialized when initializating a KalmanFilter implementation

@property
def x(self):
return self.filter.state()
return self.filter_func.state()

@property
def t(self):
return self.filter.get_filter_time()
return self.filter_func.get_filter_time()

@property
def P(self):
return self.filter.covs()
return self.filter_func.covs()

def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
if covs_diag is not None:
P = np.diag(covs_diag)
elif covs is not None:
P = covs
else:
P = self.filter.covs()
self.filter.init_state(state, P, filter_time)
P = self.filter_func.covs()
self.filter_func.init_state(state, P, filter_time)

def get_R(self, kind, n):
obs_noise = self.obs_noise[kind]
Expand All @@ -48,4 +48,4 @@ def predict_and_observe(self, t, kind, data, R=None):
if R is None:
R = self.get_R(kind, len(data))

self.filter.predict_and_update_batch(t, kind, data, R)
self.filter_func.predict_and_update_batch(t, kind, data, R)

0 comments on commit 75f545a

Please sign in to comment.