mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-07-23 14:41:25 +00:00
initial commit
This commit is contained in:
bpmcgeeney
30
build/debug/LQR.m
Normal file
30
build/debug/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)
|
||||
BIN
build/debug/main.exe
Normal file
BIN
build/debug/main.exe
Normal file
Binary file not shown.
4318
build/debug/simOut.csv
Normal file
4318
build/debug/simOut.csv
Normal file
File diff suppressed because it is too large
Load Diff
81
build/debug/simPlot.m
Normal file
81
build/debug/simPlot.m
Normal file
@@ -0,0 +1,81 @@
|
||||
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);
|
||||
Servo1 = T(:, 17);
|
||||
Servo2 = T(:, 18);
|
||||
m = T(:, 19);
|
||||
thrust = T(:, 20);
|
||||
|
||||
% 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)
|
||||
plot(t, yaw, pitch, roll)
|
||||
title('Euler Angles vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Euler Angles (deg)')
|
||||
|
||||
% Angular Velocity
|
||||
subplot(2, 1, 2)
|
||||
plot(t, yawdot, pitchdot, rolldot)
|
||||
title('Angular Velocity vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Angular Velocity (deg/s)')
|
||||
saveas(gcf,'outputs/Euler Angles vs Time.png')
|
||||
|
||||
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')
|
||||
30
build/release/LQR.m
Normal file
30
build/release/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)
|
||||
BIN
build/release/main.exe
Normal file
BIN
build/release/main.exe
Normal file
Binary file not shown.
81
build/release/simPlot.m
Normal file
81
build/release/simPlot.m
Normal file
@@ -0,0 +1,81 @@
|
||||
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);
|
||||
Servo1 = T(:, 17);
|
||||
Servo2 = T(:, 18);
|
||||
m = T(:, 19);
|
||||
thrust = T(:, 20);
|
||||
|
||||
% 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)
|
||||
plot(t, yaw, pitch, roll)
|
||||
title('Euler Angles vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Euler Angles (deg)')
|
||||
|
||||
% Angular Velocity
|
||||
subplot(2, 1, 2)
|
||||
plot(t, yawdot, pitchdot, rolldot)
|
||||
title('Angular Velocity vs Time')
|
||||
xlabel('Time (ms)')
|
||||
ylabel('Angular Velocity (deg/s)')
|
||||
saveas(gcf,'outputs/Euler Angles vs Time.png')
|
||||
|
||||
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