KalmanFilter.ks: changed order to p+q, simplified the error. Now getting RangeError for unknown reason.