diff --git a/src/main.rs b/src/main.rs
index 6353f2b8295d780f1b63a2b68a5aaec6e523a0bd..07854d9962e348b126cee9dfbd9424bb3e864dc7 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -275,7 +275,9 @@ fn tick_to_speed(tick: i32) -> f32 {
     tick/10.0
 }
 fn update_state(pos: &(f32,f32,f32), l_speed: f32, r_speed: f32) {
-    pos.0 += 0.5;
+    pos.0 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().sin());
+    pos.0 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().cos());
+    pos.2 += (r_speed - l_speed)/AXEL_WIDTH;
 }
 fn update_map_with_lidar(lidar_data: [u8; LIDAR_BUF_SIZE], message_len: usize, map: &mut [[u32; ROWS]; COLS]) {
     map[0][0] += 1;