markwmuller / controlpy Goto Github PK

def lqe(A, G, C, Q, R):
    """Solve steady-state Kalman filter for a continuous time system
    
    x' = Ax + Bu + Gw            {State equation}
    y = Cx + Du + v             {Measurements}
     
    E{ww'} = Q,    E{vv'} = R
    
    Returns K, X, eigVals:
    Returns gain the optimal filter gain K, the solution matrix X, 
    and the closed loop system eigenvalues.
    """
    # first, try to solve the ricatti equation
    X = scipy.linalg.solve_continuous_are(A.T, C.T, G.dot(Q.dot(G.T)), R)
    # compute the Kalman gain
    K = np.dot(np.linalg.inv(R),np.dot(C,X))  
    # compute EigenValues
    eigVals = np.linalg.eigvals(A-np.dot(K.T,C))
      
    return K , X, eigVals

def dlqe(A, G, C, Q, R):
    """Solve the discrete time, steady-state Kalman filter for a discrete time system.
    
    x[n+1] = Ax[n] + Bu[n] + Gw[n]    {State equation}
    y[n]   = Cx[n] + Du[n] +  v[n]    {Measurements}
     
    with unbiased process noise w[n] and measurement noise v[n] 
    with covariances
    E{ww'} = Q,    E{vv'} = R,    E{wv'} = 0 ,
   
    
    Returns K, X, eigVals:
    Returns gain the optimal filter gain K, the solution matrix X, 
    and the closed loop system eigenvalues.
    """
    # first, try to solve the ricatti equation
    X = scipy.linalg.solve_discrete_are(A.T, C.T, G.dot(Q.dot(G.T)), R)   
    # compute the Kalman gain
    K = X.dot(C.T).dot(np.linalg.inv(C.dot(X).dot(C.T)+R))
    # compute EigenValues
    eigVals = np.linalg.eigvals(A - np.dot(A,K).dot(C))
    
    ### to do!! 
    ### insert good initial estimate for covariance matrix
    ### see some proposals:
    # estimate error covariance matrix
    #cov_err = (np.identity(len(C))- K.dot(C)).dot(((A.dot(0.001*np.eye(len(Q)))).dot(A.T)) + Q)
    # alternatively
    #cov_err = (np.identity(len(C))- K.dot(C)).dot(Q)
    
    return K, X, eigVals, #cov_err