From 74ab11d8ecebbac254da960ad832bd4199f8a080 Mon Sep 17 00:00:00 2001
From: Liam O'Sullivan <liam.osullivan@uni-mainz.de>
Date: Mon, 21 Oct 2024 07:39:19 -0500
Subject: [PATCH] Reordered matrices to prevent gcc warning, added provisional
 chi2 calc

---
 src/TMS_Kalman.cpp | 18 +++++++++++++++++-
 src/TMS_Kalman.h   | 20 ++++++++++++++++++--
 2 files changed, 35 insertions(+), 3 deletions(-)

diff --git a/src/TMS_Kalman.cpp b/src/TMS_Kalman.cpp
index 9ea15529..d8b8652d 100644
--- 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
index e38bfc33..3e1628ef 100644
--- a/src/TMS_Kalman.h
+++ b/src/TMS_Kalman.h
@@ -63,7 +63,10 @@ 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);
@@ -71,11 +74,19 @@ class TMS_KalmanNode {
     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)
   {