def __init__(self, meth, nmx, V, P00):
self.meth = meth # Method: ekf, ukf, mhe
self.V = V # Variance of the Noise!!!
self.P00 = P00
self.te = SX.sym('te')
if self.meth.upper() == 'MHE':
self.MHE = Problem()
def __init__(self,meth,nmx,V,P00):
self.meth = meth # Method: ekf, ukf, mhe
self.V = V # Variance of the Noise!!!
self.P00 = P00
self.te = SX.sym('te')
if self.meth.upper() == 'MHE':
self.MHE = Problem()
def __init__(self,
N_sampling,
t_control=None,
total_plant=None,
dis='Collocation'):
self.N_sampling = N_sampling
self.dis = dis
if t_control == None and total_plant == None:
raise IOError(
'Define either control horizon or total plant run time')
if t_control is not None:
self.t_plant = t_control / 10.0 #10 Discretizations fixed
self.t_control = t_control
self.total_plant = self.t_plant * self.N_sampling
if total_plant is not None:
self.total_plant = total_plant
self.t_plant = total_plant / self.N_sampling
self.t_control = self.t_plant * 10 #10 Discretizations fixed
if total_plant is not None and t_control is not None:
if total_plant / self.N_sampling > t_control / 10:
raise IOError('Check control horizon and total plant run time')
if dis is not 'MultipleShooting':
self.prob = Problem()
else:
print "Hello"
self.prob = MultipleShootingProblem()
self.prob.setTimeRange(0.0, self.t_control)
self.t = SX.sym('t')
self.nmx = None
class Estimator(object):
def __init__(self, meth, nmx, V, P00):
self.meth = meth # Method: ekf, ukf, mhe
self.V = V # Variance of the Noise!!!
self.P00 = P00
self.te = SX.sym('te')
if self.meth.upper() == 'MHE':
self.MHE = Problem()
def addMeasurements(self, yEST):
self.HFunc = self.calcJacobian(yEST, self.xEST)
self.YFun = SXFunction([self.xEST, self.uEST], [yEST])
self.YFun.init()
self.yEST = yEST
self.nmx = self.yEST.shape[0]
def addStates(self,
n,
xmin,
xmax,
xstart=None,
method=['LagrangeColl', 10],
constr=True):
if self.meth.upper() == 'EKF':
self.xEST = SX.sym('xe', n)
elif self.meth.upper() == 'UKF':
self.xEST = SX.sym('xe', n)
elif self.meth.upper() == 'MHE':
self.xEST = self.MHE.addStates(n,
xmin,
xmax,
xstart=None,
method=['LagrangeColl', self.Nh],
constr=True)
j = 0
self.nx = n
return self.xEST
def addControls(self,
n,
umin,
umax,
ustart=None,
method=['PiecewiseConstant', 10]):
if self.meth.upper() == 'EKF':
self.uEST = SX.sym('ue', n)
elif self.meth.upper() == 'UKF':
self.uEST = SX.sym('ue', n)
elif self.meth.upper() == 'MHE':
self.uEST = self.MHE.addControls(
n,
umin,
umax,
ustart=None,
method=['PiecewiseConstant', self.Nh])
self.uEST.fix()
return self.uEST
def defineHorizon(self, Nh, Th):
if self.meth.upper() != 'MHE':
raise IOError('Horizon Length Defined Only For MHE')
self.Nh = Nh
self.Th = Th
self.MHE.setTimeRange(0.0, Nh * Th)
def addEstimatorODEs(self, x, rhs):
if self.meth.upper() == 'EKF' or self.meth.upper() == 'UKF':
self.est_model = SXFunction(
daeIn(x=self.xEST, p=self.uEST, t=self.te), daeOut(ode=rhs))
self.est_model.setOption('name', 'Estimator')
elif self.meth.upper() == 'MHE':
self.MHE.addOde(self.xEST, rhs) ## INCOMPLETE
self.RHSFunc = SXFunction([self.xEST, self.uEST], [rhs])
self.RHSFunc.init()
self.setupMHE()
# RHSFunc = SXFunction([xEST,uEST],[rhs])
self.JFunc = self.calcJacobian(rhs, self.xEST)
def calcJacobian(self, fx, x):
J = jacobian(fx, x)
JFunc = SXFunction([self.xEST, self.uEST], [J])
JFunc.init()
return JFunc
def evalFunction(self, f, xval, uval):
f.setInput(xval, 0)
f.setInput(uval, 1)
f.evaluate()
dFdx = NP.array(f.output())
return dFdx
def InitIntegrate(self, f, deltat_int):
I = Integrator('cvodes', f)
I.setOption('tf', deltat_int)
I.setOption('abstol', 1e-4)
I.setOption('reltol', 1e-4)
I.init()
return I
def getMeasurements(self, xp, up):
yo = NP.array(self.evalFunction(self.YFun, xp, up))
return yo
def setupMHE(self):
sigma_w = 0.00005 # Noise Charaterization
self.W = 1 / sigma_w * NP.eye(self.nx)
self.xARR0 = self.MHE.addFixedParameters(self.nx)
wMHE = self.MHE.addControls(self.nx, [-5] * self.nx, [5] * self.nx,
method=['PiecewiseConstant', self.Nh])
self.ym = self.MHE.addFixedParameters((self.Nh + 1) * self.nmx)
ymM = reshape(self.ym.T, self.Nh + 1, self.nmx).T
self.Psym = self.MHE.addFixedParameters(self.nx * self.nx)
Pm = reshape(self.Psym.T, self.nx, self.nx).T
cont_y = self.yEST('control')
ff = 0.0
ff += mul([(self.xEST(0) - self.xARR0).T, Pm.T, Pm,
(self.xEST(0) - self.xARR0)])
for i in range(self.Nh):
ff += mul([(cont_y[:, i] - ymM[:, i + 1]).T, self.V.T, self.V,
(cont_y[:, i] - ymM[:, i + 1])])
cont_w = wMHE('control')
ff += mul([wMHE(0).T, self.W.T, self.W, wMHE(0)])
for i in range(self.Nh):
ff += mul([(cont_w[:, i]).T, self.W.T, self.W, cont_w[:, i]])
self.MHE.addObjective(ff)
self.solMHE = Solver2(self.MHE, tol=1e-6)
def solveEKF(self, P, xep, ue, xm, dt):
I = self.InitIntegrate(self.est_model, dt) #dt is t_plant
#print ue
I.setInput(ue, 'p')
I.setInput(xep, 'x0')
I.evaluate()
xpred = NP.array(I.getOutput('xf')).flatten()
Hx = self.evalFunction(self.HFunc, xep, ue)
#print Hx
ypred = Hx.dot(
xpred
) #Hx is the jacobian of measurement! For now one of the state!
Jx = self.evalFunction(self.JFunc, xep, ue)
# print Jx
# print type(Jx)
# print type(P)
Ppred = Jx.dot(P.reshape(self.nx, self.nx)).dot(Jx.T)
yout = xm - ypred
S = Hx.dot(Ppred).dot(Hx.T) + self.V
Kk = Ppred.dot(Hx.T).dot(scipy.linalg.inv(S))
estimated_x = xpred + Kk.dot(yout)
Pout = (NP.eye(4) - Kk.dot(Hx)).dot(Ppred)
#print Pout
estimated_P = NP.reshape(Pout, 4 * 4)
return (estimated_x, estimated_P)
def solveUKF(self, P, xep, ue, xm, dt):
kappa = 3 - self.nx
I = self.InitIntegrate(self.est_model, dt)
sigma = scipy.linalg.cholesky(
(self.nx + kappa) * NP.reshape(P, (self.nx, self.nx)))
xlist = xep
nlist = 2 * self.nx + 1
for i in range(self.nx):
xlist = NP.vstack((xlist, xep + sigma[:, i]))
for i in range(self.nx):
xlist = NP.vstack((xlist, xep - sigma[:, i]))
xout = NP.zeros((nlist, self.nx))
yout = NP.zeros((nlist, self.nmx))
I.setInput(ue, 'p')
for i in range(nlist):
I.setInput(xlist[i, :], 'x0')
I.evaluate()
xout[i, :] = NP.array(I.getOutput('xf')).flatten()
yout[i, :] = self.evalFunction(self.YFun, xout[i, :], ue).flatten()
xpred = (1.0 / (self.nx + kappa)) * kappa * xout[0, :]
ypred = (1.0 / (self.nx + kappa)) * kappa * yout[0, :]
for i in range(1, nlist):
xpred += (1.0 / (self.nx + kappa)) * 0.5 * xout[i, :]
ypred += (1.0 / (self.nx + kappa)) * 0.5 * yout[i, :]
Ppred = 0
Pypred = 0
Pxypred = 0
for j in range(1, nlist):
Ppred = Ppred + (1.0 / (self.nx + kappa)) * 0.5 * NP.outer(
(xout[j, :] - xpred), (xout[j, :] - xpred))
Pypred = Pypred + (1.0 / (self.nx + kappa)) * 0.5 * NP.outer(
(yout[j, :] - ypred), (yout[j, :] - ypred))
Pxypred = Pxypred + (1.0 / (self.nx + kappa)) * 0.5 * NP.outer(
(xout[j, :] - xpred), (yout[j, :] - ypred))
Pinnov = Pypred + self.V
W = Pxypred.dot(scipy.linalg.inv(Pinnov))
estimated_x = xpred + W.dot(xm - ypred).flatten()
Pout = Ppred - W.dot(Pinnov).dot(W.T)
estimated_P = (NP.reshape(Pout, self.nx * self.nx))
return (estimated_x, estimated_P)
def solveMHE(self, P, xep, ue, xm, xARR, y_out, i, dt):
ymin = NP.reshape(y_out[:, i - self.Nh + 1:i + 2],
self.nmx * (self.Nh + 1))
self.ym.setPVals(ymin)
self.uEST.setPVals(ue[:, i - self.Nh + 1:i + 1].flatten())
self.xARR0.setPVals(xARR.flatten())
self.Psym.setPVals(P)
self.solMHE.solve()
estimated_xMHE = self.xEST(-1, True).flatten()
if i >= 2 * self.Nh - 1:
intin = xep[:, i - self.Nh + 1]
else:
intin = xARR
rhsf = self.evalFunction(self.RHSFunc, intin,
ue[:, i - self.Nh + 1]).flatten()
dFdx = self.evalFunction(self.JFunc, intin, ue[:, i - self.Nh + 1])
xtilda = rhsf - dFdx.dot(intin.flatten())
Xfuitg = dFdx
Xfuitg += NP.eye(4) / dt #why the eye and dt??
Hx = self.evalFunction(self.HFunc, intin, ue[:, i - self.Nh + 1])
A1 = NP.hstack(
(NP.reshape(P, (self.nx, self.nx)), NP.zeros((self.nx, self.nx))))
A2 = NP.hstack((-self.V.dot(Hx), NP.zeros((self.nmx, self.nx))))
A3 = NP.hstack((-self.W.dot(Xfuitg), self.W / dt)) # why the dt??
A = NP.vstack((A1, A2, A3))
b1 = (NP.reshape(P, (self.nx, self.nx))).dot(xARR)
b2 = (-self.V.dot(xm))
b3 = self.W.dot(xtilda)
B = NP.hstack((b1, b2, b3))
Q, R = NP.linalg.qr(A)
R2 = R[self.nx:, self.nx:]
Qb = Q.T.dot(B)
Qb2 = Qb[self.nx:]
result = NP.linalg.solve(R2, Qb2)
estimated_P = NP.reshape(R2, 4 * 4)
xARR = result
estimated_x = estimated_xMHE
return (estimated_x, estimated_P, xARR)
#self.y
class Estimator(object):
def __init__(self,meth,nmx,V,P00):
self.meth = meth # Method: ekf, ukf, mhe
self.V = V # Variance of the Noise!!!
self.P00 = P00
self.te = SX.sym('te')
if self.meth.upper() == 'MHE':
self.MHE = Problem()
def addMeasurements(self,yEST):
self.HFunc = self.calcJacobian(yEST,self.xEST)
self.YFun = SXFunction([self.xEST,self.uEST],[yEST])
self.YFun.init()
self.yEST = yEST
self.nmx = self.yEST.shape[0]
def addStates(self,n,xmin,xmax,xstart=None,method=['LagrangeColl',10],constr=True):
if self.meth.upper() == 'EKF':
self.xEST = SX.sym('xe',n)
elif self.meth.upper() == 'UKF':
self.xEST = SX.sym('xe',n)
elif self.meth.upper() == 'MHE':
self.xEST = self.MHE.addStates(n,xmin,xmax,xstart=None,method=['LagrangeColl',self.Nh],constr=True)
j = 0
self.nx = n
return self.xEST
def addControls(self,n,umin,umax,ustart=None,method=['PiecewiseConstant',10]):
if self.meth.upper() == 'EKF':
self.uEST = SX.sym('ue',n)
elif self.meth.upper() == 'UKF':
self.uEST = SX.sym('ue',n)
elif self.meth.upper() == 'MHE':
self.uEST = self.MHE.addControls(n,umin,umax,ustart=None,method=['PiecewiseConstant',self.Nh])
self.uEST.fix()
return self.uEST
def defineHorizon(self,Nh,Th):
if self.meth.upper() != 'MHE':
raise IOError('Horizon Length Defined Only For MHE')
self.Nh = Nh
self.Th = Th
self.MHE.setTimeRange(0.0,Nh*Th)
def addEstimatorODEs(self,x,rhs):
if self.meth.upper() == 'EKF' or self.meth.upper() == 'UKF':
self.est_model = SXFunction(daeIn(x=self.xEST,p=self.uEST,t=self.te),daeOut(ode=rhs))
self.est_model.setOption('name','Estimator')
elif self.meth.upper() == 'MHE':
self.MHE.addOde(self.xEST,rhs) ## INCOMPLETE
self.RHSFunc = SXFunction([self.xEST,self.uEST],[rhs])
self.RHSFunc.init()
self.setupMHE()
# RHSFunc = SXFunction([xEST,uEST],[rhs])
self.JFunc = self.calcJacobian(rhs,self.xEST)
def calcJacobian(self,fx,x):
J = jacobian(fx,x)
JFunc = SXFunction([self.xEST,self.uEST],[J])
JFunc.init()
return JFunc
def evalFunction(self,f,xval,uval):
f.setInput(xval,0)
f.setInput(uval,1)
f.evaluate()
dFdx = NP.array(f.output())
return dFdx
def InitIntegrate(self,f,deltat_int):
I = Integrator('cvodes',f)
I.setOption('tf',deltat_int)
I.setOption('abstol',1e-4)
I.setOption('reltol',1e-4)
I.init()
return I
def getMeasurements(self,xp,up):
yo = NP.array(self.evalFunction(self.YFun,xp,up))
return yo
def setupMHE(self):
sigma_w = 0.00005 # Noise Charaterization
self.W = 1/sigma_w * NP.eye(self.nx)
self.xARR0 = self.MHE.addFixedParameters(self.nx)
wMHE = self.MHE.addControls(self.nx,[-5]*self.nx,[5]*self.nx,method=['PiecewiseConstant',self.Nh])
self.ym = self.MHE.addFixedParameters((self.Nh+1)*self.nmx)
ymM = reshape(self.ym.T,self.Nh+1,self.nmx).T
self.Psym = self.MHE.addFixedParameters(self.nx*self.nx)
Pm = reshape(self.Psym.T,self.nx,self.nx).T
cont_y = self.yEST('control')
ff = 0.0
ff += mul([(self.xEST(0)-self.xARR0).T,Pm.T,Pm,(self.xEST(0)-self.xARR0)])
for i in range(self.Nh):
ff += mul([(cont_y[:,i] - ymM[:,i+1]).T,self.V.T,self.V,(cont_y[:,i] - ymM[:,i+1])])
cont_w = wMHE('control')
ff += mul([wMHE(0).T,self.W.T,self.W,wMHE(0)])
for i in range(self.Nh):
ff += mul([(cont_w[:,i]).T,self.W.T,self.W,cont_w[:,i]])
self.MHE.addObjective(ff)
self.solMHE = Solver2(self.MHE,tol=1e-6)
def solveEKF(self,P,xep,ue,xm,dt):
I = self.InitIntegrate(self.est_model,dt) #dt is t_plant
#print ue
I.setInput(ue,'p')
I.setInput(xep,'x0')
I.evaluate()
xpred = NP.array(I.getOutput('xf')).flatten()
Hx = self.evalFunction(self.HFunc,xep,ue)
#print Hx
ypred = Hx.dot(xpred) #Hx is the jacobian of measurement! For now one of the state!
Jx = self.evalFunction(self.JFunc,xep,ue)
# print Jx
# print type(Jx)
# print type(P)
Ppred = Jx.dot(P.reshape(self.nx,self.nx)).dot(Jx.T)
yout = xm - ypred
S = Hx.dot(Ppred).dot(Hx.T) + self.V
Kk = Ppred.dot(Hx.T).dot(scipy.linalg.inv(S))
estimated_x = xpred + Kk.dot(yout)
Pout = (NP.eye(4) - Kk.dot(Hx)).dot(Ppred)
#print Pout
estimated_P = NP.reshape(Pout,4*4)
return (estimated_x, estimated_P)
def solveUKF(self,P,xep,ue,xm,dt):
kappa = 3 - self.nx
I = self.InitIntegrate(self.est_model,dt)
sigma = scipy.linalg.cholesky((self.nx+kappa)*NP.reshape(P,(self.nx,self.nx)))
xlist = xep
nlist = 2*self.nx + 1
for i in range(self.nx):
xlist = NP.vstack((xlist,xep + sigma[:,i]))
for i in range(self.nx):
xlist = NP.vstack((xlist,xep - sigma[:,i]))
xout = NP.zeros((nlist,self.nx))
yout = NP.zeros((nlist,self.nmx))
I.setInput(ue,'p')
for i in range(nlist):
I.setInput(xlist[i,:],'x0')
I.evaluate()
xout[i,:] = NP.array(I.getOutput('xf')).flatten()
yout[i,:] = self.evalFunction(self.YFun,xout[i,:],ue).flatten()
xpred = (1.0/(self.nx+kappa))*kappa*xout[0,:]
ypred = (1.0/(self.nx+kappa))*kappa*yout[0,:]
for i in range(1,nlist):
xpred+= (1.0/(self.nx+kappa))*0.5*xout[i,:]
ypred+= (1.0/(self.nx+kappa))*0.5*yout[i,:]
Ppred = 0
Pypred = 0
Pxypred = 0
for j in range(1,nlist):
Ppred = Ppred + (1.0/(self.nx+kappa))*0.5*NP.outer((xout[j,:]-xpred),(xout[j,:]-xpred))
Pypred =Pypred + (1.0/(self.nx+kappa))*0.5*NP.outer((yout[j,:]-ypred),(yout[j,:]-ypred))
Pxypred =Pxypred + (1.0/(self.nx+kappa))*0.5*NP.outer((xout[j,:]-xpred),(yout[j,:]-ypred))
Pinnov = Pypred + self.V
W = Pxypred.dot(scipy.linalg.inv(Pinnov))
estimated_x = xpred + W.dot(xm-ypred).flatten()
Pout = Ppred - W.dot(Pinnov).dot(W.T)
estimated_P = (NP.reshape(Pout,self.nx*self.nx))
return (estimated_x,estimated_P)
def solveMHE(self,P,xep,ue,xm,xARR,y_out,i,dt):
ymin = NP.reshape(y_out[:,i-self.Nh+1:i+2],self.nmx*(self.Nh+1))
self.ym.setPVals(ymin)
self.uEST.setPVals(ue[:,i-self.Nh+1:i+1].flatten())
self.xARR0.setPVals(xARR.flatten())
self.Psym.setPVals(P)
self.solMHE.solve()
estimated_xMHE = self.xEST(-1,True).flatten()
if i >= 2*self.Nh-1:
intin = xep[:,i-self.Nh+1]
else:
intin = xARR
rhsf = self.evalFunction(self.RHSFunc,intin,ue[:,i-self.Nh+1]).flatten()
dFdx = self.evalFunction(self.JFunc,intin,ue[:,i-self.Nh+1])
xtilda = rhsf - dFdx.dot(intin.flatten())
Xfuitg = dFdx
Xfuitg += NP.eye(4)/dt #why the eye and dt??
Hx = self.evalFunction(self.HFunc,intin,ue[:,i-self.Nh+1])
A1 = NP.hstack((NP.reshape(P,(self.nx,self.nx)),NP.zeros((self.nx,self.nx))))
A2 = NP.hstack((-self.V.dot(Hx),NP.zeros((self.nmx,self.nx))))
A3 = NP.hstack((-self.W.dot(Xfuitg),self.W/dt)) # why the dt??
A = NP.vstack((A1,A2,A3))
b1 = (NP.reshape(P,(self.nx,self.nx))).dot(xARR)
b2 = (-self.V.dot(xm))
b3 = self.W.dot(xtilda)
B = NP.hstack((b1,b2,b3))
Q,R = NP.linalg.qr(A)
R2 = R[self.nx:,self.nx:]
Qb = Q.T.dot(B)
Qb2 = Qb[self.nx:]
result = NP.linalg.solve(R2,Qb2)
estimated_P = NP.reshape(R2,4*4)
xARR = result
estimated_x = estimated_xMHE
return (estimated_x,estimated_P,xARR)
#self.y
