Extended Kalman Filter using PGA Motor algebra - View it on GitHub