Augmented Kalman filter