compute Kalman gain
D = Dh@B@Dh.T + R
K = B @ Dh @ np.linalg.inv(D)
wi = np.zeros([m,N])
uai = np.zeros([n,N])
for i in range(N):