def TransportMatrix(self, k):
"""
The Transport Matrix F allows to transport the State from site i to j
"""
return KFMatrix([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.],
[0., 0., 0., 1]])
def UMatrix(udir):
""" returns the rotation matrix that transfrom vectors in the track ref. system to the global system
"""
ux, uy, uz = udir
uu = sqrt(ux * ux + uy * uy + uz * uz)
ux = ux / uu
uy = uy / uu
uz = uz / uu
#print ' u ',ux,uy,uz
if (abs(uz) == 0.): raise ZeroDivisonError
vx = uz
vy = 0.
vz = -ux
vv = sqrt(vx * vx + vy * vy + vz * vz)
if (abs(vv) == 0.): raise ZeroDivisonError
vx = vx / vv
vy = vy / vv
vz = vz / vv
#print ' v ',vx,vy,vz
wx = -uy * ux / vv
wy = vv
wz = -uy * uz / vv
ww = sqrt(wx * wx + wy * wy + wz * wz)
wx = wx / ww
wy = wy / ww
wz = wz / ww
#print ' w ',wx,wy,wz
# UMatrix convert track ref system to global system
U = KFMatrix([[vx, wx, ux], [vy, wy, uy], [vz, wz, uz]])
debug('UMatrix udir, U ', (udir, U))
return U
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 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 H(self, k):
"""
H matrix at state k
"""
dz = self.system.Nodes[k].ZSlice.dz
z0 = self.system.Nodes[k].ZSlice.Zi + dz / 2.
return KFMatrix([[1., 0., z0, 0.], [0., 1., 0., z0]])
def MultipleScatteringMatrix(self, k):
"""
The multiple scattering matrix Qk
"""
dz = self.system.Nodes[k].ZSlice.dz
z0 = self.system.Nodes[k].ZSlice.Zi
z02 = z0 * z0
edep = self.system.Nodes[k].ZSlice.Edep * 1.
Lr = self.system.Nodes[k].ZSlice.Lr
L = abs(dz) / Lr
stateDict = self.system.States[k - 1]
state = stateDict["F"]
ak = state.V
p1 = ak[0]
p2 = ak[1]
p3 = ak[2]
p4 = ak[3]
lgx.debug(
"KFWolinFilter:MultipleScatteringMatrix -> ak ={0} type ={1}".
format(ak, type(ak)))
lgx.debug(" -> p1 ={0} p2 ={1} p3 ={2} p4 ={3}, type(p1)={4}".format(
p1, p2, p3, p4, type(p1)))
lgx.debug(
"KFWolinFilter:MultipleScatteringMatrix -> dz ={0} dz2 = {1} edep ={2}"
.format(dz, dz * dz, edep))
lgx.debug(
"KFWolinFilter:MultipleScatteringMatrix -> Lr ={0} L = {1} P ={2}".
format(Lr, L, self.P))
self.P = self.CorrectP(self.P, edep)
lgx.debug(
"KFWolinFilter:MultipleScatteringMatrix -> P (after correct) ={0}".
format(self.P))
s2tms = self.Sigma2ThetaMs(self.P, L)
lgx.debug(
"KFWolinFilter:MultipleScatteringMatrix -> s2tms ={0} type ={1}".
format(s2tms, type(s2tms)))
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)
cov = 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]])
return cov
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 __init__(self, hit, hmatrix):
""" constructor with a hit (KFData) with the measurment and the HMatrix
"""
self.hit = hit
self.hmatrix = KFMatrix(hmatrix)
self.zrun = hit.zrun
self.states = {}
self.chi2 = {}
self.status = 'none'
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 StraightLineQ(x0, y0, z0, thetax, thetay):
p1 = x0 - z0 * tan(thetax)
p2 = y0 - z0 * tan(thetay)
p3 = tan(thetax)
p4 = tan(thetay)
dz = z0
dz2 = dz * dz
s2tms = 1.
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)
cov = KFMatrix([[dz2 * p3p3, dz2 * p3p4, -dz * p3p3, -dz * p3p4],
[dz2 * p3p4, dz2 * p4p4, -dz * p3p4, -dz * p4p4],
[-dz * p3p3, -dz * p3p4, p3p3, p3p4],
[-dz * p3p4, -dz * p4p4, p3p4, p4p4]])
return cov
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