reconvert ASTRAv2_Controller

Author: RobertJN64

lib/controller/ASTRAv2_Controller.cpp

@@ -27,81 +27,62 @@ void ASTRAv2_Controller_reset() {
   lastAttError = Vector3::Zero();
 }
 
-Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, t_constantsASTRA constantsASTRA, float dT) {
+Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, constantsASTRA_t constantsASTRA, float dT) {
   // Controller Limits
   float thrustMax = 1.5 * 9.8; // N
   float gimbalMax = M_PI / 18;
-
-  Vector4 uMin;
-  uMin << -gimbalMax, -gimbalMax, 0.4 * thrustMax, -M_PI / 6;
-  Vector4 uMax;
-  uMax << gimbalMax, gimbalMax, thrustMax, M_PI / 6;
-
+  Vector4 uMin = (Vector4() << -gimbalMax, -gimbalMax, 0.4 * thrustMax, -M_PI / 6).finished();
+  Vector4 uMax = (Vector4() << gimbalMax, gimbalMax, thrustMax, M_PI / 6).finished();
   Vector4 U = Vector4::Zero();
 
-  //// First Loop (P Loop)
+  // First Loop (P Loop)
   // Position Error Vector
   Vector3 PosError = PosTarget - X.segment<3>(4);
 
   // Velocity Command
-  Vector3 K_P;
-  K_P << 0.5, 0.5, 0.65;
+  Vector3 K_P = (Vector3() << 0.58, 0.58, 0.65).finished();
   Vector3 VelTarget = K_P.cwiseProduct(PosError);
 
   // Velocity Saturation Step
-  Vector3 MaxVel;
-  MaxVel << 1, 1, 1.5;
+  Vector3 MaxVel = (Vector3() << 1, 1, 1.5).finished();
   VelTarget = VelTarget.cwiseMin(MaxVel).cwiseMax(-MaxVel);
 
-  //// Second Loop (PI Loop)
+  // Second Loop (PI Loop)
   // Velocity Error Vector
   Vector3 VelError = VelTarget - X.segment<3>(7);
 
   // Integral Accumulator
-  Vector3 K_I;
-  K_I << 1.5, 1.5, 5;
-  float Leak = 0.10;
-  Vector3 Clamp;
-  Clamp << 1, 1, 2;
+  Vector3 K_I = (Vector3() << 2, 2, 5).finished();
+  float Leak = 0.35;
+  Vector3 Clamp = (Vector3() << 5, 5, 5).finished();
 
   // Normalize errors (0 to 1 scale)
-  Vector3 MaxAttError;
-  MaxAttError << 0.07, 0.07, 0.3;
-  Vector3 MaxVelError;
-  MaxVelError << 0.6, 0.6, 0.4;
-  Vector3 MaxRateError;
-  MaxRateError << M_PI / 5, M_PI / 5, M_PI / 3;
+  Vector3 MaxAttError = (Vector3() << 0.05, 0.05, 0.3).finished();
+  Vector3 MaxVelError = (Vector3() << 0.3, 0.3, 0.3).finished();
   Vector3 NormAttErr = lastAttError.cwiseAbs().cwiseQuotient(MaxAttError);
   Vector3 NormVelErr = VelError.cwiseAbs().cwiseQuotient(MaxVelError);
-  Vector3 NormRateErr = X.segment<3>(10).cwiseAbs().cwiseQuotient(MaxRateError);
 
   // Combine errors (Vector magnitude)
-  Vector3 TotalErrorMetric = NormAttErr + NormVelErr + NormRateErr;
+  Vector3 TotalErrorMetric = NormAttErr + NormVelErr;
 
   // Calculate Gate using Gaussian function
-  Vector3 LeakVec3;
-  LeakVec3 << Leak, Leak, Leak;
-  Vector3 Gate = (1 - Leak) * (-2 * TotalErrorMetric.cwiseAbs2()).array().exp().matrix() + LeakVec3;
+  Vector3 Gate = (1 - Leak) * (-2 * TotalErrorMetric.cwiseAbs2()).array().exp().matrix() + (Vector3() << Leak, Leak, Leak).finished();
 
   // Integrator
   K_I = K_I.cwiseProduct(Gate);
   VelErrorI = VelErrorI + K_I.cwiseProduct(VelError) * dT;
   VelErrorI = VelErrorI.cwiseMin(Clamp).cwiseMax(-Clamp);
-  K_P << 2.2, 2.2, 3.5;
+  K_P = (Vector3() << 3.1, 3.1, 5).finished();
 
   // Acceleration Target
-  Vector3 g_vec;
-  g_vec << 0, 0, constantsASTRA.g;
-  Vector3 AccelTarget = K_P.cwiseProduct(VelError) + VelErrorI + g_vec;
+  Vector3 AccelTarget = K_P.cwiseProduct(VelError) + VelErrorI + (Vector3() << 0, 0, constantsASTRA.g).finished();
 
   // Acceleration Saturation Step
-  Vector3 MaxAccelUp;
-  MaxAccelUp << 2, 2, 15;
-  Vector3 MaxAccelDown;
-  MaxAccelDown << -2, -2, 4;
+  Vector3 MaxAccelUp = (Vector3() << 2, 2, 15).finished();
+  Vector3 MaxAccelDown = (Vector3() << -2, -2, 4).finished();
   AccelTarget = AccelTarget.cwiseMin(MaxAccelUp).cwiseMax(MaxAccelDown);
 
-  //// Kinematics Step
+  // Kinematics Step
   // Compute thrust target
   Vector3 TargetForce_I = constantsASTRA.m * AccelTarget;
   U[2] = TargetForce_I.norm();
@@ -111,10 +92,9 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, t_constantsASTRA const
   Vector3 Z_b = AccelTarget / AccelTarget.norm();
 
   // Heading reference (+X axis rolled to north)
-  Vector3 HDGRef;
-  HDGRef << 1, 0, 0;
+  Vector3 HDGRef = (Vector3() << 1, 0, 0).finished();
   Vector3 Y_b = Z_b.cross(HDGRef);
-  Y_b = Y_b / Y_b.norm();
+  Y_b.normalize();
 
   // Complete the triad
   Vector3 X_b = Y_b.cross(Z_b);
@@ -124,24 +104,22 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, t_constantsASTRA const
   DCM << X_b[0], Y_b[0], Z_b[0], X_b[1], Y_b[1], Z_b[1], X_b[2], Y_b[2], Z_b[2];
   Vector4 TargetAtt = DCM_Quat_Conversion(DCM);
 
-  //// Third Loop (LQRi)
+  // Third Loop (LQRi)
   // Attitude Error computation
-  Vector4 Q_Conj;
-  Q_Conj << X[0], -X.segment<3>(1);
+  Vector4 Q_Conj = (Vector4() << X[0], -X.segment<3>(1)).finished();
   Vector4 AttError = HamiltonianProd(Q_Conj) * TargetAtt;
   if (AttError[0] < 0) {
     AttError = -AttError;
   }
   lastAttError = AttError.segment<3>(1);
 
   // Error accumulation and clamping
-  Clamp << 3, 3, 0.4;
+  Clamp = (Vector3() << 3, 3, 0.4).finished();
   AttErrorI = AttErrorI + AttError.segment<3>(1) * dT;
   AttErrorI = AttErrorI.cwiseMin(Clamp).cwiseMax(-Clamp);
 
   // State vector and error
-  Vector9 X_Err;
-  X_Err << -AttError.segment<3>(1), X.segment<3>(10), AttErrorI;
+  Vector9 X_Err = (Vector9() << -AttError.segment<3>(1), X.segment<3>(10), AttErrorI).finished();
 
   // LQR Controller
   Vector3 u_components = -constantsASTRA.K_Att * X_Err;
@@ -150,8 +128,7 @@ Vector4 ASTRAv2_Controller(Vector3 PosTarget, Vector16 X, t_constantsASTRA const
   // U[2] is set above
   U[3] = u_components[2];
 
-  //// Controls Saturation
-  U = U.cwiseMin(uMax).cwiseMax(uMin);
-
+  // Controls Saturation
+  U = U.cwiseMax(uMin).cwiseMin(uMax);
   return U;
 }

lib/controller/FlightEstimator.cpp

@@ -57,7 +57,7 @@ Vector13 FlightEstimator(Vector13 x_est, constantsASTRA_t constantsASTRA, Vector
     // Measurement Covariance Matrix
     float gps_pos_covar = 1 * RTK + 100 * (1 - RTK);
     float gps_vel_covar = gps_pos_covar * 1;
-    Matrix6_6 R = ((Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished()).asDiagonal();
+    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
     Matrix9_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();

lib/controller/GroundEstimator.cpp

@@ -82,7 +82,7 @@ Vector13 GroundEstimator(Vector13 x_est, constantsASTRA_t constantsASTRA, Vector
     // Measurement Covariance Matrix
     float gps_pos_covar = 1 * RTK + 130 * (1 - RTK);
     float gps_vel_covar = gps_pos_covar * 0.1;
-    R = ((Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished()).asDiagonal();
+    R = (Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished().asDiagonal();
 
     // A priori covariance and Kalman gain
     Matrix18_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();