markwmuller / controlpy Goto Github PK
View Code? Open in Web Editor NEWPython controls
License: GNU General Public License v3.0
Python controls
License: GNU General Public License v3.0
hi very good functions for control, but I realized there is no lqe().
So here is my proposal:
(everything Matlab tested)
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
and for the discrete version (the covariance estimate is still wrong compared to matlab, since I do not know their intial estimate)
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
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.