Error state Kalman Filter from MATLAB to Python
In the link you provided under kalman equations, transpose of S is inverted to calculate kalman gain.
It seems like you didn't take the transpose of S before inverting it. in the line :
K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S)))
it should be
K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S.transpose())))