@@ 112,7 112,7 @@ impl Timestamp {
             // 2^64 ms is 584 million years, so, *probably* not a concern.
             Timestamp::Time(t) => Timestamp::Time(t + ms),
             // TODO: Does a Reset mean "restart at 0", or noop?
-            Timestamp::Reset => Timestamp::Time(ms),
+            Timestamp::Reset => Timestamp::Time(0),
         }
     }
 }

          
@@ 209,14 209,16 @@ fn update_sensors(t: Timestamp, sensors:
         lzn.accel(t, imu.accel);
     }
 }
-fn update_localization(_lzn: &mut Localization) {}
+fn update_localization(t: Timestamp, lzn: &mut Localization) {
+    lzn.get_state(t);
+}
 fn update_control() {}
 fn update_vehicle() {}
 
 /// A clock that ticks in real time,  or something close,
 /// using the system monotonic time.
 pub struct RtClock {
-    last_update: Instant,
+    next_update: Instant,
     last_timestamp: Timestamp,
     rate: Rate,
 }

          
@@ 224,7 226,7 @@ pub struct RtClock {
 impl RtClock {
     pub fn new() -> Self {
         Self {
-            last_update: Instant::now(),
+            next_update: Instant::now(),
             last_timestamp: Timestamp::Reset,
             // We run our main loop at 250 hz
             // This might mean busy-waiting a fair bit, but for now that's okay.

          
@@ 234,15 236,13 @@ impl RtClock {
 
     pub fn should_tick(&self) -> bool {
         let now = Instant::now();
-        let dt = now - self.last_update;
-        let ms = (1000.0 / self.rate.0).round() as u64;
-        ms >= (dt.as_millis() as u64)
+        now >= self.next_update
     }
 
     pub fn tick(&mut self) -> Timestamp {
         let ms = (1000.0 / self.rate.0).round() as u64;
-        self.last_update += Duration::from_millis(ms);
-        self.last_timestamp.incr(ms);
+        self.next_update += Duration::from_millis(ms);
+        self.last_timestamp = self.last_timestamp.incr(ms);
         self.last_timestamp
     }
 

          
@@ 266,7 266,9 @@ fn mainloop() {
     loop {
         while clock.should_tick() {
             let t = clock.tick();
+            println!("Clock tick at {:?}", t);
             update_sensors(t, sensors, lzn);
+            update_localization(t, lzn);
             update_control();
             update_vehicle();
         }