Current state of the robot

 So currently the robot can be teleoperated via keyboard and scan the room using the lidar. I also connected the IMU and 4 ultrasound sensors. 

Driving the robot around is really fun, but because the wheels are all controlled individually it doesn't go straight. Surprisingly the strafing mode works a lot better than just driving forward. 

I did some digging online and found out that one way to solve this problem is to control the linear and angular velocities individually. 

Below is a graph of what I currently have as a setup on the robot and what I want to have.

At first I didn't want to do this, because I didn't want the kinematics calculations to be on the Arduino. I'm going to have to find a linear algebra library or figure something else out. 

One thing that's kinf of weird is that Twist commands are in Float 64 format, but that doesn't work with ros-serial. It automatically converts it to Float 32 and a loss of precision occurs. 

One thing I'm really excited for is to implement an Extended Kalman filter. I learnt about it in school during my Bachelor's degree and it seems like an advanced concept with a lot of cool applications.