slight controller tuning

Author: RobertJN64

lib/controller/ASTRAv2_Controller.cpp

@@ -40,7 +40,7 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, constantsASTRA_t const
   Vector3 PosError = PosTarget - X.segment<3>(4);
 
   // Velocity Command
-  Vector3 K_P = (Vector3() << 0.58, 0.58, 0.65).finished();
+  Vector3 K_P = (Vector3() << 0.7, 0.7, 0.65).finished();
   Vector3 VelTarget = K_P.cwiseProduct(PosError);
 
   // Velocity Saturation Step
@@ -57,8 +57,8 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, constantsASTRA_t const
   Vector3 Clamp = (Vector3() << 5, 5, 5).finished();
 
   // Normalize errors (0 to 1 scale)
-  Vector3 MaxAttError = (Vector3() << 0.05, 0.05, 0.3).finished();
-  Vector3 MaxVelError = (Vector3() << 0.3, 0.3, 0.3).finished();
+  Vector3 MaxAttError = (Vector3() << 0.06, 0.06, 0.3).finished();
+  Vector3 MaxVelError = (Vector3() << 0.3, 0.3, 0.6).finished();
   Vector3 NormAttErr = lastAttError.cwiseAbs().cwiseQuotient(MaxAttError);
   Vector3 NormVelErr = VelError.cwiseAbs().cwiseQuotient(MaxVelError);
 
@@ -72,7 +72,7 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, constantsASTRA_t const
   K_I = K_I.cwiseProduct(Gate);
   VelErrorI = VelErrorI + K_I.cwiseProduct(VelError) * dT;
   VelErrorI = VelErrorI.cwiseMin(Clamp).cwiseMax(-Clamp);
-  K_P = (Vector3() << 3.1, 3.1, 5).finished();
+  K_P = (Vector3() << 2.4, 2.4, 5).finished();
 
   // Acceleration Target
   Vector3 AccelTarget = K_P.cwiseProduct(VelError) + VelErrorI + (Vector3() << 0, 0, constantsASTRA.g).finished();
@@ -125,7 +125,6 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, constantsASTRA_t const
   Vector3 u_components = -constantsASTRA.K_Att * X_Err;
   U[0] = u_components[0];
   U[1] = u_components[1];
-  // U[2] is set above
   U[3] = u_components[2];
 
   // Controls Saturation

lib/controller/FlightEstimator.cpp

@@ -59,8 +59,8 @@ Vector19 FlightEstimator(Vector19 x_est, constantsASTRA_t constantsASTRA, Vector
     H.block<3, 3>(3, 6) = Matrix3_3::Identity();
 
     // Measurement Covariance Matrix
-    float gps_pos_covar = 1 * RTK + 100 * (1 - RTK);
-    float gps_vel_covar = gps_pos_covar * 1;
+    float gps_pos_covar = 0.2 * RTK + 10 * (1 - RTK);
+    float gps_vel_covar = 0.75;
     Matrix6_6 R = (Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished().asDiagonal();
 
     // A priori covariance and Kalman gain

lib/controller/GroundEstimator.cpp

@@ -84,8 +84,8 @@ Vector19 GroundEstimator(Vector19 x_est, constantsASTRA_t constantsASTRA, Vector
     H.block<3, 3>(3, 6) = Matrix3_3::Identity();
 
     // Measurement Covariance Matrix
-    float gps_pos_covar = 1 * RTK + 130 * (1 - RTK);
-    float gps_vel_covar = gps_pos_covar * 0.1;
+    float gps_pos_covar = 0.5 * RTK + 10 * (1 - RTK);
+    float gps_vel_covar = 0.5;
     R = (Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished().asDiagonal();
 
     // A priori covariance and Kalman gain

lib/controller/matlab_funcs.h

@@ -1,7 +1,7 @@
 #pragma once
 #include <ArduinoEigenDense.h>
 
-// Note - using matlab funcs based off of ASTRA simulation: e5e0557ebb837f220d9740cd96e1020724bceb48
+// Note - using matlab funcs based off of ASTRA simulation: 3a2af9e756f0981da411d93f59ee6b421d5fa234
 // TODO - update matlab func commit
 
 using Vector3 = Eigen::Matrix<float, 3, 1>;