def central_EIF(system, agents, X_prev_est, X_prev_act, O_prev):
#prediction steps
O_prev = mat(O_prev.reshape((system.n, system.n))).astype(np.float64)
#print "O_prev",O_prev
S_prev_est = np.matmul(
O_prev,
mat(rs(X_prev_est, (system.n, 1))).astype(np.float64))
S_prev_est = mat(rs(S_prev_est, (system.n, 1))).astype(
np.float64) #getting S in matrix form
#X_prev_est = np.matmul(O_prev.I,S_prev_est)
X_prior = mat(system.state_propagate(X_prev_est,
0).reshape(system.n,
1)).astype(np.float64)
#print "X_prior", X_prior
A = mat(system.jacobian(X_prev_est)).astype(np.float64)
##print "A:",A
O_prior = (np.matmul(np.matmul(A, O_prev.I), A.T) +
np.matmul(np.matmul(system.G, system.Q), system.G.T)).I
S_prior = np.matmul(O_prior, X_prior)
#print "O_prior:", O_prior
#update
X_act = rs(system.state_propagate(X_prev_act, system.Sigma_w),
(system.n, ))
#print "Xact:",X_act
H = mat(obs_jacobian(X_prior)).astype(np.float64)
S_est = rs(S_prior, (system.n, 1))
O_post = O_prior
for i in range(len(agents)):
#print i, " Status:", agents[i].check_fov(X_act[0],X_act[1])
if agents[i].check_fov(X_act[0], X_act[1]):
Y_act = observation(system.M, X_act, system.Sigma_nu)
#print "Yact:",Y_act
#Y_est = observation(system.M,X_prior,0)
##print "Y est:",Y_est
z = np.matmul(np.matmul(H.T, system.R.I), Y_act)
#print "z:", z
I = np.matmul(np.matmul(H.T, system.R.I), H)
#print "I:",I
O_post = O_post + I
#print "O_post:", O_post
S_est = S_est + z
#print "S est:", S_est
X_est = np.matmul(O_post.I, S_est)
#print "X_est:", X_est
return rs(X_est, (system.n, )), X_act, O_post
def EIF(system, X_prev_est, X_prev_act, O_prev):
#prediction steps
O_prev = mat(O_prev.reshape((3, 3))).astype(np.float64)
S_prev_est = np.matmul(
O_prev,
mat(rs(X_prev_est, (system.n, 1))).astype(np.float64))
S_prev_est = mat(rs(S_prev_est, (system.n, 1))).astype(
np.float64) #getting S in matrix form
#X_prev_est = np.matmul(O_prev.I,S_prev_est)
X_prior = mat(system.state_propagate(X_prev_est,
0).reshape(system.n,
1)).astype(np.float64)
#print "X_prior", X_prior
A = mat(system.jacobian(X_prev_est)).astype(np.float64)
#print "A:",A
O_prior = (np.matmul(np.matmul(A, O_prev.I), A.T) +
np.matmul(np.matmul(system.G, system.Q), system.G.T)).I
S_prior = np.matmul(O_prior, X_prior)
#update
X_act = rs(system.state_propagate(X_prev_act, system.Sigma_w),
(system.n, ))
Y_act = observation(system.M, X_act, system.Sigma_nu)
#print "Yact:",Y_act
Y_est = observation(system.M, X_prior, 0)
#print "Y est:",Y_est
#call all connected nodes and get data.
H = mat(obs_jacobian(X_prior)).astype(np.float64)
#print "H:",H
z = np.matmul(np.matmul(H.T, system.R.I), Y_est)
I = np.matmul(np.matmul(H.T, system.R.I), H)
#print "I:",I
O_post = O_prior + I
#print "O_post:", O_post
S_est = rs(X_prior, (3, 1)) + z
#print "S est:", S_est
X_est = np.matmul(O_post.I, S_est)
return rs(X_est, (3, )), X_act, O_post
def dist_CI(target,agents,adj_mat,X_prev_act,t):
global FLAG_CONVERGENCE_ICI
X_act = rs(target.state_propagate(X_prev_act,target.Sigma_w),(target.n,))
#finding partials
for i in range(len(agents)):
agents[i].X_est[:,t+1],O_new = filter.partial_EIF(target,agents[i],agents[i].X_est[:,t], X_act, agents[i].O[:,t])
agents[i].O[:,t+1] = O_new.reshape((target.n**2,))
agents[i].P[:,t+1] = O_new.I.reshape((target.n**2,))
#do CI
l = 0
while not FLAG_CONVERGENCE_ICI:
for i in range(len(agents)):
agents[i].X_est[:,t+1],O_new= filter.CI(agents,adj_mat,i,t+1)
agents[i].O[:,t+1] = O_new.reshape((target.n**2,))
agents[i].P[:,t+1] = O_new.I.reshape((target.n**2,))
if l>max_iter_conv:
FLAG_CONVERGENCE_ICI = True
else:
l = l+1
FLAG_CONVERGENCE_ICI = False
return X_act
def partial_EIF(system, agent, X_prev_est, X_act, O_prev):
#prediction steps
O_prev = mat(O_prev.reshape((system.n, system.n))).astype(np.float64)
#print "O_prev",O_prev
S_prev_est = np.matmul(
O_prev,
mat(rs(X_prev_est, (system.n, 1))).astype(np.float64))
S_prev_est = mat(rs(S_prev_est, (system.n, 1))).astype(
np.float64) #getting S in matrix form
#X_prev_est = np.matmul(O_prev.I,S_prev_est)
X_prior = mat(system.state_propagate(X_prev_est,
0).reshape(system.n,
1)).astype(np.float64)
#print "X_prior", X_prior
A = mat(system.jacobian(X_prev_est)).astype(np.float64)
#print "A:",A
O_prior = (np.matmul(np.matmul(A, O_prev.I), A.T) +
np.matmul(np.matmul(system.G, system.Q), system.G.T)).I
S_prior = np.matmul(O_prior, X_prior)
#print "O_prior:", O_prior
#update
if agent.check_fov(X_act[0], X_act[1]):
H = mat(obs_jacobian(X_prior)).astype(np.float64)
Y_act = observation(system.M, X_act, system.Sigma_nu)
z = np.matmul(np.matmul(H.T, system.R.I), Y_act)
#print "z:", z
I = np.matmul(np.matmul(H.T, system.R.I), H)
S_est = rs(S_prior, (system.n, 1)) + z
O_post = O_prior + I
X_est = np.matmul(O_post.I, S_est)
else:
O_post = O_prior
X_est = X_prior
return rs(X_est, (system.n, )), O_post
def priors_meas(system, agent, X_prev_est, X_act, O_prev):
#prediction steps
O_prev = mat(O_prev.reshape((system.n, system.n))).astype(np.float64)
#print "O_prev",O_prev
S_prev_est = np.matmul(
O_prev,
mat(rs(X_prev_est, (system.n, 1))).astype(np.float64))
S_prev_est = mat(rs(S_prev_est, (system.n, 1))).astype(
np.float64) #getting S in matrix form
#X_prev_est = np.matmul(O_prev.I,S_prev_est)
X_prior = mat(system.state_propagate(X_prev_est,
0).reshape(system.n,
1)).astype(np.float64)
#print "X_prior", X_prior
A = mat(system.jacobian(X_prev_est)).astype(np.float64)
#print "A:",A
O_prior = (np.matmul(np.matmul(A, O_prev.I), A.T) +
np.matmul(np.matmul(system.G, system.Q), system.G.T)).I
S_prior = np.matmul(O_prior, X_prior)
#print "O_prior:", O_prior
#update
if agent.check_fov(X_act[0], X_act[1]):
H = mat(obs_jacobian(X_prior)).astype(np.float64)
Y_act = observation(system.M, X_act, system.Sigma_nu)
z = np.matmul(np.matmul(H.T, system.R.I), Y_act)
#print "z:", z
I = np.matmul(np.matmul(H.T, system.R.I), H)
else:
z = np.zeros(system.n)
I = (10**(-6)) * np.eye(system.n) #setting to 0 information
return X_prior.reshape((system.n, )), O_prior, z.reshape(
(system.n, )), I.reshape((system.n**2, ))
def state_propagate(self, X0, Sigma_w):
F, x1, x2 = self.kinematics()
w1 = np.random.normal(0, np.sqrt(Sigma_w))
w2 = np.random.normal(0, np.sqrt(Sigma_w))
w = mat([[w1], [w2]])
X = rs(
F.subs([(x1, X0[0]), (x2, X0[1])]) + np.matmul(self.G, w),
(self.n, ))
return X
def dist_hybrid(target,agents,adj_mat,X_prev_act,t):
global FLAG_CONVERGENCE
X_act = rs(target.state_propagate(X_prev_act,target.Sigma_w),(target.n,))
#finding priors
for i in range(len(agents)):
agents[i].X_est[:,t+1],O_new,agents[i].del_i[:,t+1],agents[i].del_I[:,t+1] = filter.priors_meas(target,agents[i],agents[i].X_est[:,t], X_act, agents[i].O[:,t])
agents[i].O[:,t+1] = O_new.reshape((target.n**2,))
agents[i].P[:,t+1] = O_new.I.reshape((target.n**2,))
l = 0
while not FLAG_CONVERGENCE:
for i in range(len(agents)):
#do CI on priors
agents[i].X_est[:,t+1],O_new = filter.CI(agents,adj_mat,i,t+1)
agents[i].O[:,t+1] = O_new.reshape((target.n**2,))
agents[i].P[:,t+1] = O_new.I.reshape((target.n**2,))
#consensus on likelihoods
agents[i].del_i[:,t+1],agents[i].del_I[:,t+1] = filter.consensus(agents,adj_mat,i,t+1)
if l>max_iter_conv:
FLAG_CONVERGENCE = True
else:
l = l+1
for i in range(len(agents)):
#hybrid update
S_post = np.matmul(mat(agents[i].O[:,t+1].reshape((target.n,target.n))).astype(np.float64),
mat(agents[i].X_est[:,t+1].reshape((target.n,1))).astype(np.float64)) + \
np.sum(adj_mat[i,:])*agents[i].del_i[:,t+1].reshape((target.n,1))
O_post = mat(agents[i].O[:,t+1].reshape((target.n,target.n))).astype(np.float64) + \
np.sum(adj_mat[i,:])*mat(agents[i].del_I[:,t+1].reshape((target.n,target.n))).astype(np.float64)
agents[i].X_est[:,t+1] = np.matmul(O_post.I,S_post).reshape((target.n,))
agents[i].O[:,t+1] = O_post.reshape((target.n**2,))
agents[i].P[:,t+1] = O_post.I.reshape((target.n**2,))
FLAG_CONVERGENCE = False
return X_act
pwd = os.getcwd() + '/'
os.chdir(pwd)
import astropy.io.fits as ft
from numpy import reshape as rs
fitsfile = '' # without suffix
outfitsfile = fitsfile + '_rdv' # output file
hdul = ft.open(fitsfile + '.fits')
hdr, dat = hdul[0].header, hdul[0].data
hdul.close()
""" remove redundant axis """
if hdr['NAXIS'] == 4 and hdr['CTYPE4'] == 'STOKES':
oldsp = list(dat.shape)
oldsp.pop(hdr['NAXIS'] - 4)
newsp = tuple(oldsp)
dat = rs(dat, newsp)
del hdr['CTYPE4'], hdr['CRVAL4'], hdr['CDELT4'], hdr['CRPIX4'], hdr[
'CUNIT4']
del hdr['PC4_1'], hdr['PC4_2'], hdr['PC4_3'], hdr['PC1_4'], hdr[
'PC2_4'], hdr['PC3_4'], hdr['PC4_4']
""" change velocity unit """
if hdr['CUNIT3'] == 'm/s':
hdr['CRVAL3'] = hdr['CRVAL3'] / 1e3
hdr['CDELT3'] = hdr['CDELT3'] / 1e3
hdr['CUNIT3'] = 'km/s'
hdr['HISTORY'] = 'Modified from ' + fitsfile + '.fits'
ft.writeto(outfitsfile + '.fits',
dat,
hdr,
def ParticleF(system, n):
X_est = np.zeros((system.X0.shape[0], n + 1))
X_act = np.zeros((system.X0.shape[0], n + 1))
X_est[:, 0] = np.reshape(system.X0, (system.X0.shape[0], ))
X_act[:, 0] = np.reshape(system.X0, (system.X0.shape[0], ))
P = np.zeros((system.X0.shape[0] * system.X0.shape[0], n + 1))
P[:, 0] = system.P0.reshape((9, ))
#Sampling
N = 200 #no. of particles
X_hyps = np.random.multivariate_normal(np.reshape(system.X0, (3, )),
system.P0, N) #sampling from prior
X_hyps = X_hyps.T
#print "Hyposthesis:",X_hyps
#w = (1.0/N)*np.ones(N) #weights
w = np.zeros(N)
for i in range(N):
X_err = mat(
(X_hyps[:, i] - rs(system.X0, (system.state_dim, ))).reshape(
(system.state_dim, 1))).astype(np.float64)
w[i] = m.exp(
-0.5 * np.matmul(np.matmul(X_err.T, system.P0.I), X_err)) / (
m.sqrt((2 * m.pi)**system.state_dim) *
np.sqrt(np.linalg.det(system.P0))) #calculating l
w = np.true_divide(w, np.sum(w))
np.random.seed(1)
"""
#Visualising particles
pylab.plot(X_hyps[1,:],w,'ro',markersize=2,linewidth=1,label='Particles x1')
pylab.legend()
pylab.show()
"""
for t in range(n):
X_act[:, t + 1] = np.reshape(
system.state_propagate(X_act[:, t], system.Q, t + 1), (3, ))
Y_act = observation(system.M, X_act[:, t + 1], system.R)
for i in range(N):
X_hyps[:, i] = system.state_propagate(
X_hyps[:, i], system.Q, t + 1).reshape(
(3, )) #propagate dynamics of particles
if (t + 1) % 5 == 0:
observ_err = Y_act - observation(
system.M, X_hyps[:, i],
np.zeros((system.meas_dim,
system.meas_dim))) #observation error
if system.R[2, 2] == 0:
system.R[2, 2] = 10**(-6)
#likelihood
w[i] = w[i] * m.exp(-0.5 * np.matmul(
np.matmul(observ_err.T, system.R.I), observ_err)) / (
m.sqrt((2 * m.pi)**system.meas_dim) *
np.sqrt(np.linalg.det(system.R))
) #calculating likelihood and updating weight
w = np.true_divide(w, np.sum(w)) #normalising weights
# pylab.figure(1)
# pylab.plot(X_hyps[1,:],w,'ro',markersize=2,linewidth=1,label='Particles x1')
# pylab.legend()
#plot particles
# if (t+1) == 5:
#
# pylab.plot(X_hyps[0,:],X_hyps[1,:],'ro',markersize=2,linewidth=1,label='Predicted PF')
# pylab.plot(X_act[0,t+1],X_act[1,t+1],'bo',markersize=2,linewidth=1,label='Actual')
# pylab.legend()
# pylab.xlabel('x')
# pylab.ylabel('y')
# #pylab.show()
# pylab.savefig('/home/naveed/Dropbox/Sem 3/Aero 626/HW3/'+'2_1_PF_t5.pdf', format='pdf',bbox_inches='tight',pad_inches = .06)
#resampling
c = np.zeros(N)
c[0] = 0
for i in range(1, N):
c[i] = c[i - 1] + w[i]
u = np.zeros(N)
u[0] = np.random.uniform(0, 1.0 / N)
i = 0 #starting at bottom of cdf
for j in range(N):
u[j] = u[0] + (1.0 / N) * j
while u[j] > c[i]:
i = i + 1
i = min(N - 1, i)
if i == N - 1:
break
#print "j:",j,"i:",i
X_hyps[:, j] = X_hyps[:, i]
w[j] = 1.0 / N
#print "w:",w
#print "X_hyps:",X_hyps[0,:]
# if (t+1) == 5:
# pylab.plot(X_hyps[0,:],X_hyps[1,:],'ro',markersize=2,linewidth=1,label='updated PF')
# pylab.plot(X_act[0,t+1],X_act[1,t+1],'bo',markersize=2,linewidth=1,label='Actual')
# pylab.legend()
# pylab.xlabel('x')
# pylab.ylabel('y')
# pylab.xlim(-4,4)
# pylab.ylim(-1,4)
# #pylab.show()
# pylab.savefig('/home/naveed/Dropbox/Sem 3/Aero 626/HW3/'+'2_1_PF_t5_update.pdf', format='pdf',bbox_inches='tight',pad_inches = .06)
#calculating estimate
X_temp = np.zeros(3)
for i in range(N):
X_temp = X_temp + w[i] * X_hyps[:, i]
X_est[:, t + 1] = X_temp.reshape((3, ))
#calculating variance
P_temp = np.zeros((system.state_dim, system.state_dim))
for i in range(N):
X_err = mat(
(X_hyps[:, i] - X_est[:, t + 1]).reshape(3,
1)).astype(np.float64)
P_temp = P_temp + np.matmul(X_err, X_err.T)
P[:, t + 1] = (1.0 / (N - 1)) * P_temp.reshape((9, ))
#print "P:", P[:,t+1]
# pylab.figure(2)
# pylab.plot(X_hyps[1,:],w,'ro',markersize=2,linewidth=1,label='Particles x1')
# pylab.legend()
# pylab.show()
return X_est, X_act, P