Completed simulation and usage example.

CPP_Implementation/main.cpp

@@ -6,7 +6,7 @@
 #include "apid_controller.h"
 
 // Simple first order system
-double systemResponse(double input, double measurement, double dt) {
+double simulate_system(double input, double measurement, double dt) {
     return (input - measurement) * dt ;
 }
 
@@ -18,12 +18,59 @@ int main() {
 
     aPIDController apid(Kp, Ki, Kd, dt);
 
-    double target = 1.0;
+    int n_centers = 5;
-    double measured_value = 0.0;
+    int input_dim = 3;
+    double sigma = 1.0;
+    RBFModel rbf(n_centers, input_dim, sigma, true);
 
-    double control_signal = apid.update(target, measured_value);
+    double target = 1.0, measured_value = 0.0, learning_rate = 0.01;
 
-    std::cout << "Output: " << control_signal << std::endl;
+    for (int step = 0; step < 100; ++step) {
+        double error = target - measured_value;
+
+        double control_signal = apid.update(target, measured_value);
+
+        double gains[3] = {apid.get_Kp(), apid.get_Ki(), apid.get_Kd()};
+        rbf.adapt(error, learning_rate, gains);
+
+        control_signal += rbf.predict(gains);
+        
+        measured_value += simulate_system(control_signal, measured_value, dt);
+
+        std::cout << "Step: " << step 
+                << ", Target: " << target
+                << ", Measured: " << measured_value
+                << ", Error: " << error
+                << ", Control Signal: " << control_signal
+                << std::endl;
+    }
+    
+    // Example using training on recorded data
+    std::cout << "Training Example" << std::endl;
+    double inputs[] = {1.0, 0.1, 0.01, 1.0, 0.1, 0.01}; 
+    double targets[] = {1.0, 1.0}; 
+
+    rbf.train(inputs, targets, 2, 100, 0.01);
+
+    for (int step = 0; step < 100; ++step) {
+        double error = target - measured_value;
+
+        double control_signal = apid.update(target, measured_value);
+
+        double gains[3] = {apid.get_Kp(), apid.get_Ki(), apid.get_Kd()};
+        rbf.adapt(error, learning_rate, gains);
+
+        control_signal += rbf.predict(gains);
+        
+        measured_value += simulate_system(control_signal, measured_value, dt);
+
+        std::cout << "Step: " << step 
+                << ", Target: " << target
+                << ", Measured: " << measured_value
+                << ", Error: " << error
+                << ", Control Signal: " << control_signal
+                << std::endl;
+    }
 
     return 0;
 }