Add Vision Inputs To Pose Subsystem

[nancyesquivel10]

• vision added to pose estimator
• need to mount camera and pi
• test vision code and pose accuracy