// -------------------------------------------------------------------------
// -----                  CbmLitKalmanFilter source file               -----
// -----                  Created 16/07/07  by A. Lebedev               -----
// -------------------------------------------------------------------------
 

#include "CbmLitKalmanFilter.h"


// Constructor
CbmLitKalmanFilter::CbmLitKalmanFilter():
   CbmTrackUpdate("CbmLitKalmanFilter")
{

}

// Destructor
CbmLitKalmanFilter::~CbmLitKalmanFilter() 
{

}

// Initialization
void CbmLitKalmanFilter::Initialize()
{

}

// Finalization
void CbmLitKalmanFilter::Finalize()
{

}

void CbmLitKalmanFilter::Update( 
                          const CbmTrackParam *pParamIn,
                          CbmTrackParam *pParamOut,
                          CbmTrkHit *pHit)
{
   *pParamOut = *pParamIn;
   Update(pParamOut, pHit);
                              
}

void CbmLitKalmanFilter::Update(
                          CbmTrackParam *pParam, 
                          CbmTrkHit *pHit)
{

   // Instantiate initial state vector and covariance matrix
   Double_t xIn[5] = { pParam->GetX(), pParam->GetY(),
                        pParam->GetTx(), pParam->GetTy(),
                        pParam->GetQp() };
   Double_t cIn[15];
   pParam->CovMatrix(cIn);
   
   Double_t errX2 = pHit->GetDx() * pHit->GetDx();
   Double_t errY2 = pHit->GetDy() * pHit->GetDy();
      
   // Calculate residual
   Double_t dx = pHit->GetX() - xIn[0];
   Double_t dy = pHit->GetY() - xIn[1];
   
   // Calculate and inverse residual covariance matrix
   Double_t denominator = errX2 * errY2 + errX2 * cIn[5] + cIn[0] * errY2 + 
                          cIn[0] * cIn[5] - cIn[1] * cIn[1];
   Double_t R00 = (errY2 + cIn[5]) / denominator;
   Double_t R10 = - cIn[1] / denominator;
   Double_t R11 = (errX2 + cIn[0]) / denominator;
   
   // Calculate Kalman gain matrix
   Double_t K00 = cIn[0] * R00 + cIn[1] * R10;
   Double_t K01 = cIn[0] * R10 + cIn[1] * R11;
   Double_t K10 = cIn[1] * R00 + cIn[5] * R10;
   Double_t K11 = cIn[1] * R10 + cIn[5] * R11;
   Double_t K20 = cIn[2] * R00 + cIn[6] * R10;
   Double_t K21 = cIn[2] * R10 + cIn[6] * R11;
   Double_t K30 = cIn[3] * R00 + cIn[7] * R10;
   Double_t K31 = cIn[3] * R10 + cIn[7] * R11;
   Double_t K40 = cIn[4] * R00 + cIn[8] * R10;
   Double_t K41 = cIn[4] * R10 + cIn[8] * R11;
   
   // Calculate filtered state vector
   Double_t xOut[5];
   xOut[0] = xIn[0] + K00 * dx + K01 * dy;
   xOut[1] = xIn[1] + K10 * dx + K11 * dy;
   xOut[2] = xIn[2] + K20 * dx + K21 * dy;
   xOut[3] = xIn[3] + K30 * dx + K31 * dy;
   xOut[4] = xIn[4] + K40 * dx + K41 * dy;
   
   // Calculate filtered covariance matrix
   Double_t cOut[15];
   cOut[0] = (-K00 + 1) * cIn[0] - K01 * cIn[1];
   cOut[1] = (-K00 + 1) * cIn[1] - K01 * cIn[5];
   cOut[2] = (-K00 + 1) * cIn[2] - K01 * cIn[6];
   cOut[3] = (-K00 + 1) * cIn[3] - K01 * cIn[7];
   cOut[4] = (-K00 + 1) * cIn[4] - K01 * cIn[8];
   
   cOut[5] = (-K11 + 1) * cIn[5] - K10 * cIn[1];
   cOut[6] = (-K11 + 1) * cIn[6] - K10 * cIn[2];
   cOut[7] = (-K11 + 1) * cIn[7] - K10 * cIn[3];
   cOut[8] = (-K11 + 1) * cIn[8] - K10 * cIn[4];
   
   cOut[9] =  -K20 * cIn[2] - K21 * cIn[6] + cIn[9];
   cOut[10] = -K20 * cIn[3] - K21 * cIn[7] + cIn[10];
   cOut[11] = -K20 * cIn[4] - K21 * cIn[8] + cIn[11];
   
   cOut[12] = -K30 * cIn[3] - K31 * cIn[7] + cIn[12];
   cOut[13] = -K30 * cIn[4] - K31 * cIn[8] + cIn[13];
   
   cOut[14] = -K40 * cIn[4] - K41 * cIn[8] + cIn[14];
   
   // Copy filtered state to output
   pParam->SetX(xOut[0]);
   pParam->SetY(xOut[1]);
   pParam->SetTx(xOut[2]);
   pParam->SetTy(xOut[3]);
   pParam->SetQp(xOut[4]);
   pParam->SetCovMatrix(cOut);
}

ClassImp(CbmLitKalmanFilter)