有没有办法修正卡尔曼增益矩阵

Question

我试图将扩展卡尔曼滤波器应用于恒定速度恒定航向模型，当我写下卡尔曼增益公式时，我在下面得到了这个错误，但我找不到修复它的方法

TypeError: No loop matching the specified signature and casting was found for ufunc inv

直到K，它工作正常，但当我加上K（kalman增益）时，它产生了误差

for filterstep in range(m):
    
    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    # see "Dynamic Matrix"
    x[0] = x[0] + dt[filterstep]*x[3]*np.cos(x[2])
    x[1] = x[1] + dt[filterstep]*x[3]*np.sin(x[2])
    x[2] = (x[2]+ np.pi) % (2.0*np.pi) - np.pi
    x[3] = x[3]
    
    # Calculate the Jacobian of the Dynamic Matrix A
    # see "Calculate the Jacobian of the Dynamic Matrix with respect to the state vector"
    a13 = -dt[filterstep]*x[3]*np.sin(x[2])
    a14 = dt[filterstep]*np.cos(x[2])
    a23 = dt[filterstep]*x[3]*np.cos(x[2])
    a24 = dt[filterstep]*np.sin(x[2])
    JA = np.matrix([[1.0, 0.0, a13, a14],
                    [0.0, 1.0, a23, a24],
                    [0.0, 0.0, 1.0, 0.0],
                    [0.0, 0.0, 0.0, 1.0]])
    
    
    # Calculate the Process Noise Covariance Matrix
    sGPS     = 0.5*8.8*dt[filterstep]**2  # assume 8.8m/s2 as maximum acceleration
    sCourse  = 2.0*dt[filterstep] # assume 0.5rad/s as maximum turn rate
    sVelocity= 35.0*dt[filterstep] # assume 8.8m/s2 as maximum acceleration

    Q = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2])
    
    # Project the error covariance ahead
    P = JA*P*JA.T + Q
    
    # Measurement Update (Correction)
    # ===============================
    # Measurement Function
    hx = np.matrix([[float(x[0])],
                    [float(x[1])]])

    if GPS[filterstep]: # with 10Hz, every 5th step
        JH = np.matrix([[1.0, 0.0, 0.0, 0.0],
                        [0.0, 1.0, 0.0, 0.0]])
    else: # every other step
        JH = np.matrix([[0.0, 0.0, 0.0, 0.0],
                        [0.0, 0.0, 0.0, 0.0]])        
    
    S = JH*P*JH.T + R
    K = (P*JH.T) * np.linalg.inv(S)

    # Update the estimate via
    Z = measurements[:,filterstep].reshape(JH.shape[0],1)
    y = Z - (hx)                         # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*JH))*P

    # Save states for Plotting
    savestates(x, Z, P, K)

谢谢你的帮助和建议