def XUrandom(self, p, dis, x0, udir):
""" return a position and direction (in the global system)
after a random MS of a particle with momentum p
that traverses a distance dis with a direction udir
"""
udir.Unit()
U = UMatrix(udir)
#print ' U ',U
Q0 = self.Q0Matrix(p, dis)
#print ' Q0 ',Q0
x1, x2, t1, t2 = Random.cov(Q0)
#print ' x1,x2,t1,t2 ',x1,x2,t1,t2
t1 = tan(t1)
t2 = tan(t2)
nor = sqrt(1. + t1 * t1 + t2 * t2)
xt = KFVector([x1, x2, 0.])
ut = KFVector([t1 / nor, t2 / nor, 1. / nor])
#print ' xt ',xt
#print ' ut ',ut
xf = x0 + dis * udir
#print ' xf ',xf
xf = xf + U * xt
#print ' xf ',xf
uf = U * ut
#print ' uf ',uf
if (DEBUG):
print 'msnoise.XUrandom p,dis,x0,udir ', p, dis, x0, udir
print 'msnoise.XUrandom xt,ut ', xt, ut
print 'msnoise.XUrandom xf,uf ', xf, uf
return xf, uf
def execute(self):
# Generate Track
#----------------
#empty this
nullhits = map(lambda z: KFData(KFVector([0, 0]), self.V, z), self.zs)
print " null hits ", len(nullhits)
# create NEXT Kalman Filter
next = NEXT(self.pgas)
kf = nextfilter(next)
#kf = simplefilter(self.radlen)
kf.clear()
map(lambda hit: kf.addnode(hit, H0), nullhits)
print " null nodes ", len(kf)
state0 = zstate([0., 0., 0., 0., 0., 1., self.E0])
# generate hits and states into nodes
knodes = kf.generate(state0)
print " generated nodes ", len(knodes)
print " kf nodes ", len(kf)
ok = (len(knodes) > 0)
if (ok):
self.evt['sim/nodes'] = knodes
return ok
def InitialState():
x0 = 5.
y0 = 10.
ux0 = 0.25
uy0 = 0.25
sx0 = 0.1
sy0 = 0.1
x0x0 = sx0**2
y0y0 = sy0**2
sux0 = 0.1
suy0 = 0.1
ux0ux0 = sux0**2
uy0uy0 = suy0**2
x0ux0 = sx0 * sux0
x0uy0 = sx0 * suy0
y0ux0 = sy0 * sux0
y0uy0 = sy0 * suy0
x0y0 = sx0 * sy0
ux0uy0 = sux0 * suy0
traj0 = KFVector([x0, y0, ux0, uy0])
cov0 = KFMatrix([[x0x0, x0y0, x0ux0, x0uy0], [x0y0, y0y0, y0ux0, y0uy0],
[x0ux0, y0ux0, ux0ux0, ux0uy0],
[x0uy0, y0uy0, ux0uy0, uy0uy0]])
return (traj0, cov0)
def fitsegment(self, nodes):
if (len(nodes) <= 2): return
next = NEXT(self.pgas)
kf = nextfilter(next)
if (self.radlen > 0.):
self.msg.info('KF Simple radlen', self.radlen)
kf = simplefilter(self.radlen)
#kf = simplefilter(self.radlen)
kf.clear()
kf.setnodes(nodes)
print " fitsegment nodes ", len(kf.nodes)
# fit!
x0true = nodes[0].getstate('true').vec
x0 = KFVector(x0true)
z0 = nodes[0].zrun
dz = nodes[1].zrun - z0
if (dz == 0.):
print ' Fit failed, not valid input nodes ', z0, dz
return False, None
zdir = dz / abs(dz)
C1 = 1. * KFMatrixUnitary(5)
state0 = KFData(x0, C1, z0 - 0.1 * zdir, pars={'uz': zdir})
ok, fchi, schi = kf.fit(state0)
if (not ok):
print " Fit Failed! "
return False, None
print ' Fit chi2 ', dz, fchi, schi
return ok, kf
def propagate(self, state, zrun):
""" propagate this state to a zrun position
"""
#print ' propagate state ',state
#print ' propagate at ',zrun
ok = self.validstep(state, zrun)
if (not ok):
warning("kfmodel.propagate not possible ", (zrun, state.vec))
return ok, None, None, None
x = KFVector(state.vec)
C = KFMatrix(state.cov)
deltaz = self.deltazrun(state, zrun)
F = self.FMatrix(x, deltaz)
FT = F.Transpose()
#print ' F ',F
#print ' FT ',FT
Q = self.QMatrix(x, deltaz)
#print ' Q ',Q
xp = F * x
#print ' xp ',xp
Cp = F * C * FT
if (Q): Cp = Cp + Q
#print ' Cp ',Cp
pstate = KFData(xp, Cp, zrun, state.pars)
#print ' propagated state ',pstate
#print " F ",F
#print " Q ",Q
debug('kfmodel.propagate state ', pstate)
return ok, pstate, F, Q
def __init__(self, vec, cov, zrun, pars={}):
""" create KFData with a vector a cov matrix and the zrun parameter
"""
self.vec = KFVector(vec)
self.cov = KFMatrix(cov)
self.zrun = zrun
self.pars = dict(pars)
for key in self.pars.keys():
setattr(self, key, pars[key])
return
def __KSystem(self,Hits,hitErrors,radiationLength):
"""
Returns a KFSystem
"""
Nodes=[]
numberOfHits = len(Hits)
Zi =[]
sxk =hitErrors[0]
syk =hitErrors[1]
###########
ghits = []
#etot = 0.; # debug
for k in range(0,numberOfHits-1):
hitk =Hits[k]
##########
ghits.append( KalmanMeasurement( Array.Vector( hitk[0], hitk[1], hitk[2], hitk[3] ), Array.Matrix( [sxk**2,0.], [0.,syk**2] ) ) )
hitkp1 =Hits[k+1]
zki =hitk[2]
zkf = hitkp1[2]
edepk = hitk[3]
#etot += edepk #debug
xk = hitk[0]
yk =hitk[1]
zslice=KFZSlice(radiationLength,zki,zkf,edepk)
hit =KFVector([xk,yk])
cov=KFMatrix([[sxk**2,0.],
[0.,syk**2]])
measurement = KFMeasurement(hit,cov)
node = KFNode(measurement,zslice)
Nodes.append(node)
Zi.append(zki)
#etot += Hits[numberOfHits-1][3] #debug
if debug >= Debug.draw.value:
mpldv = MPLDrawVector(Zi)
mpldv.Draw()
###########
for i,hit in enumerate(ghits):
self.Track.AddNode( KalmanNode(step = i, hit = hit) )
#print "Created KSystem with total deposited energy of {0}".format(etot); # debug
setup = KFSetup(Nodes)
system =KFSystem(setup)
return system
def step(self, state0):
x0, y0, z0, ux0, uy0, uz0, ee0 = state0
de, ds = self.eloss.deltax(ee0, self.deltae)
ok = self.msnoiser.validstep(ee0, ds)
if (not ok): return ok, state0
xv0 = KFVector([x0, y0, z0])
uv0 = KFVector([ux0, uy0, uz0])
uv0.Unit()
#print ' x0 ',xv0
#print ' u0 ',uv0
#print ' ee0 ',ee0
#print ' de, ds ',de,ds
pp = kinmomentum(ee0)
xvf, uvf = self.msnoiser.XUrandom(pp, ds, xv0, uv0)
x, y, z = xvf
ux, uy, uz = uvf
ee = ee0 - de
state = [x, y, z, ux, uy, uz, ee]
debug('kgenerator.step state ', state)
return ok, state
def zstate(ustate):
""" create a zstate from a ustate (x,y,z,ux,uy,uz,ee)
into (x,y,tx,ty,ee) and uz in the pars of KFData
"""
x0, y0, z0, ux, uy, uz, ee = ustate
assert uz != 0., "zline.zstate uz cos is null!"
assert ee != 0., "zline.zstate energy is null!"
x = KFVector([x0, y0, ux / uz, uy / uz, ee])
C = KFMatrixNull(5)
zst = KFData(x, C, zrun=z0, pars={'uz': uz})
debug('zsstate ', (ustate, zst))
return zst
def createhits(digits,xres=0.1):
""" from the list of digits (x,y,z,delta-ene), return a list of hits.
Each hit is computed with a given resolution
A hit is a KFData with a vector (x,y) and a cov-matrix.
Hit also has an attribute with the delta-ene (dene)
"""
hits = []
for digit in digits:
x,y,z,dene = digit
x = x+random.gauss(0.,xres)
y = y+random.gauss(0.,xres)
V = (xres*xres)*KFMatrixUnitary(2)
hit = KFData(KFVector([x,y]),V,zrun=z)
hit.dene = dene
hits.append(hit)
# print 'createhits ',hits
return hits
def seedstate(nodes):
""" from a list of nodes generate the seed state (a KFData (x,y,tx,ty,ene) and cov-matrix)
where tx,ty are the tangent in the x,y axis
"""
x0 = nodes[0].hit.vec
x1 = nodes[1].hit.vec
z0 = nodes[0].zrun
dz = nodes[1].zrun-z0
if (dz==0.):
print ' Fit failed, not valid input nodes ',z0,dz
#print ' node 0 ',nodes[0]
#print ' node 1 ',nodes[1]
return False,None
ene = nodes[0].hit.ene
tx,ty = (x0[0]-x1[0])/dz,(x0[1]-x1[1])/dz
xx = KFVector([x0[0],x0[1],tx,ty,ene])
zdir = dz/abs(dz)
C1 = 100.*KFMatrixUnitary(5)
state0 = KFData(xx,C1,z0-0.1*zdir,pars={'uz':zdir})
#print "seedstate ",state0
return state0
def Setup():
LrXe = 15300. # radiation length of xenon in mm
Pr = 10. #pressure in bar
Nodes = []
for i in range(1, 6):
zslice = KFZSlice(LrXe / Pr, i, i + 1, 0.1)
x0 = 5 + i
y0 = 15 + i
hit = KFVector([x0, y0])
sigma_x = 0.1
sigma_y = 0.2
cov = KFMatrix([[sigma_x**2, 0], [0, sigma_y**2]])
measurement = KFMeasurement(hit, cov)
node = KFNode(measurement, zslice)
Nodes.append(node)
setup = KFSetup(Nodes)
return setup
def SetupFilter(self, k):
"""
Constructs a state with momentum equal to the current
momentum of the Kalman filter and using the directional fit
from slices k to k+4.
"""
p0 = self.P
nodes = self.GetNodes()
nfpts = 4
# Set up the lists.
fpt_x = []
fpt_y = []
fpt_z = []
for tk in range(k, k + nfpts):
if (tk < len(nodes)):
fV = nodes[tk].Measurement.V
fx = fV[0]
fy = fV[1]
fzi = nodes[tk].ZSlice.Zi
fzf = nodes[tk].ZSlice.Zf
fz = (fzi + fzf) / 2.
fpt_x.append(fx)
fpt_y.append(fy)
fpt_z.append(fz)
# Set the z0 and z0**2 for (zi of the slice) for calculation of MS.
z0 = nodes[k].ZSlice.Zi
z02 = z0**2
# Fit the points to a line to predict the next direction vector.
# Direction vector of line is vv[0] returned by np.linalg.svd:
# code from: http://stackoverflow.com/questions/2298390/fitting-a-line-in-3d
xvals = np.array(fpt_x)
yvals = np.array(fpt_y)
zvals = np.array(fpt_z)
dmatrix = np.concatenate(
(xvals[:, np.newaxis], yvals[:, np.newaxis], zvals[:, np.newaxis]),
axis=1)
dmean = np.array([fpt_x[0], fpt_y[0], fpt_z[0]
]) # ensure the line passes through the point k
uu, dd, vv = np.linalg.svd(dmatrix - dmean)
uvec = vv[0]
# Calculate the length of the next step.
Lr = nodes[k].ZSlice.Lr
dz = nodes[k].ZSlice.dz
L = abs(dz) / Lr
# Set the initial trajectory (get initial tangent angles from first two points).
x0 = fpt_x[0]
y0 = fpt_y[0]
ux = uvec[0]
uy = uvec[1]
uz = uvec[2]
tanX0 = ux / uz
tanY0 = uy / uz
p1 = x0 - z0 * tanX0
p2 = y0 - z0 * tanY0
p3 = tanX0
p4 = tanY0
traj0 = KFVector([p1, p2, p3, p4])
# Compute the initial MS covariance matrix.
s2tms = self.SigmaThetaMs(p0, L)
s2tms = s2tms**2
p3p3 = s2tms * (1 + p3**2) * (1 + p3**2 + p4**2)
p4p4 = s2tms * (1 + p4**2) * (1 + p3**2 + p4**2)
p3p4 = s2tms * p3 * p4 * (1 + p3**2 + p4**2)
cov0 = KFMatrix([[z02 * p3p3, z02 * p3p4, -z0 * p3p3, -z0 * p3p4],
[z02 * p3p4, z02 * p4p4, -z0 * p3p4, -z0 * p4p4],
[-z0 * p3p3, -z0 * p3p4, p3p3, p3p4],
[-z0 * p3p4, -z0 * p4p4, p3p4, p4p4]])
# Create the state object.
state0 = KFState(traj0, cov0)
return state0
KFHVector.__init__(self, hit, cov)
def __str__(self):
s = "<\nHit :"
s += self.V.__str__() + "\nCov :" + self.Cov.__str__() + "\n>"
return s
def __repr__(self):
return self.__str__()
if __name__ == '__main__':
x0 = 5.
y0 = 15.
hit = KFVector([x0, y0])
sigma_x = 0.1
sigma_y = 0.2
cov = KFMatrix([[sigma_x**2, 0], [0, sigma_y**2]])
print "hit is a KFVector", isinstance(hit, KFVector)
print "hit =", hit
print "cov is a KFMatrix", isinstance(cov, KFMatrix)
print "cov =", cov
m = KFMeasurement(hit, cov)
print "Measurement=", m
def random(self):
x0 = KFVector(self.vec)
sx = Random.cov(self.cov)
x = x0 + sx
debug('kfdata.random x', x)
return x
def StraightLineTrajectory(x0, y0, z0, thetax, thetay):
return KFVector([
x0 - z0 * tan(thetax), y0 - z0 * tan(thetay),
tan(thetax),
tan(thetay)
])
