def TransportMatrix(self, index0, index1):
This = self.Track.GetNode(index0)
Other = self.Track.GetNode(index1)
state = This.Pstate.State if This.status < 2 else This.Fstate.State
tx = state[2]
ty = state[3]
dz = Other.running - This.running
ds = abs(dz) * (tx**2 + ty**2 + 1)**0.5
F = Array.Identity(self.Sdim)
F[-1][-1] = self.Eloss(state[-1], ds)
return F
def Filter(self, index):
C_filtered = (self.this_node.pred_state.CovarianceMatrix.Inverse() +
self.MMMatrixT**self.NMatrixI**self.MMMatrix).Inverse()
GainMatrix = C_filtered**self.MMMatrixT**NMatrixI
x_filtered = self.prev_state + GainMatrix**(
self.this_hit - self.MMMatrix**self.prev_state)
projector = Array.Identity(self.ndim) - self.MMMatrix**GainMatrix
r_filtered = projector**self.this_node.pred_resid
R_filtered = projector**self.NMatrix
chi2plus = r_filtered**R_filtered.Inverse()**r_filtered
newchi2 = self.prev_node.cumchi2 + chi2plus
self.this_node.filt_state = KalmanMeasurement(x_filtered, C_filtered)
self.this_node.filt_resid = KalmanMeasurement(r_filtered, R_filtered)
self.this_node.chi2 = chi2plus
self.this_node.cumchi2 = newchi2
def NoiseMatrix(self, index):
V = Array.Identity(self.Mdim) * self.xyres2
V[-1][-1] = self.Eres2
return V
def NoiseMatrix(self, index):
return Array.Identity(2) * self.xyresolution**2
def MultipleScatteringMatrix(self, index):
return Array.Identity(4) * 1e-12
def NoiseMatrix(self, index):
V = Array.Identity(self.Mdim) * self.xyres2
V[-1][-1] = self.Eresolution**2 / self.Track.GetNode(
index).hit.State[-1]
return V
[0., 0., 0., 1.])
def MeasurementMatrix(self, index):
return Array.Matrix([1., 0., 0., 0.], [0., 1., 0., 0.])
def MultipleScatteringMatrix(self, index):
return Array.Identity(4) * 1e-12
def NoiseMatrix(self, index):
return Array.Identity(2) * self.xyresolution**2
if __name__ == '__main__':
import ROOT
R = ROOT.TRandom3(0)
V = Array.Identity(2) * 0.01
Nhits = 500
hits = [
Array.Vector(
sin(0.01 * i) + R.Gaus(0, .1), -cos(0.1 * i) + R.Gaus(0, .1))
for i in range(Nhits)
]
runnings = map(float, range(Nhits))
measurements = [KalmanData(h, V) for h in hits]
istate = Array.Vector(hits[0][0], hits[0][1], 0., 0.)
istate = Array.Vector(0., 0., 0., 0.)
icvmatrix = Array.Identity(4) * 2
nextkf = NEXTKF()
nextkf = TrigoKF()
nextkf.SetMeasurements(runnings, measurements)