Reordered matrices to prevent gcc warning, added provisional chi2 calc

DUNE · Oct 21, 2024 · 74ab11d · 74ab11d
1 parent b7bed3f
commit 74ab11d
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 3 deletions.
diff --git a/src/TMS_Kalman.cpp b/src/TMS_Kalman.cpp
@@ -380,7 +380,23 @@ void TMS_Kalman::Predict(TMS_KalmanNode &Node) {
   CurrentState.dxdz = FilteredVec[2];
   CurrentState.dydz = FilteredVec[3];
 
-  //'CovarianceMatrix.Print();
+
+
+  // Calculate chi^2
+  Node.rVec  = (Measurement - H*UpdateVec);
+  //Node.rVecT = (Measurement - H*UpdateVec); Node.rVecT.T(); // Transpose it
+  Node.RMatrix = (Node.NoiseMatrix - H*UpdatedCovarianceMatrix*H_T).Invert();
+  //TMatrixD RInvMatrix = (Node.RMatrix).Invert();
+  //Node.chi2 = Node.RMatrix*Node.rVec;
+  //Node.chi2 = Node.rVecT*Node.RMatrix*Node.rVec;
+  Node.chi2 = Node.rVec*(Node.RMatrix*Node.rVec);
+  //(Node.RMatrix*Node.rVec).Print();
+  //Node.chi2 *= Node.rVec;
+  std::cout << "chi2: " << Node.chi2 << std::endl;
+
+
+
+  //CovarianceMatrix.Print();
   //GainMatrix.Print();
   //CurrentState.Print();
   if ( (CurrentState.x < TMS_Const::TMS_Start[0]) || (CurrentState.x > TMS_Const::TMS_End[0]) ) // point outside x region

diff --git a/src/TMS_Kalman.h b/src/TMS_Kalman.h
@@ -63,19 +63,30 @@ class TMS_KalmanNode {
     NoiseMatrix(KALMAN_DIM,KALMAN_DIM),
     CovarianceMatrix(KALMAN_DIM,KALMAN_DIM),
     UpdatedCovarianceMatrix(KALMAN_DIM,KALMAN_DIM),
-    MeasurementMatrix(KALMAN_DIM,KALMAN_DIM)
+    MeasurementMatrix(KALMAN_DIM,KALMAN_DIM),
+    rVec(KALMAN_DIM),
+    rVecT(KALMAN_DIM),
+    RMatrix(KALMAN_DIM,KALMAN_DIM)
   {
     TransferMatrix.ResizeTo(KALMAN_DIM, KALMAN_DIM);
     TransferMatrixT.ResizeTo(KALMAN_DIM, KALMAN_DIM);
     NoiseMatrix.ResizeTo(KALMAN_DIM, KALMAN_DIM);
     CovarianceMatrix.ResizeTo(KALMAN_DIM, KALMAN_DIM);
     UpdatedCovarianceMatrix.ResizeTo(KALMAN_DIM, KALMAN_DIM);
     MeasurementMatrix.ResizeTo(KALMAN_DIM, KALMAN_DIM);
+    rVec.ResizeTo(KALMAN_DIM);
+    rVecT.ResizeTo(KALMAN_DIM);
+    RMatrix.ResizeTo(KALMAN_DIM, KALMAN_DIM);
 
     // Make the transfer matrix for each of the states
     // Initialise to zero
     TransferMatrix.Zero();
     TransferMatrixT.Zero(); // Transposed
+    rVec.Zero();
+    rVecT.Zero();
+    RMatrix.Zero();
+
+
     // Diagonal element
     for (int j = 0; j < KALMAN_DIM; ++j)
     {
@@ -119,9 +130,14 @@ class TMS_KalmanNode {
   TMatrixD NoiseMatrix;
   TMatrixD CovarianceMatrix;
   TMatrixD UpdatedCovarianceMatrix;
-
   // Measurement matrix
   TMatrixD MeasurementMatrix;
+  // For chi2 stuff
+  TVectorD rVec;
+  TVectorD rVecT;
+  TMatrixD RMatrix;
+  double chi2;
+
 
   void SetRecoXY(TMS_KalmanState& State)
   {