From 90c4d94b13472114daab71d3e368660224423c90 Mon Sep 17 00:00:00 2001
From: Dawsyn Schraiber <32221234+dawsynth@users.noreply.github.com>
Date: Thu, 9 May 2024 01:20:17 -0400
Subject: 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 <jazzjacksonhomebase@gmail.com>
Co-authored-by: Cian Capacci <46614295+BeeGuyDude@users.noreply.github.com>
---
 include/kalmanfilter.hpp | 98 ++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 98 insertions(+)
 create mode 100644 include/kalmanfilter.hpp

(limited to 'include/kalmanfilter.hpp')

diff --git a/include/kalmanfilter.hpp b/include/kalmanfilter.hpp
new file mode 100644
index 0000000..2c8398c
--- /dev/null
+++ b/include/kalmanfilter.hpp
@@ -0,0 +1,98 @@
+#pragma once
+#include "eigen3/Eigen/Dense"
+#include <iostream>
+#include <cmath>
+
+using namespace Eigen;
+
+class KalmanFilter {
+
+    private:
+
+        VectorXf state_vector; // x
+        MatrixXf state_covariance; // P
+
+        MatrixXf state_transition_M; // F
+        MatrixXf control_input_M; // B
+        MatrixXf measurement_M; // H
+
+        MatrixXf process_noise_covariance; // Q
+        MatrixXf measurement_covariance; // R
+
+        MatrixXf I; // Identity
+
+        int n; // State Vector Dimension
+        int p; // Control Vector Dimension
+        int m; // Measurement Vector Dimension
+
+        double dt; // timestep
+
+        /**
+         * @brief Initialize all necessary matrices.
+         * 
+         */
+        void matrixInit();
+
+        /**
+         * @brief Update any existing variable elements in your State Transition 
+         * & Control Input matrices.
+         * 
+         */
+        void updateMatrices();
+
+        /**
+         * @brief Predict current State Vector & State Covariance 
+         * given the current control input.
+         * 
+         * @param control_vec The control input to be applied to the
+         * previous State Vector
+         */
+        void prediction(VectorXf control_vec);
+
+        /**
+         * @brief Correct the State Vector & State Covariance predictions
+         * given a related current measurement.
+         * 
+         * @param measurement Current measurement
+         */
+        void update(VectorXf measurement);
+
+    public:
+
+        KalmanFilter();
+        
+        /**
+         * @brief Construct a new Kalman Filter object
+         * Set the sizes of the Filter's user inputs
+         * 
+         * @param state_dim State Vector Dimension. i.e. dim(x)
+         * @param control_dim Control/Input Vector Dimension. i.e. dim(u)
+         * @param measurement_dim Measurement Vector Dimension. i.e. dim(z)
+         * @param dt timestep
+         */
+        KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt);
+
+
+        /**
+         * @brief Optional function to set a custom initial state for the Filter.
+         * If not called, State Vector & State Covariance are zero-initialized 
+         * 
+         * @param state_vec Initial State Vector
+         * @param state_cov Initial State Covariance
+         * 
+         * @return Whether state initialization was successful
+         */
+        bool setInitialState(VectorXf state_vec, MatrixXf state_cov);
+
+        /**
+         * @brief Perform Kalman Filter operation with given control input vector
+         * and measurement.
+         * 
+         * @param control current control command
+         * @param measurement current measurement
+         * @param dt timestep
+         * 
+         * @return Filtered state vector
+         */
+        VectorXf run(VectorXf control, VectorXf measurement, double _dt);
+};
\ No newline at end of file
-- 
cgit v1.2.3