compute Kalman gain
D = Dh@B@Dh.T + R
K=B@ Dh.T @ np.linalg.inv(D)