Fixed Beal version of Kalman filter.

Basically didn't use his updates.