diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py b/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py index 0cea963168..0459e234b2 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py @@ -127,7 +127,8 @@ def inverse_kinematics(self, ref_ee_pose, plot=False): if plot: self.fig = plt.figure() - self.ax = Axes3D(self.fig) + self.ax = Axes3D(self.fig, auto_add_to_figure=False) + self.fig.add_axes(self.ax) x_list = [] y_list = [] diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 17051623ca..ba54a3d12b 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -96,10 +96,10 @@ def calc_covariance(x_est, px, pw): calculate covariance matrix see ipynb doc """ - cov = np.zeros((3, 3)) + cov = np.zeros((4, 4)) n_particle = px.shape[1] for i in range(n_particle): - dx = (px[:, i:i + 1] - x_est)[0:3] + dx = (px[:, i:i + 1] - x_est) cov += pw[0, i] * dx @ dx.T cov *= 1.0 / (1.0 - pw @ pw.T) diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 8e4d166c4e..0575353a11 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -2,7 +2,7 @@ numpy == 1.26.4 scipy == 1.13.0 matplotlib == 3.8.4 cvxpy == 1.4.3 -pytest == 8.1.1 # For unit test -pytest-xdist == 3.5.0 # For unit test -mypy == 1.9.0 # For unit test -ruff == 0.4.1 # For unit test +pytest == 8.2.0 # For unit test +pytest-xdist == 3.6.1 # For unit test +mypy == 1.10.0 # For unit test +ruff == 0.4.2 # For unit test