update

jiedxu · Dec 14, 2019 · 890d042 · 890d042
1 parent f9f9059
commit 890d042
Show file tree

Hide file tree

Showing 5 changed files with 116 additions and 11 deletions.
diff --git a/examples/dynamic/project/README.tex.md b/examples/dynamic/project/README.tex.md
@@ -81,6 +81,18 @@ $$
 \end{align}
 $$
 
+or
+
+$$
+\begin{align}
+	\lambda^z_{i+1} &= \lambda^{z}_{i} \\
+	\lambda^y_{i+1} &= \lambda^{y}_{i} - m g \\
+	\theta_i &= \arg \min_{\theta_i} \left\{ m g y_{i} + \frac{1}{2} m g l \sin(\theta_i) + \lambda^z_{i+1} \left[ z_{i} + \cos \left(\theta_{i} \right) \right] + \lambda^y_{i+1} \left[ y_{i} + \sin \left(\theta_{i} \right) \right] \right\} \\
+	z_{i+1} &= z_{i} + \cos \left(\theta_{i} \right) \\
+	y_{i+1} &= y_{i} + \sin \left(\theta_{i} \right) \\
+\end{align}
+$$
+
 When $h = 6, L = 10, M = 14, N = 6$, the results are:
 
 The value of the costate vector at 0, $\left[ \lambda^{z}_{i}, \lambda^{y}_{i} \right]^{T}_{0}$, is:

diff --git a/src/dynamic/cal_chain.m b/src/dynamic/cal_chain.m
@@ -1,5 +1,6 @@
 
-function [err, z_t, y_t, lambda_z_t, lambda_y_t] = cal_chain(vec_guess, s_data)
+function [err, z_t, y_t, theta_t, lambda_z_t, lambda_y_t] = ...
+	cal_chain(vec_guess, s_data)
 
 m = s_data.m;
 g = s_data.g;
@@ -23,13 +24,58 @@
 for j = 0:(n-1)
 	i = j + 1;
 	% [z_t(i); y_t(i); lambda_z_t(i); lambda_y_t(i)]
-
 	lambda_z_t(i+1) = lambda_z_t(i);
 	lambda_y_t(i+1) = lambda_y_t(i) - m * g;
-	theta_t(i) = atan(lambda_y_t(i+1) + 0.5 * m * g * l / lambda_z_t(i+1));
+	% [z_t(i), y_t(i), lambda_z_t(i+1), lambda_y_t(i+1), m, g, l]
+	% [theta_t(i), ~, ~, ~] = min_theta_i(z_t(i), y_t(i), lambda_z_t(i+1), ...
+	% 	lambda_y_t(i+1), m, g, l);
+	theta_t(i) = min_theta_i(z_t(i), y_t(i), lambda_z_t(i+1), ...
+		lambda_y_t(i+1), m, g, l);
+	% theta_t(i) = atand(lambda_y_t(i+1) + 0.5 * m * g * l / lambda_z_t(i+1));
 	z_t(i+1) = z_t(i) + cos(theta_t(i));
 	y_t(i+1) = y_t(i) + sin(theta_t(i));
 end
 
 %% the error in the end point condition
-err = (z_t(i+1) - h)^2 + y_t(i+1)^2;
+% err = (z_t(i+1) - h)^2 + y_t(i+1)^2;
+err = [z_t(i+1) - h; y_t(i+1)];
+
+
+% function [err, z_t, y_t, x, lambda_z_t, lambda_y_t] = ...
+% 	cal_chain(vec_guess, s_data)
+%
+% m = s_data.m;
+% g = s_data.g;
+% l = s_data.l;
+% n = s_data.n;
+% h = s_data.h;
+%
+% %% Initialize the Vectors to Store the Results
+% z_t = zeros(n, 1);
+% y_t = zeros(n, 1);
+% lambda_z_t = zeros(n, 1);
+% lambda_y_t = zeros(n, 1);
+% x = [0; 0];
+%
+% z_t(1) = s_data.z_0;
+% y_t(1) = s_data.y_0;
+% lambda_z_t(1) = vec_guess(1);
+% lambda_y_t(1) = vec_guess(1);
+%
+% %% Begin Iteration
+% for j = 0:(n-1)
+% 	i = j + 1;
+% 	% [z_t(i); y_t(i); lambda_z_t(i); lambda_y_t(i)]
+% 	lambda_z_t(i+1) = lambda_z_t(i);
+% 	lambda_y_t(i+1) = lambda_y_t(i) - m * g;
+% 	% [z_t(i), y_t(i), lambda_z_t(i+1), lambda_y_t(i+1), m, g, l]
+% 	[x, ~, ~, ~] = min_theta_i(z_t(i), y_t(i), lambda_z_t(i+1), ...
+% 		lambda_y_t(i+1), m, g, l);
+% 	% theta_t(i) = atand(lambda_y_t(i+1) + 0.5 * m * g * l / lambda_z_t(i+1));
+% 	z_t(i+1) = z_t(i) + x(2);
+% 	y_t(i+1) = y_t(i) + x(1);
+% end
+%
+% %% the error in the end point condition
+% % err = (z_t(i+1) - h)^2 + y_t(i+1)^2;
+% err = [z_t(i+1) - h; y_t(i+1)];
diff --git a/src/dynamic/get_data_chain.m b/src/dynamic/get_data_chain.m
@@ -2,7 +2,7 @@
 function s_data = get_data_chain(varargin)
 
 l_total = 10;
-n = 6;
+n = 20;
 m_total = 14;
 
 s_data = struct(...

diff --git a/src/dynamic/min_theta_i.m b/src/dynamic/min_theta_i.m
@@ -0,0 +1,45 @@
+
+%
+% function [sol, fval, exitflag, output] = ...
+% 	min_theta_i(z_i, y_i, lambda_z_i1, lambda_y_i1, m, g, l)
+%
+% theta_i = optimvar('theta_i', 'LowerBound', -pi/2, 'UpperBound', pi/2);
+% obj = m * g * y_i + 0.5 * m * g * l * sin(theta_i) + ...
+% 	lambda_z_i1 * (z_i + cos(theta_i)) + lambda_y_i1 * (y_i + sin(theta_i));
+% prob = optimproblem('Objective',obj);
+% x_0.theta_i = 0;
+%
+% [sol, fval, exitflag, output] = solve(prob, x_0);
+
+
+% function [sol_theta_i, fval, exitflag, output] = ...
+% 	min_theta_i(z_i, y_i, lambda_z_i1, lambda_y_i1, m, g, l)
+%
+% x = optimvar('x', 2, 1, 'LowerBound', [-1.0; 0.0], 'UpperBound', [1.0; 1.0]);
+% obj = m * g * y_i + 0.5 * m * g * l * x(1) + ...
+% 	lambda_z_i1 * (z_i + x(2)) + lambda_y_i1 * (y_i + x(1));
+% prob = optimproblem('Objective', obj);
+% nlcons = x(1)^2 + x(2)^2 - 1 <= 10E-6;
+% prob.Constraints.circlecons = nlcons;
+% x_0.x = [1 / sqrt(2); 1 / sqrt(2)];
+%
+% [sol, fval, exitflag, output] = solve(prob, x_0);
+% sol_theta_i = asin(sol.x(1));
+% % sol_x = sol.x;
+
+
+function sol_theta_i = ...
+	min_theta_i(z_i, y_i, lambda_z_i1, lambda_y_i1, m, g, l)
+
+fun = @(theta_i) m * g * y_i + 0.5 * m * g * l * sin(theta_i) + ...
+	lambda_z_i1 * (z_i + cos(theta_i)) + lambda_y_i1 * (y_i + sin(theta_i));
+A = [];
+b = [];
+Aeq = [];
+beq = [];
+lb = -pi / 2;
+ub = pi / 2;
+nonlcon = [];
+x0 = 0;
+options = optimoptions('fmincon', 'Display', 'off', 'Algorithm', 'sqp');
+sol_theta_i = fmincon(fun, x0, A, b, Aeq, beq, lb, ub, nonlcon, options);
diff --git a/src/dynamic/solve_project.m b/src/dynamic/solve_project.m
@@ -3,14 +3,16 @@
 
 s_data = get_data_chain();
 
-opt = optimset('fsolve');
-opt = optimset(opt, 'Display', 'iter');
+opt = optimoptions('fsolve', 'Display', 'iter');
+% , 'Algorithm', 'levenberg-marquardt'
 
-vec_guess_star = fsolve(@(vec_guess) cal_chain(vec_guess, s_data), [0; 0], opt);
-[~, z_t, y_t, lambda_z_t, lambda_y_t] = cal_chain(vec_guess_star, s_data);
+vec_guess_star = fsolve(@(vec_guess) cal_chain(vec_guess, s_data), ...
+	[10; 10], opt);
+[err_star, z_t, y_t, theta_t, lambda_z_t, lambda_y_t] = ...
+	cal_chain(vec_guess_star, s_data)
 
-z_t
-y_t
+% [err_star, z_t, y_t, theta_t, lambda_z_t, lambda_y_t] = ...
+% 	cal_chain([2; 2], s_data)
 
 %% Plot the results