From 59a947c4785cd1ba7c2866324ed9560ce8b716b2 Mon Sep 17 00:00:00 2001
From: Brendan McGeeney <bpmcgeeney@gmail.com>
Date: Thu, 12 Aug 2021 00:33:05 +0000
Subject: [PATCH] initial commit

---
 calcLQR.m | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)
 create mode 100644 calcLQR.m

diff --git a/calcLQR.m b/calcLQR.m
new file mode 100644
index 0000000..b278b55
--- /dev/null
+++ b/calcLQR.m
@@ -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;