BmnKalmanFilter.cxx

Go to the documentation of this file.

#include "BmnKalmanFilter.h"
 
#include "BmnFieldMap.h"
 
using namespace std;
using namespace TMath;
 
BmnKalmanFilter::BmnKalmanFilter()
{
    fMaterial = new BmnMaterialEffects();
    fNavigator = new BmnGeoNavigator();
}
 
BmnKalmanFilter::~BmnKalmanFilter()
{
    delete fMaterial;
    delete fNavigator;
}
 
BmnStatus BmnKalmanFilter::Update(FairTrackParam* par, const BmnHit* hit, Double_t& chiSq)
{
    Double_t cIn[15];
    par->CovMatrix(cIn);
 
    static const Double_t ONE = 1., TWO = 2.;
 
    Double_t dxx = hit->GetDx() * hit->GetDx();
    Double_t dxy = 0.0;
    Double_t dyy = hit->GetDy() * hit->GetDy();
 
    Double_t dx = hit->GetX() - par->GetX();
    Double_t dy = hit->GetY() - par->GetY();
 
    Double_t t =
        (dxx * dyy + dxx * cIn[5] + dyy * cIn[0] + cIn[0] * cIn[5] - dxy * dxy - TWO * dxy * cIn[1] - cIn[1] * cIn[1]);
    if (t == 0.0)
        return kBMNERROR;
    else
        t = ONE
            / (dxx * dyy + dxx * cIn[5] + dyy * cIn[0] + cIn[0] * cIn[5] - dxy * dxy - TWO * dxy * cIn[1]
               - cIn[1] * cIn[1]);
 
    Double_t R00 = (dyy + cIn[5]) * t;
    Double_t R01 = -(dxy + cIn[1]) * t;
    Double_t R11 = (dxx + cIn[0]) * t;
 
    Double_t K00 = cIn[0] * R00 + cIn[1] * R01;
    Double_t K01 = cIn[0] * R01 + cIn[1] * R11;
    Double_t K10 = cIn[1] * R00 + cIn[5] * R01;
    Double_t K11 = cIn[1] * R01 + cIn[5] * R11;
    Double_t K20 = cIn[2] * R00 + cIn[6] * R01;
    Double_t K21 = cIn[2] * R01 + cIn[6] * R11;
    Double_t K30 = cIn[3] * R00 + cIn[7] * R01;
    Double_t K31 = cIn[3] * R01 + cIn[7] * R11;
    Double_t K40 = cIn[4] * R00 + cIn[8] * R01;
    Double_t K41 = cIn[4] * R01 + cIn[8] * R11;
 
    Double_t xOut[5] = {par->GetX(), par->GetY(), par->GetTx(), par->GetTy(), par->GetQp()};
    xOut[0] += K00 * dx + K01 * dy;
    xOut[1] += K10 * dx + K11 * dy;
    xOut[2] += K20 * dx + K21 * dy;
    xOut[3] += K30 * dx + K31 * dy;
    xOut[4] += K40 * dx + K41 * dy;
 
    Double_t cOut[15];
    for (Int_t i = 0; i < 15; ++i)
        cOut[i] = cIn[i];
 
    cOut[0] += -K00 * cIn[0] - K01 * cIn[1];
    cOut[1] += -K00 * cIn[1] - K01 * cIn[5];
    cOut[2] += -K00 * cIn[2] - K01 * cIn[6];
    cOut[3] += -K00 * cIn[3] - K01 * cIn[7];
    cOut[4] += -K00 * cIn[4] - K01 * cIn[8];
 
    cOut[5] += -K11 * cIn[5] - K10 * cIn[1];
    cOut[6] += -K11 * cIn[6] - K10 * cIn[2];
    cOut[7] += -K11 * cIn[7] - K10 * cIn[3];
    cOut[8] += -K11 * cIn[8] - K10 * cIn[4];
 
    cOut[9] += -K20 * cIn[2] - K21 * cIn[6];
    cOut[10] += -K20 * cIn[3] - K21 * cIn[7];
    cOut[11] += -K20 * cIn[4] - K21 * cIn[8];
 
    cOut[12] += -K30 * cIn[3] - K31 * cIn[7];
    cOut[13] += -K30 * cIn[4] - K31 * cIn[8];
 
    cOut[14] += -K40 * cIn[4] - K41 * cIn[8];
 
    par->SetX(xOut[0]);
    par->SetY(xOut[1]);
    par->SetTx(xOut[2]);
    par->SetTy(xOut[3]);
    par->SetQp(xOut[4]);
    par->SetCovMatrix(cOut);
 
    Double_t xmx = hit->GetX() - par->GetX();
    Double_t ymy = hit->GetY() - par->GetY();
    Double_t C0 = cOut[0];
    Double_t C1 = cOut[1];
    Double_t C5 = cOut[5];
 
    Double_t norm = dxx * dyy - dxx * C5 - dyy * C0 + C0 * C5 - dxy * dxy + 2 * dxy * C1 - C1 * C1;
 
    chiSq = ((xmx * (dyy - C5) - ymy * (dxy - C1)) * xmx + (-xmx * (dxy - C1) + ymy * (dxx - C0)) * ymy) / norm;
 
    return kBMNSUCCESS;
}
 
BmnStatus BmnKalmanFilter::Smooth(BmnFitNode* thisNode, BmnFitNode* prevNode)
{
    if (thisNode->GetPredictedParam()->GetQp() == 0.0)
        return kBMNERROR;
    if (prevNode->GetPredictedParam()->GetQp() == 0.0)
        return kBMNERROR;
 
    Double_t cov1[15];
    prevNode->GetPredictedParam()->CovMatrix(cov1);
    vector<Double_t> invPrevPredC(cov1, cov1 + 15);
    if (!InvSym15(invPrevPredC))
        return kBMNERROR;
 
    vector<Double_t> Ft(prevNode->GetF());
    Transpose25(Ft);
 
    Double_t cov2[15];
    thisNode->GetUpdatedParam()->CovMatrix(cov2);
    vector<Double_t> thisUpdC(cov2, cov2 + 15);
 
    vector<Double_t> A(25);
    vector<Double_t> temp1(25);
    Mult15On25(thisUpdC, Ft, temp1);
    Mult25On15(temp1, invPrevPredC, A);
 
    vector<Double_t> thisUpdX;
    thisUpdX.push_back(thisNode->GetUpdatedParam()->GetX());
    thisUpdX.push_back(thisNode->GetUpdatedParam()->GetY());
    thisUpdX.push_back(thisNode->GetUpdatedParam()->GetTx());
    thisUpdX.push_back(thisNode->GetUpdatedParam()->GetTy());
    thisUpdX.push_back(thisNode->GetUpdatedParam()->GetQp());
 
    vector<Double_t> prevSmoothedX;
    prevSmoothedX.push_back(prevNode->GetSmoothedParam()->GetX());
    prevSmoothedX.push_back(prevNode->GetSmoothedParam()->GetY());
    prevSmoothedX.push_back(prevNode->GetSmoothedParam()->GetTx());
    prevSmoothedX.push_back(prevNode->GetSmoothedParam()->GetTy());
    prevSmoothedX.push_back(prevNode->GetSmoothedParam()->GetQp());
 
    vector<Double_t> prevPredX;
    prevPredX.push_back(prevNode->GetPredictedParam()->GetX());
    prevPredX.push_back(prevNode->GetPredictedParam()->GetY());
    prevPredX.push_back(prevNode->GetPredictedParam()->GetTx());
    prevPredX.push_back(prevNode->GetPredictedParam()->GetTy());
    prevPredX.push_back(prevNode->GetPredictedParam()->GetQp());
 
    vector<Double_t> temp2(5), temp3(5);
    Subtract(prevSmoothedX, prevPredX, temp2);
    Mult25On5(A, temp2, temp3);
    vector<Double_t> thisSmoothedX(5);
    Add(thisUpdX, temp3, thisSmoothedX);
 
    Double_t cov3[15];
    prevNode->GetSmoothedParam()->CovMatrix(cov3);
    vector<Double_t> prevSmoothedC(cov3, cov3 + 15);
 
    Double_t cov4[15];
    prevNode->GetPredictedParam()->CovMatrix(cov4);
    vector<Double_t> prevPredC(cov4, cov4 + 15);
 
    vector<Double_t> temp4(15);
    Subtract(prevSmoothedC, prevPredC, temp4);
 
    vector<Double_t> temp5(15);
    Similarity(A, temp4, temp5);
    vector<Double_t> thisSmoothedC(15);
    Add(thisUpdC, temp5, thisSmoothedC);
 
    FairTrackParam par;
    par.SetX(thisSmoothedX[0]);
    par.SetY(thisSmoothedX[1]);
    par.SetTx(thisSmoothedX[2]);
    par.SetTy(thisSmoothedX[3]);
    par.SetQp(thisSmoothedX[4]);
 
    Double_t cov5[15];
    for (Int_t i = 0; i < 15; ++i)
        cov5[i] = thisSmoothedC[i];
    par.SetCovMatrix(cov5);
    par.SetZ(thisNode->GetUpdatedParam()->GetZ());
 
    thisNode->SetSmoothedParam(&par);
 
    return kBMNSUCCESS;
}
 
BmnStatus BmnKalmanFilter::RK4TrackExtrapolate(FairTrackParam* par, Double_t zOut, Double_t* F)
{
    Double_t zIn = par->GetZ();
    FairField* field = FairRunAna::Instance()->GetField();
 
    Double_t xIn[5];
    xIn[0] = par->GetX();
    xIn[1] = par->GetY();
    xIn[2] = par->GetTx();
    xIn[3] = par->GetTy();
    xIn[4] = par->GetQp();
 
    Double_t xOut[5];
    Double_t F1[25];
 
    RK4Order(xIn, zIn, xOut, zOut, F1, field);
 
    Double_t cIn[15];
    par->CovMatrix(cIn);
 
    Double_t cOut[15];
    TransportC(cIn, F1, cOut);
 
    par->SetX(xOut[0]);
    par->SetY(xOut[1]);
    par->SetTx(xOut[2]);
    par->SetTy(xOut[3]);
    par->SetQp(xOut[4]);
    par->SetCovMatrix(cOut);
    par->SetZ(zOut);
 
    // Копируем F1 в F (если F — не nullptr)
    if (F != NULL) {
        for (Int_t i = 0; i < 25; i++)
            F[i] = F1[i];
    }
    return kBMNSUCCESS;
}
 
void BmnKalmanFilter::RK4Order(const Double_t* xIn,
                               Double_t zIn,
                               Double_t* xOut,
                               Double_t zOut,
                               Double_t* derivs,
                               FairField* field)
{
    const Double_t fC = 0.000299792458;
    const Double_t coef[4] = {0.0, 0.5, 0.5, 1.0};
 
    Double_t Ax[4], Ay[4];
    Double_t dAx_dtx[4], dAy_dtx[4], dAx_dty[4], dAy_dty[4];
    Double_t k[4][4];
 
    Double_t h = zOut - zIn;
    Double_t hC = h * fC;
    Double_t hCqp = h * fC * xIn[4];
    Double_t x0[4];
    Double_t x[4] = {xIn[0], xIn[1], xIn[2], xIn[3]};
 
    for (UInt_t iStep = 0; iStep < 4; iStep++) {
        if (iStep > 0) {
            for (UInt_t i = 0; i < 4; i++)
                x[i] = xIn[i] + coef[iStep] * k[i][iStep - 1];
        }
 
        Double_t zCur = zIn + coef[iStep] * h;
        Double_t Bx = field->GetBx(x[0], x[1], zCur);
        Double_t By = field->GetBy(x[0], x[1], zCur);
        Double_t Bz = field->GetBz(x[0], x[1], zCur);
 
        Double_t tx = x[2], ty = x[3];
        Double_t txtx = tx * tx, tyty = ty * ty, txty = tx * ty;
        Double_t inv = 1.0 / (1.0 + txtx + tyty);
        Double_t t1 = Sqrt(1.0 + txtx + tyty);
 
        Ax[iStep] = (txty * Bx + ty * Bz - (1.0 + txtx) * By) * t1;
        Ay[iStep] = (-txty * By - tx * Bz + (1.0 + tyty) * Bx) * t1;
 
        dAx_dtx[iStep] = Ax[iStep] * tx * inv + (ty * Bx - 2.0 * tx * By) * t1;
        dAx_dty[iStep] = Ax[iStep] * ty * inv + (tx * Bx + Bz) * t1;
        dAy_dtx[iStep] = Ay[iStep] * tx * inv + (-ty * By - Bz) * t1;
        dAy_dty[iStep] = Ay[iStep] * ty * inv + (-tx * By + 2.0 * ty * Bx) * t1;
 
        k[0][iStep] = tx * h;
        k[1][iStep] = ty * h;
        k[2][iStep] = Ax[iStep] * hCqp;
        k[3][iStep] = Ay[iStep] * hCqp;
    }
 
    for (UInt_t i = 0; i < 4; i++)
        xOut[i] = CalcOut(xIn[i], k[i]);
    xOut[4] = xIn[4];
 
    derivs[0] = 1.;
    derivs[5] = 0.;
    derivs[10] = 0.;
    derivs[15] = 0.;
    derivs[20] = 0.;
    derivs[1] = 0.;
    derivs[6] = 1.;
    derivs[11] = 0.;
    derivs[16] = 0.;
    derivs[21] = 0.;
 
    x[0] = x0[0] = 0.0;
    x[1] = x0[1] = 0.0;
    x[2] = x0[2] = 1.0;
    x[3] = x0[3] = 0.0;
    for (UInt_t iStep = 0; iStep < 4; iStep++) {
        if (iStep > 0) {
            for (UInt_t i = 0; i < 4; i++)
                if (i != 2)
                    x[i] = x0[i] + coef[iStep] * k[i][iStep - 1];
        }
        k[0][iStep] = x[2] * h;
        k[1][iStep] = x[3] * h;
        k[3][iStep] = (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]) * hCqp;
    }
    derivs[2] = CalcOut(x0[0], k[0]);
    derivs[7] = CalcOut(x0[1], k[1]);
    derivs[12] = 1.0;
    derivs[17] = CalcOut(x0[3], k[3]);
    derivs[22] = 0.0;
 
    x[0] = x0[0] = 0.0;
    x[1] = x0[1] = 0.0;
    x[2] = x0[2] = 0.0;
    x[3] = x0[3] = 1.0;
    for (UInt_t iStep = 0; iStep < 4; iStep++) {
        if (iStep > 0) {
            for (UInt_t i = 0; i < 4; i++)
                if (i != 3)
                    x[i] = x0[i] + coef[iStep] * k[i][iStep - 1];
        }
        k[0][iStep] = x[2] * h;
        k[1][iStep] = x[3] * h;
        k[2][iStep] = (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]) * hCqp;
    }
    derivs[3] = CalcOut(x0[0], k[0]);
    derivs[8] = CalcOut(x0[1], k[1]);
    derivs[13] = CalcOut(x0[2], k[2]);
    derivs[18] = 1.;
    derivs[23] = 0.;
 
    x[0] = x0[0] = 0.0;
    x[1] = x0[1] = 0.0;
    x[2] = x0[2] = 0.0;
    x[3] = x0[3] = 0.0;
    for (UInt_t iStep = 0; iStep < 4; iStep++) {
        if (iStep > 0) {
            for (UInt_t i = 0; i < 4; i++)
                x[i] = x0[i] + coef[iStep] * k[i][iStep - 1];
        }
        k[0][iStep] = x[2] * h;
        k[1][iStep] = x[3] * h;
        k[2][iStep] = Ax[iStep] * hC + hCqp * (dAx_dtx[iStep] * x[2] + dAx_dty[iStep] * x[3]);
        k[3][iStep] = Ay[iStep] * hC + hCqp * (dAy_dtx[iStep] * x[2] + dAy_dty[iStep] * x[3]);
    }
    derivs[4] = CalcOut(x0[0], k[0]);
    derivs[9] = CalcOut(x0[1], k[1]);
    derivs[14] = CalcOut(x0[2], k[2]);
    derivs[19] = CalcOut(x0[3], k[3]);
    derivs[24] = 1.;
}
 
BmnStatus BmnKalmanFilter::TGeoTrackPropagate(FairTrackParam* par,
                                              Double_t zOut,
                                              Int_t pdg,
                                              vector<Double_t>* F,
                                              Double_t* length)
{
    BmnFieldMap* field = (BmnFieldMap*)FairRunAna::Instance()->GetField();
    Bool_t isField = !(field->IsFieldOff());
 
    if (!IsParCorrect(par, isField))
        return kBMNERROR;
 
    Double_t zIn = par->GetZ();
    Double_t dz = zOut - zIn;
    Bool_t downstream = dz > 0;
 
    if (isField) {
        if (F != NULL) {
            if (F->size() != 25)
                F->resize(25);
            std::fill(F->begin(), F->end(), 0.0);
            (*F)[0] = 1.;
            (*F)[6] = 1.;
            (*F)[12] = 1.;
            (*F)[18] = 1.;
            (*F)[24] = 1.;
        }
 
        Int_t nofSteps = Int_t(abs(dz) / 10);
        Double_t stepSize = (nofSteps == 0) ? abs(dz) : 10.0;
        Double_t z = zIn;
 
        vector<BmnMaterialInfo> inter;
 
        for (Int_t iStep = 0; iStep < nofSteps + 1; iStep++) {
            if (z == zOut)
                break;
 
            if (iStep != nofSteps)
                z = (downstream) ? z + stepSize : z - stepSize;
            else
                z = zOut;
 
            inter.clear();
            if (fNavigator->FindIntersections(par, z, inter) == kBMNERROR)
                return kBMNERROR;
 
            for (UInt_t iMat = 0; iMat < inter.size(); iMat++) {
                BmnMaterialInfo& mat = inter[iMat];
 
                if (F != NULL) {
                    Double_t Fnew[25];
                    RK4TrackExtrapolate(par, mat.GetZpos(), Fnew);   // ← если переделать сигнатуру
                    UpdateF(F->data(), Fnew);
                } else {
                    if (RK4TrackExtrapolate(par, mat.GetZpos(), nullptr) == kBMNERROR)
                        return kBMNERROR;
                }
 
                fMaterial->Update(par, &mat, pdg, downstream);
                if (length)
                    *length += mat.GetLength();
            }
        }
    } else {
        Float_t x0 = par->GetX(), y0 = par->GetY(), z0 = par->GetZ();
        Float_t tx = par->GetTx(), ty = par->GetTy();
        Float_t xOut = tx * (zOut - z0) + x0;
        Float_t yOut = ty * (zOut - z0) + y0;
        par->SetX(xOut);
        par->SetY(yOut);
        par->SetZ(zOut);
        if (length)
            *length = Sqrt(Sq(xOut - x0) + Sq(yOut - y0) + Sq(zOut - z0));
    }
 
    return IsParCorrect(par, isField) ? kBMNSUCCESS : kBMNERROR;
}
BmnStatus BmnKalmanFilter::TGeoTrackPropagate(FairTrackParam* par,
                                              Double_t zOut,
                                              Int_t pdg,
                                              vector<Double_t>* F,
                                              Double_t* length,
                                              Bool_t isField)
{
    return TGeoTrackPropagate(par, zOut, pdg, F, length);
}
 
// ============================================================
//                     Residuals
// ============================================================
 
BmnStatus BmnKalmanFilter::ComputeResiduals(CbmStsTrack* track,
                                            BmnHit** hitsArray,
                                            Int_t nHits,
                                            Int_t pdg,
                                            Double_t* resX,
                                            Double_t* resY,
                                            Int_t* station,
                                            Int_t& nRes)
{
    nRes = 0;
 
    if (!track || !hitsArray)
        return kBMNERROR;
    if (nHits < 2)
        return kBMNSUCCESS;
    if (nHits > kMaxHits)
        return kBMNERROR;
 
    Double_t xL[kMaxHits], yL[kMaxHits];
    Bool_t validL[kMaxHits];
 
    for (Int_t k = 0; k < nHits; k++) {
        xL[k] = 0.0;
        yL[k] = 0.0;
        validL[k] = kFALSE;
    }
 
    FairTrackParam parF(*(track->GetParamFirst()));
    FairTrackParam parL(*(track->GetParamLast()));
 
    FairTrackParam parFtmp = parF;
    for (Int_t k = 0; k < nHits; k++) {
        BmnHit* hit = hitsArray[k];
 
        if (TGeoTrackPropagate(&parFtmp, hit->GetZ(), pdg, nullptr, nullptr) == kBMNERROR)
            continue;
 
        xL[k] = parFtmp.GetX();
        yL[k] = parFtmp.GetY();
        validL[k] = kTRUE;
 
        Double_t chi2 = 0;
        Update(&parFtmp, hit, chi2);
    }
 
    FairTrackParam parLtmp = parL;
    for (Int_t k = nHits - 1; k >= 0; k--) {
        BmnHit* hit = hitsArray[k];
 
        if (TGeoTrackPropagate(&parLtmp, hit->GetZ(), pdg, nullptr, nullptr) == kBMNERROR)
            continue;
 
        Double_t xR = parLtmp.GetX();
        Double_t yR = parLtmp.GetY();
 
        if (validL[k]) {
            Double_t xAvg = 0.5 * (xL[k] + xR);
            Double_t yAvg = 0.5 * (yL[k] + yR);
 
            resX[nRes] = xAvg - hit->GetX();
            resY[nRes] = yAvg - hit->GetY();
            station[nRes] = hit->GetStation();
            nRes++;
        }
 
        Double_t chi2 = 0;
        Update(&parLtmp, hit, chi2);
    }
 
    return kBMNSUCCESS;
}
 
BmnStatus BmnKalmanFilter::GetResidual(CbmStsTrack* track,
                                       BmnHit** hitsArray,
                                       Int_t nHits,
                                       Int_t station,
                                       Int_t pdg,
                                       Double_t& resX,
                                       Double_t& resY)
{
    if (!hitsArray)
        return kBMNERROR;
    if (nHits > kMaxHits)
        return kBMNERROR;
 
    Double_t resX_arr[kMaxHits], resY_arr[kMaxHits];
    Int_t station_arr[kMaxHits];
    Int_t nRes = 0;
 
    BmnStatus status = ComputeResiduals(track, hitsArray, nHits, pdg, resX_arr, resY_arr, station_arr, nRes);
    if (status != kBMNSUCCESS)
        return kBMNERROR;
 
    for (Int_t k = 0; k < nRes; k++) {
        if (station_arr[k] == station) {
            resX = resX_arr[k];
            resY = resY_arr[k];
            return kBMNSUCCESS;
        }
    }
 
    return kBMNERROR;
}