initial commit

build/release/LQR.m

@@ -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)