fixed LQR

build/debug/gainCalc.m

@@ -0,0 +1,66 @@
+clear all; clc;
+%% User Defined Values
+M0              = 1.2;                                                      % [kg]
+vehicleHeight   = 0.5318;                                                   % [m]
+vehicleRadius   = 0.05105;                                                  % [m]
+
+%% Calcs
+I11         = (1/12) * vehicleHeight^2 + 0.25 * vehicleRadius^2;            % (1/12) * h^2 + 0.25 * r^2
+I22         = (1/12) * vehicleHeight^2 + 0.25 * vehicleRadius^2;            % (1/12) * h^2 + 0.25 * r^2
+I33         = 0.5 * vehicleRadius^2;                                        % 0.5 * r^2
+I           = M0 * [I11 0 0; 0 I22 0; 0 0 I33];
+
+Q = eye(6) * I(3, 3);
+Mu = 398600;
+r0 = [6678; 0; 0];
+
+k1 = (I(2,2) - I(3,3)) / I(1,1);
+k2 = (I(1,1) - I(3,3)) / I(2,2);
+k3 = (I(2,2) - I(1,1)) / I(3,3);
+
+n = sqrt(Mu/(norm(r0)^3));
+
+F = -2 * n^2 * [4 * k1, 0, 0; 0, 3 * k2, 0; 0, 0, k3];
+G = n * [0, 0, (1 - k1); 0, 0, 0; (k3 - 1), 0, 0];
+
+Iinv = [1 / I(1,1), 0, 0; 0, 1 / I(2,2), 0; 0, 0, 1 / I(3,3)];
+
+A = [zeros(3,3), 0.5 * eye(3); F, G];
+B = [zeros(3,3); Iinv];
+
+R = eye(3)*10^-6;
+Rinv = R^-1;
+
+S = icare(A, B, Q, R);
+
+K = Rinv * B' * S;
+
+%% Outputs
+%  Copy results in command window to LQRcalc function in C++
+fprintf("double K11 = %3.5f;\n", K(1, 1))
+fprintf("double K12 = %3.5f;\n", K(1, 2))
+fprintf("double K13 = %3.5f;\n", K(1, 3))
+fprintf("double K14 = %3.5f;\n", K(1, 4))
+fprintf("double K15 = %3.5f;\n", K(1, 5))
+fprintf("double K16 = %3.5f;\n", K(1, 6))
+fprintf("double K21 = %3.5f;\n", K(2, 1))
+fprintf("double K22 = %3.5f;\n", K(2, 2))
+fprintf("double K23 = %3.5f;\n", K(2, 3))
+fprintf("double K24 = %3.5f;\n", K(2, 4))
+fprintf("double K25 = %3.5f;\n", K(2, 5))
+fprintf("double K26 = %3.5f;\n", K(2, 6))
+fprintf("double K31 = %3.5f;\n", K(3, 1))
+fprintf("double K32 = %3.5f;\n", K(3, 2))
+fprintf("double K33 = %3.5f;\n", K(3, 3))
+fprintf("double K34 = %3.5f;\n", K(3, 4))
+fprintf("double K35 = %3.5f;\n", K(3, 5))
+fprintf("double K36 = %3.5f;\n", K(3, 6))
+
+syms yaw pitch roll yawdot pitchdot rolldot gain
+w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]';
+
+syms K11 K12 K13 K14 K15 K16 K21 K22 K23 K24 K25 K26 K31 K32 K33 K34 K35 K36
+K = gain * [K11 K12 K13 K14 K15 K16; K21 K22 K23 K24 K25 K26; K31 K32 K33 K34 K35 K36];
+
+simplify(K*w)
+

build/debug/simPlot.m

@@ -18,10 +18,19 @@ roll        = T(:, 13);
 yawdot      = T(:, 14);
 pitchdot    = T(:, 15);	 
 rolldot     = T(:, 16);
-Servo1      = T(:, 17);
+yawddot     = T(:, 17);
-Servo2      = T(:, 18);
+pitchddot   = T(:, 18);	 
-m           = T(:, 19);
+rollddot    = T(:, 19);
-thrust      = T(:, 20);
+I11         = T(:, 20);
+I22         = T(:, 21);	 
+I33         = T(:, 22);
+I11dot      = T(:, 23);
+I22dot      = T(:, 24);	 
+I33dot      = T(:, 25);
+Servo1      = T(:, 26);
+Servo2      = T(:, 27);
+m           = T(:, 28);
+thrust      = T(:, 29);
 
 % Acceleration
 subplot(3, 1, 1)

include/sVars.h

@@ -21,7 +21,7 @@ struct sVars
     double yawddotPrev, pitchddotPrev, rollddotPrev;
 
     double m, m0, mp, mb, mdot;
-    double vehicleHeight, vehicleRadius;
+    double vehicleHeight, vehicleRadius, momentArm;
 
     double tb;
     double vb;

src/main.cpp

@@ -14,22 +14,23 @@ int main()
     Vars.vz = 0; // [m/s]
 
     // Initial YPR
-    Vars.yaw = 0;   // [rad]
+    Vars.yaw = 15 * 3.1416 / 180;  // [rad]
-    Vars.pitch = 0; // [rad]
+    Vars.pitch = 0 * 3.1416 / 180; // [rad]
-    Vars.roll = 0;  // [rad]
+    Vars.roll = 0 * 3.1416 / 180;  // [rad]
 
     // Initial YPRdot
-    Vars.yawdot = 0;   // [rad/s]
+    Vars.yawdot = 0 * 3.1416 / 180;   // [rad/s]
-    Vars.pitchdot = 0; // [rad/s]
+    Vars.pitchdot = 0 * 3.1416 / 180; // [rad/s]
-    Vars.rolldot = 0;  // [rad/s]
+    Vars.rolldot = 0 * 3.1416 / 180;  // [rad/s]
 
     // Servo Limitation
-    Vars.maxServo = 0; // [degs]
+    Vars.maxServo = 15; // [degs]
 
     // Vehicle Properties
     Vars.m0 = 1.2;                // [kg]
     Vars.vehicleHeight = 0.5318;  // [m]
     Vars.vehicleRadius = 0.05105; // [m]
+    Vars.momentArm = 0.145;       // [m]
 
     // Sim Step Size
     Vars.stepSize = 1; // [ms]
@@ -43,6 +44,7 @@ int main()
     sim(Vars);
 
     std::cout << "Finished";
+    std::cin.get();
 
     return 0;
 }

src/sim.cpp

@@ -28,7 +28,7 @@ void sim(struct sVars &Vars)
     // Output file header. These are the variables that we output - useful for
     // debugging
     outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
-               "pitchdot, rolldot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
+               "pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
                "LQRx, LQRy"
             << std::endl;
 
@@ -108,8 +108,7 @@ void thrustSelection(struct sVars &Vars, int t)
     }
 
     else
-        Vars.burnElapsed =
+        Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
-            2000; // 2000 just an arbitrary number to make sure we don't burn
 
     bool b_thrustSelect;
 
@@ -135,12 +134,15 @@ void thrustSelection(struct sVars &Vars, int t)
 
 void lqrCalc(struct sVars &Vars)
 {
+
     Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
                          pow(Vars.vehicleRadius, 2) / 4);
     Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
                          pow(Vars.vehicleRadius, 2) / 4);
     Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2);
 
+    /*
+
     double n = sqrt(398600 / pow(6678, 3));
     double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
     double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
@@ -164,6 +166,7 @@ void lqrCalc(struct sVars &Vars)
      Algorithm taken from LQR wikipedia page:
      https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
   */
+    /*
     double gain = 0.25;
 
     double K11 =
@@ -190,9 +193,47 @@ void lqrCalc(struct sVars &Vars)
          (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
 
     // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
-    double momentArm = 0.145;
+    Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
-    Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / momentArm;
+    Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm;
-    Vars.LQRy = (K22 * Vars.pitch / 2) / momentArm;
+
+    */
+
+    double K11 = 39.54316;
+    double K12 = 0.00000;
+    double K13 = -0.00000;
+    double K14 = 39.55769;
+    double K15 = 0.00000;
+    double K16 = 0.00000;
+    double K21 = 0.00000;
+    double K22 = 39.54316;
+    double K23 = 0.00000;
+    double K24 = 0.00000;
+    double K25 = 39.55769;
+    double K26 = 0.00000;
+    double K31 = 0.00000;
+    double K32 = 0.00000;
+    double K33 = 39.54316;
+    double K34 = 0.00000;
+    double K35 = 0.00000;
+    double K36 = 39.54394;
+
+    double gain = 0.25 * pow(10, -4);
+
+    // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
+    Vars.LQRx = gain * ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 + K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) / -Vars.momentArm;
+    Vars.LQRy = gain * ((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 + K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) / -Vars.momentArm;
+
+    // LQR Force limiter X
+    if (Vars.LQRx > Vars.thrust)
+        Vars.LQRx = Vars.thrust;
+    else if (Vars.LQRx < -1 * Vars.thrust)
+        Vars.LQRx = -1 * Vars.thrust;
+
+    // LQR Force limiter Y
+    if (Vars.LQRy > Vars.thrust)
+        Vars.LQRy = Vars.thrust;
+    else if (Vars.LQRy < -1 * Vars.thrust)
+        Vars.LQRy = -1 * Vars.thrust;
 }
 
 void TVC(struct sVars &Vars, double g)
@@ -236,9 +277,8 @@ void TVC(struct sVars &Vars, double g)
                   (Vars.m * g);
 
         // Calculate moment created by Fx and Fy
-        double momentArm = 0.145;
+        Vars.momentX = Vars.Fx * Vars.momentArm;
-        Vars.momentX = Vars.Fx * momentArm;
+        Vars.momentY = Vars.Fy * Vars.momentArm;
-        Vars.momentY = Vars.Fy * momentArm;
         Vars.momentZ = 0;
     }
 }
@@ -378,6 +418,18 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t)
     outfile << Vars.pitchdot * 180 / 3.1416 << ", ";
     outfile << Vars.rolldot * 180 / 3.1416 << ", ";
 
+    outfile << Vars.yawddot * 180 / 3.1416 << ", ";
+    outfile << Vars.pitchddot * 180 / 3.1416 << ", ";
+    outfile << Vars.rollddot * 180 / 3.1416 << ", ";
+
+    outfile << Vars.I11 << ", ";
+    outfile << Vars.I22 << ", ";
+    outfile << Vars.I33 << ", ";
+
+    outfile << Vars.I11dot << ", ";
+    outfile << Vars.I22dot << ", ";
+    outfile << Vars.I33dot << ", ";
+
     outfile << Vars.xServoDegs << ", ";
     outfile << Vars.yServoDegs << ", ";