WIP: Major redesign of yaw rate controller.

This commit implements a LQR/LQG controller/estimator for the four bicycle
states. To be implemented next is a yaw rate PI controller as an outer loop so
that tracking yaw rate commands is possible.