Update Kalman.h

Fixed issue with Kalman filter P Covariance Matrix calculation