In [1]:
# the tensorflow version of the wong/wang dynamics that only uses a TF version of the controller
#the rest is in numpy
import tensorflow as tf
import numpy as np
import matplotlib.pyplot as plt
import math
from tf_funs import * #EKF, and helper in tensorflow
import sys
tf.set_random_seed(101)
pi = math.pi
np.random.seed(101)
sess = tf.Session()
In [ ]:
In [2]:
def ip_dynamics(X,C,g,L,dt):
#function for inverted pendulum dynamics
return dt*np.array([X[1]+C[0],g/L*math.sin(X[0])+C[1]])
def ip_dynamics_tf(X,C,g,L,dt):
#function for inverted pendulum dynamics, using tensorflow
return tf.stack([dt*(X[1]+C[0]),dt*(g/L*tf.sin(X[0])+C[1])])
In [6]:
#build graphs
xdim = 2 #state dimension
udim = 2 #control dimension
X_est = tf.placeholder(shape=(xdim),dtype=tf.float32,name='X_est') #the state estimate
PI_est = tf.placeholder(shape = (xdim,xdim),dtype=tf.float32, name = 'PI_est') #estimated covariance
Y_tp1 = tf.placeholder(shape=(xdim),dtype=tf.float32, name = 'Y_tp1') #the most recent observation
#Q = tf.placeholder(dtype=tf.float32)
#R = tf.placeholder(dtype=tf.float32)
Control = tf.placeholder(shape = udim, dtype=tf.float32, name='Control')
#params for experiment
#wong dynamics
T = 2000 #number of steps
#define the noise for the system
dt = 1.0e-3
sigsstate = (1./dt)*9e-9 #not sure how strong this should be yet
sigsobs = 1.0e-6 #good around 0.5
Q = sigsstate*np.eye(xdim)
Q_tf = tf.constant(Q,dtype=tf.float32, name = 'Q') #state noise covariance
R = sigsobs*np.eye(xdim)
R_tf = tf.constant(R,dtype=tf.float32, name = 'R') #observation noise covariance
gamma = 1.0e-8
#graphs for updating state and observation
g = 9.8
gtarg = -9.8
L = 0.1
true_model_est = ip_dynamics_tf(X_est,Control,g,L,dt)#state est. gradient, full myopic
true_model_est_null = ip_dynamics_tf(X_est,[0.,0.],g,L,dt)#state est. gradient w/o control
target_model_est = ip_dynamics_tf(X_est,[0.,0.],gtarg,L,dt) #state est. target model
#the non-tensorflow anonymous functions, for generalizations
true_nontf = lambda x,c: ip_dynamics(x,c,g,L,dt)
target_nontf = lambda x: ip_dynamics(x,[0.,0.],gtarg,L,dt)
X_plus,PI_plus = EKF(X_est,Y_tp1,PI_est,true_model_est,true_model_est_null,Q_tf,R_tf,xdim,dt)
#myopic controller
#graphs for controller
useMO = 1 #handle to use mean only. If ==1, then use mean-only. otherwise full
if useMO ==1:
print('using mean-only control')
Cnew = myopicController_meanonly(
X_est,PI_est,Control,gamma,true_model_est,
true_model_est_null,target_model_est,xdim,udim)
else:
print('using full myopic control')
Cnew = myopicController_noBdiff(
X_est,PI_est,Control,gamma,true_model_est,
true_model_est_null,target_model_est,xdim,udim)
#covariance prediction update graph
Ak = dynamics_linearized(X_est,true_model_est_null,xdim)
loss_tf = loss_full(X_est,PI_est,true_model_est,target_model_est)
In [7]:
ns = 1 #number of samples
#make these numpy version
tf.set_random_seed(101)
pi = math.pi
np.random.seed(101)
statenoise = np.random.normal(0,sigsstate**0.5,[xdim,T,ns])
obsnoise = np.random.normal(0,sigsobs**0.5,[xdim,T,ns])
G = dt**(0.5)*np.eye(xdim) #system noise matrix, for covariance prediction
x_estvec = np.zeros((xdim,T,ns))
xvec = np.zeros((xdim,T,ns))
yvec = np.zeros((xdim,T,ns))
x_targvec = np.zeros((xdim,T,ns))
PI_estvec = np.zeros((xdim,xdim,T,ns))
contall = np.zeros((udim,T,ns))
loss = np.zeros((T,ns))
loss_test = np.zeros((T,ns))
loss_test1 = np.zeros((4,T,ns)) #temporary. will remove
loss_nocont = np.zeros((T,ns))
lag = 0 #how many steps in the past will we receive observations
init = tf.global_variables_initializer()
sess.run(init)
#failure happened at iteration 76
x_init = np.array([ 0.1, 0.0])
for m in range(ns):
#x_init = [0.9,0.76] #initial state
#x_init = np.random.uniform(0.,1.,(2,))
PI_init = [[0.,0.],[0.,0.]] #initial covariance
c_init = [0.,0.]
xest_k = x_init
pi_k = PI_init
c_k = c_init
x_k = x_init
x_targ_k = x_init
ykp1 = np.array(x_init)
x_estvec[:,0,m] = x_init
xvec[:,0,m] = x_init
x_targvec[:,0,m] = x_init
PI_estvec[:,:,0,m] = PI_init
print(m)
#go ahead and propagate lag-steps ahead before starting state estimation and such
for k in range(1,lag):
#update actual dynamics
grad_cont = true_nontf(xvec[:,k-1,m],c_init)
grad_targ = target_nontf(x_targvec[:,k-1,m])
xvec[:,k,m] = xvec[:,k-1,m] + grad_cont + statenoise[:,k,m]
x_targvec[:,k,m] = x_targvec[:,k-1,m] + grad_targ + statenoise[:,k,m]
yvec[:,k,m] = xvec[:,k,m] + obsnoise[:,k,m]
#set estimates in beginning lags to initial state
x_estvec[:,k,m] = x_init
PI_estvec[:,:,k,m] = PI_init
for k in range(max(1,lag),T):
#update actual dynamics
grad_cont = true_nontf(xvec[:,k-1,m],contall[:,k-1,m])
grad_targ = target_nontf(x_targvec[:,k-1,m])
xvec[:,k,m] = xvec[:,k-1,m] + grad_cont + statenoise[:,k,m]
x_targvec[:,k,m] = x_targvec[:,k-1,m] + grad_targ + statenoise[:,k,m]
yvec[:,k,m] = xvec[:,k,m] + obsnoise[:,k,m]
test = sess.run([X_plus,PI_plus],
{X_est:x_estvec[:,k-lag-1,m], PI_est:PI_estvec[:,:,k-lag-1,m],
Control:contall[:,k-lag-1,m], Y_tp1:yvec[:,k-lag,m]})
x_estvec[:,k-lag,m] = test[0]
PI_estvec[:,:,k-lag,m] = test[1]
#predit lag states in the future to calculate control
x_est_n = x_estvec[:,k-lag,m]
PI_est_n = PI_estvec[:,:,k-lag,m]
for n in range(lag):
#state prediction step
grad_cont = true_nontf(x_est_n,contall[:,k-lag-1+n,m])
#covariance prediction step. calculate jacobian
Ak_n= sess.run(Ak,
{X_est: x_est_n, PI_est: PI_est_n,
Control: contall[:,0,m], Y_tp1:yvec[:,0,m]})
x_est_n = x_est_n + grad_cont
PI_est_n = np.matmul(Ak_n,PI_est_n) + np.matmul(PI_est_n,np.transpose(Ak_n)) + np.matmul(
np.matmul(G,Q),np.transpose(G))
#run myopic controller using predicted state estimated. cov, doesnt matter
#find control for time k
c_k = sess.run(Cnew,{X_est:x_est_n, PI_est:PI_est_n,
Control:contall[:,k-1,m], Y_tp1:yvec[:,k,m]})
contall[:,k,m] = c_k
#the mean expected loss
loss[k-lag,m] = np.linalg.norm(true_nontf(x_estvec[:,k-lag,m],contall[:,k-lag,m])-
target_nontf(x_estvec[:,k-lag,m]))**2
#the expected loss to second order
ltest = sess.run(loss_tf,{X_est:x_estvec[:,k-lag,m],
PI_est:PI_estvec[:,:,k-lag,m],
Control:contall[:,k-lag,m]
})
loss_test1[:,k-lag,m] = ltest
loss_nocont[k-lag,m] = np.linalg.norm(
true_nontf(x_estvec[:,k-lag,m],[0.,0.])-
target_nontf(x_estvec[:,k-lag,m]))**2
#set final lag estimate values to esimate
for k in range(lag):
x_targvec[:,T-lag+k,m] = x_targvec[:,T-lag-1,m]
x_estvec[:,T-lag+k,m] = x_estvec[:,T-lag-1,m]
PI_estvec[:,:,T-lag+k,m] = PI_estvec[:,:,T-lag-1,m]
In [8]:
#loss_both = loss
#loss_test_both = loss_test1
loss_meanonly = loss
loss_test_meanonly = loss_test1
In [12]:
#true state
tind = 0
plt.plot(x_estvec[0,:,tind],label='controlled dynamics')
plt.plot(x_targvec[0,:,tind],label = 'target dynamics')
plt.legend()
plt.xlabel('S_1')
plt.ylabel('S_2')
plt.title('controlled trajecotory')
plt.show()
#plt.plot(test[2][:,0]+ (test[3][:,0,0])**0.5)
#plt.plot(test[2][:,0]- (test[3][:,0,0])**0.5)
#plt.show()
plt.plot(contall[1,:,tind],label='control on dtheta/dt, full myopic')
plt.ylabel('C(t)')
plt.title('control signals')
plt.legend()
plt.show()
#expected state
fullloss = plt.plot(np.cumsum(loss[:,tind]),label='lagged, full myopic control')
nocontloss = plt.plot(np.cumsum(loss_nocont[:,tind]),label='no control')
#fullloss = plt.plot(np.cumsum(loss_both[:,tind]),label='full myopic control')
#nocontloss = plt.plot(np.cumsum(loss_meanonly[:,tind]),label='mean-only')
#fullloss_test = plt.plot(np.cumsum(loss_test_both[0,:,tind]),label='full')
#nocontloss_test = plt.plot(np.cumsum(loss_test_both[3,:,tind]),label='full first term ')
#nocontloss_test = plt.plot(loss_test_meanonly[:,tind]-loss_test_both[:,tind],
# label='test difference mean-only - both')
plt.xlabel('time')
plt.ylabel('loss')
plt.title('cumulative loss over time, sum_i(loss_i)')
plt.legend()
plt.show()
#standard deviation
In [65]:
fp = grad_elemwise(true_model_est,X_est)
gp = grad_elemwise(target_model_est,X_est)
test = tf.matmul(tf.transpose(fp-gp),fp-gp)
testfull = tf.trace(tf.matmul(test,PI_est))
testrun = sess.run(test,
{X_est:x_estvec[:,20,0], PI_est:PI_estvec[:,:,20,0],
Control:contall[:,19,0], Y_tp1:yvec[:,20,0]})
In [66]:
testrun
Out[66]:
In [ ]: