Anson/Lander-Simulink
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-sim.git synced 2025-08-03 11:51:27 +00:00
Code Releases Activity

initial commit

Browse Source
This commit is contained in:
Brendan McGeeney
2021-08-12 00:33:05 +00:00
parent 118df87621
commit 59a947c478
1 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
calcLQR.m Normal file
View File

@@ -0,0 +1,25 @@
function [K] = calcLQR(I)
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;