�[200~fix: avoid double-counting of cross-coupling terms in MPC model

Author: manjeetjha070

src/gem_controllers/mpc_current_controller.py

@@ -156,6 +156,12 @@ def control(self, state, reference):
             u_in=0,  # inputs (or adapt for your motor)
             omega=omega
         )
+        # Local indices for motor states
+        self.local_idx = {name: i for i, name in enumerate(self.motor_state_names)}
+
+        # remove duplicated cross-coupling terms from g
+        g[0] -= self.p * self.l_q / self.l_d * x[self.local_idx['i_sq']]
+        g[1] += self.p * self.l_d / self.l_q * x[self.local_idx['i_sd']]
 
         # Pass everything needed to _simulate_sequence
         _, best_sequence = self._simulate_sequence(