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

fixed LQR

Browse Source
This commit is contained in:
bpmcgeeney 2021-09-12 14:18:22 -07:00
parent 90311ff17e
commit c6b1f0fc2c
14 changed files with 8786 additions and 4339 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
build/debug/gainCalc.m Normal file
View File

@ -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)

BIN
build/debug/main.exe
View File

Binary file not shown.

BIN
build/debug/outputs/Accel-Vel-Alt vs Time.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 39 KiB

BIN
build/debug/outputs/Euler Angles vs Time.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

BIN
build/debug/outputs/Servo Position vs Time.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 35 KiB

8636
build/debug/simOut.csv
View File

File diff suppressed because it is too large Load Diff

17
build/debug/simPlot.m
View File

@ -18,10 +18,19 @@ roll = T(:, 13);
yawdot = T(:, 14);
pitchdot = T(:, 15);
rolldot = T(:, 16);
Servo1 = T(:, 17);
Servo2 = T(:, 18);
m = T(:, 19);
thrust = T(:, 20);
yawddot = T(:, 17);
pitchddot = T(:, 18);
rollddot = T(:, 19);
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)

BIN
build/release/outputs/Accel-Vel-Alt vs Time.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 39 KiB

BIN
build/release/outputs/Euler Angles vs Time.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 31 KiB

BIN
build/release/outputs/Servo Position vs Time.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 34 KiB

4318
build/release/simOut.csv Normal file
View File

File diff suppressed because it is too large Load Diff

2
include/sVars.h
View File

@ -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;

16
src/main.cpp
View File

@ -14,22 +14,23 @@ int main()
Vars.vz = 0; // [m/s]
// Initial YPR
Vars.yaw = 0; // [rad]
Vars.pitch = 0; // [rad]
Vars.roll = 0; // [rad]
Vars.yaw = 15 * 3.1416 / 180; // [rad]
Vars.pitch = 0 * 3.1416 / 180; // [rad]
Vars.roll = 0 * 3.1416 / 180; // [rad]
// Initial YPRdot
Vars.yawdot = 0; // [rad/s]
Vars.pitchdot = 0; // [rad/s]
Vars.rolldot = 0; // [rad/s]
Vars.yawdot = 0 * 3.1416 / 180; // [rad/s]
Vars.pitchdot = 0 * 3.1416 / 180; // [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;
}

70
src/sim.cpp
View File

@ -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 =
2000; // 2000 just an arbitrary number to make sure we don't burn
Vars.burnElapsed = 2000; // arbitrary number to ensure 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) / momentArm;
Vars.LQRy = (K22 * Vars.pitch / 2) / momentArm;
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.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 * momentArm;
Vars.momentY = Vars.Fy * momentArm;
Vars.momentX = Vars.Fx * Vars.momentArm;
Vars.momentY = Vars.Fy * Vars.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 << ", ";