fixed one problem and added test

wetb/control/control.py

@@ -42,7 +42,6 @@ class Control(object):
 
         Returns
         -------
-
         kp: float
             Proportional gain [rad/(rad/s)]
         ki: float
@@ -54,11 +53,11 @@ class Control(object):
 
 
         """
-        pitch *= np.pi/180.
-        I *= 1e-3
-        dQdt *= 180./np.pi
-        Omr *= np.pi/30.
-        om *= 2.*np.pi
+        pitch = pitch * np.pi/180.
+        I = I * 1e-3
+        dQdt = dQdt * 180./np.pi
+        Omr = Omr * np.pi/30.
+        om = om * 2.*np.pi
 
         # Quadratic fitting of dQdt
         A = np.ones([dQdt.shape[0], 3])
@@ -73,8 +72,8 @@ class Control(object):
         kp = -(2*csi*om*I[0] - P/(Omr**2))/x[2]
         ki = -(om**2*I[0])/x[2]
 
-        K1 = x[2]/x[0] * (180./np.pi)
-        K2 = x[2]/x[1] * (180./np.pi)**2
+        K1 = x[2]/x[1]*(180./np.pi)
+        K2 = x[2]/x[0]*(180./np.pi)**2
 
         return kp, ki, K1, K2
 

wetb/control/tests/test_control.py

@@ -61,10 +61,10 @@ class TestControl(unittest.TestCase):
                                                      self.dQdt[i:],
                                                      P, Omr, om, csi)
 
-        self.assertEqual(kp, 1.5960902436444313)
-        self.assertEqual(ki, 0.71632362627138402)
-        self.assertEqual(K1, 10.463887621856747)
-        self.assertEqual(K2, 573.59471652017498)
+        self.assertEqual(kp, 1.596090243644432)
+        self.assertEqual(ki, 0.71632362627138424)
+        self.assertEqual(K1, 10.01111637532056)
+        self.assertEqual(K2, 599.53659803157643)
 
 if __name__ == "__main__":