Finally got time to finish the odometry and basic way point system. It still has a pretty big error, but some more tinkering should fix that.
This is a video of it navigating a set of points in my apartment, only quadrature encoders on the wheels, no other sensors.
