From ea8b3b479973edd72354eb7fd159231e9598b51b Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Tue, 26 Sep 2023 09:40:22 -0400
Subject: [PATCH] kalman filter base

oh yeah yeah you already know wtf is going on (i don't)
---
 RELENTLESS/include/KalmanFilter.h | 35 +++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)
 create mode 100644 RELENTLESS/include/KalmanFilter.h

diff --git a/RELENTLESS/include/KalmanFilter.h b/RELENTLESS/include/KalmanFilter.h
new file mode 100644
index 0000000..bf3043c
--- /dev/null
+++ b/RELENTLESS/include/KalmanFilter.h
@@ -0,0 +1,35 @@
+#ifndef _Kalman_h_
+#define _Kalman_h_
+
+class Kalman {
+public:
+    Kalman();
+
+    double getAngle(double newAngle, double newRate, double dt);
+
+    void setAngle(double angle); 
+    double getRate();
+
+    void setQangle(double Q_angle);
+    
+    void setQbias(double Q_bias);
+    void setRmeasure(double R_measure);
+
+    double getQangle();
+    double getQbias();
+    double getRmeasure();
+
+private:
+    // Kfilter vars
+    double Q_angle;
+    double Q_bias;
+    double R_measure;
+
+    double angle;
+    double bias;
+    double rate;
+
+    double P[2][2];
+};
+
+#endif
\ No newline at end of file