Adds another way to find the trim condition, works on LQR controller …

…at that fixed point.

FindTrim.m

@@ -0,0 +1,28 @@
+% find trim condition for the aircraft using fmincon
+
+
+
+Ceq = @(in) tbsc_model_less_vars(in(1:2), in(3:5));
+
+
+x  = lsqnonlin(Ceq, [0; 20; 0; 0; 3.5])
+
+% 
+% A = [];
+% b = [];
+% Aeq = [];
+% beq = [];
+% lb = [];
+% ub = [];
+% 
+% 
+% sol = fmincon([], [0; 20; 0; 0; 3.5], A, b, Aeq, beq, lb, ub, @fmincon_nonlincon);
+
+
+
+
+
+
+
+
+parameters = { 1.92, 1.84, 2.41, 0.48, 0.57 };

FindTrimDrake.m

@@ -1,4 +1,4 @@
-% find fixed point
+%% find fixed point
 
 clear
 
@@ -38,4 +38,25 @@
 full_state(5) = x(1);
 full_state(7) = x(2);
 
-p.dynamics(0, full_state, x(3:5))
+p.dynamics(0, full_state, x(3:5))
+
+
+%% build lqr controller based on that trim
+
+x0 = zeros(12, 1);
+x0(5) = x(1);
+x0(7) = x(2);
+
+u0 = zeros(3,1);
+
+u0(1) = x(3);
+u0(2) = x(4);
+u0(3) = x(5);
+
+Q = eye(12);
+R = 10*eye(3);
+
+
+[A, B, C, D, xdot0] = p.linearize(0, x0, u0); % TODO: this isn't perfect even at the linearization point (?)
+
+K = lqr(full(A), full(B), Q, R);