mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-07-25 23:51:35 +00:00
Resolve "Implement CI/CD"
This commit is contained in:
30
matlabHelpers/LQR.m
Normal file
30
matlabHelpers/LQR.m
Normal file
@@ -0,0 +1,30 @@
|
||||
clear all; clc;
|
||||
|
||||
syms R11 R22 R33
|
||||
R = [R11 0 0;
|
||||
0 R22 0;
|
||||
0 0 R33];
|
||||
|
||||
syms I33
|
||||
Q = eye(6) * I33;
|
||||
|
||||
syms F11 F22 F33
|
||||
F = [F11 0 0;
|
||||
0 F22 0
|
||||
0 0 F33];
|
||||
|
||||
syms G13 G31
|
||||
G = [0 0 G13;
|
||||
0 0 0;
|
||||
G31 0 0];
|
||||
|
||||
syms I11 I22 d
|
||||
I = [I11 0 0;
|
||||
0 I22 0;
|
||||
0 0 I33];
|
||||
|
||||
A = [zeros(3,3), d * eye(3); F, G];
|
||||
B = [zeros(3,3); inv(I)];
|
||||
P = [-Q, -A'] * pinv([A, -B*inv(R)*B']);
|
||||
|
||||
K = simplify((R^-1) * B' * P)
|
||||
66
matlabHelpers/gainCalc.m
Normal file
66
matlabHelpers/gainCalc.m
Normal 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)
|
||||
|
||||
98
matlabHelpers/simPlot.m
Normal file
98
matlabHelpers/simPlot.m
Normal file
@@ -0,0 +1,98 @@
|
||||
clear all; close all; clc;
|
||||
|
||||
% Read data and transfer to variables
|
||||
T = readmatrix('../simOut.csv');
|
||||
t = T(:, 1);
|
||||
x = T(:, 2);
|
||||
y = T(:, 3);
|
||||
z = T(:, 4);
|
||||
vx = T(:, 5);
|
||||
vy = T(:, 6);
|
||||
vz = T(:, 7);
|
||||
ax = T(:, 8);
|
||||
ay = T(:, 9);
|
||||
az = T(:, 10);
|
||||
yaw = T(:, 11);
|
||||
pitch = T(:, 12);
|
||||
roll = T(:, 13);
|
||||
yawdot = T(:, 14);
|
||||
pitchdot = T(:, 15);
|
||||
rolldot = T(:, 16);
|
||||
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)
|
||||
plot(t, az)
|
||||
title('Acceleration vs Time')
|
||||
xlabel('Time (s)')
|
||||
ylabel('Acceleration (g''s)')
|
||||
|
||||
% Velocity
|
||||
subplot(3, 1, 2)
|
||||
plot(t, vz)
|
||||
title('Velocity vs Time')
|
||||
xlabel('Time (s)')
|
||||
ylabel('Velocity (m/s)')
|
||||
|
||||
% Altitude
|
||||
subplot(3, 1, 3)
|
||||
plot(t, z)
|
||||
title('Altitude vs Time')
|
||||
xlabel('Time (s)')
|
||||
ylabel('Altitude (m)')
|
||||
ylim([0 z(1)+5])
|
||||
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
|
||||
|
||||
figure(2)
|
||||
|
||||
% Euler Angles
|
||||
subplot(2, 1, 1)
|
||||
hold on;
|
||||
plot(t, yaw)
|
||||
plot(t, pitch)
|
||||
plot(t, roll)
|
||||
title('Euler Angles vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Euler Angles (deg)')
|
||||
legend("yaw", "pitch", "roll")
|
||||
|
||||
% Angular Velocity
|
||||
subplot(2, 1, 2)
|
||||
hold on;
|
||||
plot(t, yawdot)
|
||||
plot(t, pitchdot)
|
||||
plot(t, rolldot)
|
||||
title('Angular Velocity vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Angular Velocity (deg/s)')
|
||||
saveas(gcf,'outputs/Euler Angles vs Time.png')
|
||||
legend("yawdot", "pitchdot", "rolldot")
|
||||
|
||||
figure(3)
|
||||
|
||||
% Servo 1 Position
|
||||
subplot(2, 1, 1)
|
||||
plot(t, Servo1)
|
||||
title('Servo 1 Position vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Servo 1 Position (rad)')
|
||||
|
||||
% Servo 2 Position
|
||||
subplot(2, 1, 2)
|
||||
plot(t, Servo2)
|
||||
title('Servo 2 Position vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Servo 2 Position (rad)')
|
||||
saveas(gcf,'outputs/Servo Position vs Time.png')
|
||||
Reference in New Issue
Block a user