Kalman Update in separate function
included KalmanUpdate

much better suited for multi stream fusion