Build EKF sim

- Start with a linear KF with cartesian updates
- Learn and write up equations for the bicycle model/Ackermann steering
    - https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/BicycleModel.html
- Run an EKF simulation with Ackermann model
- Incorporate third dimension