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>
---
 src/kalmanfilter.cpp | 106 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 106 insertions(+)
 create mode 100644 src/kalmanfilter.cpp

(limited to 'src/kalmanfilter.cpp')

diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp
new file mode 100644
index 0000000..735e8c8
--- /dev/null
+++ b/src/kalmanfilter.cpp
@@ -0,0 +1,106 @@
+#include "../include/kalmanfilter.hpp"
+
+// Private----------------------------------------------------------------------
+void KalmanFilter::matrixInit() {
+
+    state_vector.setZero(n);
+    state_covariance.setZero(n, n); 
+    state_transition_M = MatrixXf::Zero(n, n);  
+    control_input_M = MatrixXf::Zero(n, p);
+    I = MatrixXf::Identity(n, n);
+    measurement_M.setIdentity(m, n); // Setup Measurement Matrix
+    process_noise_covariance = MatrixXf::Zero(n, n); 
+    measurement_covariance = MatrixXf::Zero(m, m); 
+
+    // Setup State Transition Matrix
+    state_transition_M << 1.0, dt, 
+        0.0, 1.0;
+    
+    // Setup Control Input Matrix
+    control_input_M << 0.5 * std::pow(dt, 2), // (Linear Displacement Eq.)
+        dt; 
+    
+    // Setup Process-Noise Covariance
+    process_noise_covariance(0,0) = 0.01;
+    process_noise_covariance(1,1) = 0.1;
+
+    // Setup Measurement Covariance
+    measurement_covariance << 0.1;
+}
+
+
+void KalmanFilter::updateMatrices() {
+
+    state_transition_M(0, 1) = dt;
+    control_input_M(0, 0) = 0.5 * std::pow(dt, 2);
+    control_input_M(1, 0) = dt;
+}
+
+
+void KalmanFilter::prediction(VectorXf control_vec) {
+
+    state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec);
+    state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance;
+}
+
+void KalmanFilter::update(VectorXf measurement) {
+
+    // Innovation
+    VectorXf y = measurement - (measurement_M * state_vector);
+
+    // Residual/Innovation Covariance
+    MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance;
+
+    // Kalman Gain
+    MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse();
+
+    // Update
+    state_vector = state_vector + (K * y);
+    state_covariance = (I - (K * measurement_M)) * state_covariance;
+}
+
+
+
+// Public----------------------------------------------------------------------
+KalmanFilter::KalmanFilter() {
+    
+}
+
+
+KalmanFilter::KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt) 
+    : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) {
+
+    matrixInit();
+}
+
+bool KalmanFilter::setInitialState(VectorXf state_vec, MatrixXf state_cov) {
+
+    if (state_vec.size() != n || state_cov.rows() != n) {
+        std::cout << "Error: Max State & Covariance Dimension should be " << n << std::endl;
+        return false;
+    }
+
+    state_vector = state_vec;
+    state_covariance = state_cov;
+    return true;
+}
+
+
+
+
+VectorXf KalmanFilter::run(VectorXf control, VectorXf measurement, double _dt) {
+    
+     if (control.size() != p || measurement.size() != m) {
+        std::cout << "Error: Control Vector Size should be "<< p 
+            << " Measurement Vector Size should be " << m << std::endl;
+        return state_vector;
+    }
+
+    dt = _dt;
+    updateMatrices();
+
+    prediction(control);
+    update(measurement);
+
+    return state_vector;
+}
\ No newline at end of file
-- 
cgit v1.2.3