Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 15:17:23 +00:00
Code Releases Activity

delete old lqr code

Browse Source
This commit is contained in:
Anson Biggs 2021-09-13 14:00:02 -07:00
parent 9f5cd7bdf7
commit 804bc637b8
1 changed files with 0 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
include/sim.h
View File

@ -128,63 +128,6 @@ void lqrCalc(struct sVars &Vars) {
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;
double k3 = (Vars.I22 - Vars.I11) / Vars.I33;
double R11 = pow(10, 2);
double R22 = pow(10, 2);
double R33 = pow(10, 2);
double F11 = -2 * pow(n, 2) * 4 * k1;
double F22 = -2 * pow(n, 2) * 3 * k2;
double F33 = -2 * pow(n, 2) * k3;
double G31 = n * (1 - k1);
double G13 = n * (k3 - 1);
double d = 0.5;
/*
The following calculations are based on output of LQR.m to avoid working
with matrices in C++ Please see .m file for details on calculation.
Algorithm taken from LQR wikipedia page:
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
*/
/*
double gain = 0.25;
double K11 =
-1 * gain *
(Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) +
R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) /
(R11 * d * Vars.I11 *
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
double K33 =
-1 * gain *
(Vars.I33 *
(Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) +
R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) /
(R33 * d * pow(abs(Vars.I33), 2) *
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
double K34 =
gain * (R11 * pow(abs(Vars.I11), 2) * G13) /
(R33 * Vars.I33 *
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
double K16 =
gain * (R33 * pow(abs(Vars.I33), 2) * G31) /
(R11 * Vars.I11 *
(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
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm;
*/
// Paste in Values from gainCalc.m
double K11 = 39.54316;
double K12 = 0.00000;