mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
67 lines
2.2 KiB
Matlab
67 lines
2.2 KiB
Matlab
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 PIDcalc 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)
|
|
|