diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py
index 46ddd72..335a8db 100644
--- a/Massage/MassageControl/MassageRobot_nova5.py
+++ b/Massage/MassageControl/MassageRobot_nova5.py
@@ -128,7 +128,14 @@ class MassageRobot:
     # 更新步骤
     def kalman_update(self,x_predict, P_predict, z, R):
         # 卡尔曼增益
-        K = P_predict @ np.linalg.inv(P_predict + R)
+        # K = P_predict @ np.linalg.inv(P_predict + R)
+        S = P_predict + R
+        s = np.diag(S)            # shape (6,)
+        p_diag = np.diag(P_predict)
+        K_diag = np.zeros_like(s)
+        nonzero_mask = s != 0
+        K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
+        K = np.diag(K_diag)
         # 更新状态
         x_update = x_predict + K @ (z - x_predict)
         # 更新误差协方差
diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py
index 6034c33..d88784f 100644
--- a/Massage/MassageControl/algorithms/admittance_controller.py
+++ b/Massage/MassageControl/algorithms/admittance_controller.py
@@ -33,6 +33,7 @@ class AdmittanceController(BaseController):
             if damp_rot[i] < 0:
                 damp_rot[i] = 2 * desired_xi * np.sqrt(stiff_rot[i] * mass_rot[i])
         self.M = np.diag(np.concatenate([mass_tran, mass_rot]))
+        self.M_inv = np.linalg.inv(self.M)
         self.K = np.diag(np.concatenate([stiff_tran, stiff_rot]))
         self.D = np.diag(np.concatenate([damp_tran, damp_rot]))
 
@@ -59,7 +60,8 @@ class AdmittanceController(BaseController):
         wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
         # if time.time() - self.laset_print_time > 1:
         #     print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}')
-        self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
+        # self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
+        self.state.arm_desired_acc = self.M_inv @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
         # if time.time() - self.laset_print_time > 5:
         #     print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
         self.clip_command(self.state.arm_desired_acc,"acc")