02/24/2024 Test Launch Version (BB Black) (#11)

* Adding a 90% completed, compilable but untested ADS

* Made basic changes to actuator & sensor. Also added motor class

* Removed unnecessary .cpp files

* Updated sensor & actuator classes, finished ads, added variable time step to kalman filter, set up all tests for future assertions

* Relocated 'main' to 'active-drag-system.cpp'. Added more info to README

* Removed main.cpp

* Added more details to README

* Changed some function parameters from pass-by-pointer to pass-by-reference. Also removed the std namespace

* Started writing the test cases

* Updated the .gitignore file

* Removed some files that should be gitignored

* Up to date with Jazz's pull request

* Test Launch Branch Created; PRU Servo Control with Test Program

* Added I2C device class and register IDs for MPL [INCOMPLETE SENSOR IMPLEMENTATION]

Needs actual data getting function implementation for both sensors and register IDs for BNO, will implement shortly.

* Partial implementation of MPL sensor

Added startup method, still needs fleshed out data getters and setters and finished I2C implementation. MOST LIKELY WILL HAVE COMPILATION ISSUES.

* *Hypothetically* complete MPL implementation

NEEDS HARDWARE TESTING

* IMU Header and init() method implementation

Needs like, all data handling still lol

* Hypothetically functional (Definitely won't compile)

* We ball?

---------

Co-authored-by: Jazz Jackson <[email protected]>
Co-authored-by: Cian Capacci <[email protected]>

1 files changed, 101 insertions, 0 deletions

diff --git a/test/kalmanFilterTest.cpp b/test/kalmanFilterTest.cpp
new file mode 100644
index 0000000..e3dc1e4
--- /dev/null
+++ b/test/kalmanFilterTest.cpp
@@ -0,0 +1,101 @@
+#include <iostream>
+#include <gtest/gtest.h>
+#include "eigen3/Eigen/Dense"
+#include "../include/kalmanfilter.hpp"
+
+using namespace Eigen;
+
+class KalmanFilterTest : public ::testing::Test {
+
+	protected:
+
+		KalmanFilterTest() {
+
+			kf = new KalmanFilter(2, 1, 1, 1);
+		}
+			
+		//~KalmanFilterTest() {}
+
+		KalmanFilter *kf;
+};
+
+
+/**
+ *	@brief Test Setting the initial state x & P
+ *
+ * **/
+TEST_F(KalmanFilterTest, setInitialState) {
+	
+	VectorXf state_vec(2);
+	MatrixXf state_cov(2, 2);
+	state_vec << 1, 2;
+	state_cov << 1, 3, 4, 9;
+	
+	// Success Case
+	EXPECT_TRUE(kf->setInitialState(state_vec, state_cov));
+
+	// Failure Case
+	VectorXf state_vec2(4);
+	state_vec2 << 1, 2, 3, 4;
+	EXPECT_FALSE(kf->setInitialState(state_vec2, state_cov));
+}
+
+/**
+ *	@brief Test a single iteration of the Kalman Filter
+ *
+ * **/
+TEST_F(KalmanFilterTest, run) {
+	
+	VectorXf control(1);
+	VectorXf measurement(1);
+	control << 1;
+	measurement << 1;
+	VectorXf res(1);
+
+	res = kf->run(control, measurement, 1);
+
+	EXPECT_NEAR(0.5454, res(0), 0.0001);
+	EXPECT_NEAR(1, res(1), 0.0001);
+}
+
+
+/**
+ * @brief Test run() when the time step value is changed between function calls.
+ * 
+ */
+TEST_F(KalmanFilterTest, runChange) {
+	
+	VectorXf control(1);
+	VectorXf measurement(1);
+	control << 1;
+	measurement << 1;
+	VectorXf res(1);
+
+	res = kf->run(control, measurement, 0.1);
+	EXPECT_NEAR(0.09545, res(0), 0.00001);
+	EXPECT_NEAR(0.1, res(1), 0.1);
+
+	res = kf->run(control, measurement, 0.15);
+	EXPECT_NEAR(0.2761, res(0), 0.0001);
+	EXPECT_NEAR(0.3585, res(1), 0.0001);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+