working kalman in corner planes, slight tweaks for performance