logs and some delay compensation stuff

estimation/__init__.py

@@ -8,14 +8,15 @@
 from vicon_t import vicon_pos_t
 from ukf import UnscentedKalmanFilter
 from ekf import ExtendedKalmanFilter
-from models import DoubleIntegrator
+from models import DoubleIntegrator, Crazyflie2
 from transforms import angularvel2rpydot, body2world, quat2rpy, world2body
 
 
 class StateEstimator():
 
 	def __init__(self, listen_to_vicon=False, publish_to_lcm=False, 
-				 use_rpydot=False, use_ekf=False, use_ukf=False):
+				 use_rpydot=False, use_ekf=False, use_ukf=False,
+				 delay_comp=False):
 
 		self._tvlqr_counting = False
 		self._last_time_update = time.time()
@@ -49,6 +50,11 @@ def __init__(self, listen_to_vicon=False, publish_to_lcm=False,
 
 		self._last_input = [0.0, 0.0, 0.0, 0.0]
 
+		self._delay_comp = delay_comp
+		if delay_comp:
+			self._cf_model = Crazyflie2()
+			self._delay = 0.02 # delay in the control loop in seconds
+
 		self._use_ekf = use_ekf
 		self._use_ukf = use_ukf
 		self._use_kalman = use_ekf or use_ukf
@@ -153,6 +159,9 @@ def get_xhat(self):
 					self._last_dxyz[0],self._last_dxyz[1],self._last_dxyz[2],
 					self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]
 
+		if self._delay_comp:
+			xhat = self._cf_model.simulate(xhat,self._last_input,self._delay)
+
 		if self._use_rpydot:
 			try:
 				xhat[9:12] = angularvel2rpydot(self._last_rpy, body2world(self._last_rpy, self._last_gyro))

estimation/models.py

@@ -125,7 +125,7 @@ def dynamics(self, state, control_input):
     def simulate(self, x0, u0, tspan):
         """ simulates the input u0 as a zero-order hold starting at x0 """
 
-        dt = 0.010 # time-step size in ms
+        dt = 0.010 # time-step size in seconds
         nstep = int(tspan/dt)
         x = array(x0)
         for n in range(nstep):

lcm/crazyflie_t.lcm

@@ -90,4 +90,9 @@ struct kalman_out_t
 
   double smooth_xyz_noblackout[3];
   double smooth_dxyz_noblackout[3];
+}
+
+struct vortex_sensor_t
+{
+  double sensor1;
 }

lcm/crazyflie_t/__init__.py

@@ -14,3 +14,4 @@
 from .dxyz_compare_t import dxyz_compare_t
 from .kalman_args_t import kalman_args_t
 from .kalman_out_t import kalman_out_t
+from .vortex_sensor_t import vortex_sensor_t

lcm/crazyflie_t/vortex_sensor_t.py

@@ -0,0 +1,57 @@
+"""LCM type definitions
+This file automatically generated by lcm.
+DO NOT MODIFY BY HAND!!!!
+"""
+
+try:
+    import cStringIO.StringIO as BytesIO
+except ImportError:
+    from io import BytesIO
+import struct
+
+class vortex_sensor_t(object):
+    __slots__ = ["sensor1"]
+
+    def __init__(self):
+        self.sensor1 = 0.0
+
+    def encode(self):
+        buf = BytesIO()
+        buf.write(vortex_sensor_t._get_packed_fingerprint())
+        self._encode_one(buf)
+        return buf.getvalue()
+
+    def _encode_one(self, buf):
+        buf.write(struct.pack(">d", self.sensor1))
+
+    def decode(data):
+        if hasattr(data, 'read'):
+            buf = data
+        else:
+            buf = BytesIO(data)
+        if buf.read(8) != vortex_sensor_t._get_packed_fingerprint():
+            raise ValueError("Decode error")
+        return vortex_sensor_t._decode_one(buf)
+    decode = staticmethod(decode)
+
+    def _decode_one(buf):
+        self = vortex_sensor_t()
+        self.sensor1 = struct.unpack(">d", buf.read(8))[0]
+        return self
+    _decode_one = staticmethod(_decode_one)
+
+    _hash = None
+    def _get_hash_recursive(parents):
+        if vortex_sensor_t in parents: return 0
+        tmphash = (0x154b3a526e452642) & 0xffffffffffffffff
+        tmphash  = (((tmphash<<1)&0xffffffffffffffff)  + (tmphash>>63)) & 0xffffffffffffffff
+        return tmphash
+    _get_hash_recursive = staticmethod(_get_hash_recursive)
+    _packed_fingerprint = None
+
+    def _get_packed_fingerprint():
+        if vortex_sensor_t._packed_fingerprint is None:
+            vortex_sensor_t._packed_fingerprint = struct.pack(">Q", vortex_sensor_t._get_hash_recursive([]))
+        return vortex_sensor_t._packed_fingerprint
+    _get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
+

logs/cf2/sysid0331/times.txt

@@ -0,0 +1,5 @@
+53.66 54.45
+17.95 19.88
+102.3 107
+25.63 27.9
+41.42 43.27

simpleclient.py

@@ -4,16 +4,17 @@
 ############################ CLIENT OPTIONS ##########################################
 TXRX_FREQUENCY = 1000.0
 STARTUP_NANOKONTROL = True
-USE_DRAKE_CONTROLLER = True
+USE_DRAKE_CONTROLLER = False
 
-SE_LISTEN_TO_VICON = True
+SE_LISTEN_TO_VICON = False
 SE_PUBLISH_TO_LCM = True
-SE_USE_RPYDOT = True
-SE_USE_EKF = True
+SE_USE_RPYDOT = False
+SE_USE_EKF = False
 SE_USE_UKF = False
+SE_DELAY_COMP = True
 
 CTRL_INPUT_TYPE = 'omegasqu'
-CTRL_LISTEN_TO_LCM = True
+CTRL_LISTEN_TO_LCM = False
 CTRL_LISTEN_TO_EXTRA_INPUT = True
 CTRL_PUBLISH_TO_LCM = False
 ######################################################################################
@@ -67,7 +68,8 @@ def _connected(self, link_uri):
                                                publish_to_lcm=SE_PUBLISH_TO_LCM,
                                                use_rpydot=SE_USE_RPYDOT,
                                                use_ekf=SE_USE_EKF,
-                                               use_ukf=SE_USE_UKF)
+                                               use_ukf=SE_USE_UKF,
+                                               delay_comp=SE_DELAY_COMP)
 
         # controller
         self._control_input_updated_flag = Event()

testdynamics.py → tests/testdynamics.py

@@ -3,9 +3,11 @@
 # note the state is angular vels
 # 0.1576,0.9706,0.9572 -> -0.1160,0.9763,0.9418
 # xx = [0.9058,0.1270,0.9134,0.6324,0.0975,0.2785,0.5469,0.9575,0.9649,-0.1160,0.9763,0.9418]
-xx = [0.9157,0.7922,0.9595,0.6557,0.0357,0.8491,0.9340,0.6787,0.7577,0.1962,0.8166,0.6289]
+# xx = [0.9157,0.7922,0.9595,0.6557,0.0357,0.8491,0.9340,0.6787,0.7577,0.1962,0.8166,0.6289]
+xx = [0.6088,0.9580,0.0954,0.0356,0.8862,0.2469,0.0089,0.8149,0.1405,0.5162,0.2285,-0.3291]
 
-uu = [0.4854,0.8003,0.1419,0.4218]
+#uu = [0.4854,0.8003,0.1419,0.4218]
+uu = [0.4334,0.2442,0.4290,0.0102]
 
 cf = Crazyflie2()