BmnKalmanFilter.h

Go to the documentation of this file.

/*
 * File:   BmnKalmanFilter.h
 *
 * Created on July 29, 2015, 9:48 AM
 */
 
#ifndef BmnKalmanFilter_H
#define BmnKalmanFilter_H
 
#include "BmnEnums.h"
#include "BmnFitNode.h"
#include "BmnGeoNavigator.h"
#include "BmnHit.h"
#include "BmnMaterialEffects.h"
#include "BmnMath.h"
#include "BmnMatrixMath.h"
#include "CbmStsTrack.h"
#include "FairField.h"
#include "FairRunAna.h"
#include "FairTrackParam.h"
#include "TClonesArray.h"
#include "TMath.h"
#include "TMatrixD.h"
 
#include <vector>
 
class BmnKalmanFilter
{
  public:
    BmnKalmanFilter();
    virtual ~BmnKalmanFilter();
 
    static const Int_t kMaxHits = 11;   // 4 FSD + 7 GEM
 
    BmnStatus Update(FairTrackParam* par, const BmnHit* hit, Double_t& chiSq);
    BmnStatus Smooth(BmnFitNode* thisNode, BmnFitNode* prevNode);
    BmnStatus TGeoTrackPropagate(FairTrackParam* par,
                                 Double_t zOut,
                                 Int_t pdg,
                                 vector<Double_t>* F,
                                 Double_t* length,
                                 Bool_t isField);
    BmnStatus TGeoTrackPropagate(FairTrackParam* par, Double_t zOut, Int_t pdg, vector<Double_t>* F, Double_t* length);
 
    // === Residuals ===
    BmnStatus ComputeResiduals(CbmStsTrack* track,
                               BmnHit** hitsArray,
                               Int_t nHits,
                               Int_t pdg,
                               Double_t* resX,
                               Double_t* resY,
                               Int_t* station,
                               Int_t& nRes);
 
    BmnStatus GetResidual(CbmStsTrack* track,
                          BmnHit** hitsArray,
                          Int_t nHits,
                          Int_t station,
                          Int_t pdg,
                          Double_t& resX,
                          Double_t& resY);
 
  private:
    void RK4Order(const Double_t* xIn, Double_t zIn, Double_t* xOut, Double_t zOut, Double_t* derivs, FairField* field);
 
    inline void UpdateF(Double_t* F, const Double_t* newF)
    {
        Double_t A[25];
        A[0] = newF[0] * F[0] + newF[1] * F[5] + newF[2] * F[10] + newF[3] * F[15] + newF[4] * F[20];
        A[1] = newF[0] * F[1] + newF[1] * F[6] + newF[2] * F[11] + newF[3] * F[16] + newF[4] * F[21];
        A[2] = newF[0] * F[2] + newF[1] * F[7] + newF[2] * F[12] + newF[3] * F[17] + newF[4] * F[22];
        A[3] = newF[0] * F[3] + newF[1] * F[8] + newF[2] * F[13] + newF[3] * F[18] + newF[4] * F[23];
        A[4] = newF[0] * F[4] + newF[1] * F[9] + newF[2] * F[14] + newF[3] * F[19] + newF[4] * F[24];
        A[5] = newF[5] * F[0] + newF[6] * F[5] + newF[7] * F[10] + newF[8] * F[15] + newF[9] * F[20];
        A[6] = newF[5] * F[1] + newF[6] * F[6] + newF[7] * F[11] + newF[8] * F[16] + newF[9] * F[21];
        A[7] = newF[5] * F[2] + newF[6] * F[7] + newF[7] * F[12] + newF[8] * F[17] + newF[9] * F[22];
        A[8] = newF[5] * F[3] + newF[6] * F[8] + newF[7] * F[13] + newF[8] * F[18] + newF[9] * F[23];
        A[9] = newF[5] * F[4] + newF[6] * F[9] + newF[7] * F[14] + newF[8] * F[19] + newF[9] * F[24];
        A[10] = newF[10] * F[0] + newF[11] * F[5] + newF[12] * F[10] + newF[13] * F[15] + newF[14] * F[20];
        A[11] = newF[10] * F[1] + newF[11] * F[6] + newF[12] * F[11] + newF[13] * F[16] + newF[14] * F[21];
        A[12] = newF[10] * F[2] + newF[11] * F[7] + newF[12] * F[12] + newF[13] * F[17] + newF[14] * F[22];
        A[13] = newF[10] * F[3] + newF[11] * F[8] + newF[12] * F[13] + newF[13] * F[18] + newF[14] * F[23];
        A[14] = newF[10] * F[4] + newF[11] * F[9] + newF[12] * F[14] + newF[13] * F[19] + newF[14] * F[24];
        A[15] = newF[15] * F[0] + newF[16] * F[5] + newF[17] * F[10] + newF[18] * F[15] + newF[19] * F[20];
        A[16] = newF[15] * F[1] + newF[16] * F[6] + newF[17] * F[11] + newF[18] * F[16] + newF[19] * F[21];
        A[17] = newF[15] * F[2] + newF[16] * F[7] + newF[17] * F[12] + newF[18] * F[17] + newF[19] * F[22];
        A[18] = newF[15] * F[3] + newF[16] * F[8] + newF[17] * F[13] + newF[18] * F[18] + newF[19] * F[23];
        A[19] = newF[15] * F[4] + newF[16] * F[9] + newF[17] * F[14] + newF[18] * F[19] + newF[19] * F[24];
        A[20] = newF[20] * F[0] + newF[21] * F[5] + newF[22] * F[10] + newF[23] * F[15] + newF[24] * F[20];
        A[21] = newF[20] * F[1] + newF[21] * F[6] + newF[22] * F[11] + newF[23] * F[16] + newF[24] * F[21];
        A[22] = newF[20] * F[2] + newF[21] * F[7] + newF[22] * F[12] + newF[23] * F[17] + newF[24] * F[22];
        A[23] = newF[20] * F[3] + newF[21] * F[8] + newF[22] * F[13] + newF[23] * F[18] + newF[24] * F[23];
        A[24] = newF[20] * F[4] + newF[21] * F[9] + newF[22] * F[14] + newF[23] * F[19] + newF[24] * F[24];
        for (Int_t i = 0; i < 25; i++)
            F[i] = A[i];
    }
 
    // === TransportC inline ===
    inline void TransportC(const Double_t* cIn, const Double_t* F, Double_t* cOut)
    {
        Double_t A = cIn[2] + F[2] * cIn[9] + F[3] * cIn[10] + F[4] * cIn[11];
        Double_t B = cIn[3] + F[2] * cIn[10] + F[3] * cIn[12] + F[4] * cIn[13];
        Double_t C = cIn[4] + F[2] * cIn[11] + F[3] * cIn[13] + F[4] * cIn[14];
 
        Double_t D = cIn[6] + F[7] * cIn[9] + F[8] * cIn[10] + F[9] * cIn[11];
        Double_t E = cIn[7] + F[7] * cIn[10] + F[8] * cIn[12] + F[9] * cIn[13];
        Double_t G = cIn[8] + F[7] * cIn[11] + F[8] * cIn[13] + F[9] * cIn[14];
 
        Double_t H = cIn[9] + F[13] * cIn[10] + F[14] * cIn[11];
        Double_t I = cIn[10] + F[13] * cIn[12] + F[14] * cIn[13];
        Double_t J = cIn[11] + F[13] * cIn[13] + F[14] * cIn[14];
 
        Double_t K = cIn[13] + F[17] * cIn[11] + F[19] * cIn[14];
 
        cOut[0] = cIn[0] + F[2] * cIn[2] + F[3] * cIn[3] + F[4] * cIn[4] + A * F[2] + B * F[3] + C * F[4];
        cOut[1] = cIn[1] + F[2] * cIn[6] + F[3] * cIn[7] + F[4] * cIn[8] + A * F[7] + B * F[8] + C * F[9];
        cOut[2] = A + B * F[13] + C * F[14];
        cOut[3] = B + A * F[17] + C * F[19];
        cOut[4] = C;
 
        cOut[5] = cIn[5] + F[7] * cIn[6] + F[8] * cIn[7] + F[9] * cIn[8] + D * F[7] + E * F[8] + G * F[9];
        cOut[6] = D + E * F[13] + G * F[14];
        cOut[7] = E + D * F[17] + G * F[19];
        cOut[8] = G;
 
        cOut[9] = H + I * F[13] + J * F[14];
        cOut[10] = I + H * F[17] + J * F[19];
        cOut[11] = J;
 
        cOut[12] = cIn[12] + F[17] * cIn[10] + F[19] * cIn[13] + (F[17] * cIn[9] + cIn[10] + F[19] * cIn[11]) * F[17]
                   + K * F[19];
        cOut[13] = K;
 
        cOut[14] = cIn[14];
    }
 
    // === CalcOut inline ===
    inline Double_t CalcOut(Double_t in, const Double_t k[4])
    {
        return in + k[0] / 6. + k[1] / 3. + k[2] / 3. + k[3] / 6.;
    }
    BmnStatus RK4TrackExtrapolate(FairTrackParam* par, Double_t zOut, Double_t* F);
    void UpdateF(vector<Double_t>& F, const vector<Double_t>& newF);
    BmnGeoNavigator* fNavigator;
    BmnMaterialEffects* fMaterial;   //!
};
 
#endif /* BmnKalmanFilter_H */