update

jiedxu · Dec 13, 2019 · aef5f39 · aef5f39
1 parent 3d07d37
commit aef5f39
Show file tree

Hide file tree

Showing 22 changed files with 304 additions and 0 deletions.
diff --git a/examples/dynamic/Continuous/dlq.m b/examples/dynamic/Continuous/dlq.m
@@ -0,0 +1,12 @@
+%------------------------------------------------------------------------
+function dz=dlq(t,z,A,B,P,R,Q,n)
+%------------------------------------------------------------------------
+% Determine the derivative of x and la as function of t, x and la
+% A and B are system matrices
+% Q, R and P are weight matrices in the objective function
+% n is number of states.
+%------------------------------------------------------------------------
+x=z(1:n); la=z(n+1:end);
+u=-inv(R)*B'*la;
+dz=[ A*x+B*u;
+     -Q*x-A'*la];
diff --git a/examples/dynamic/Continuous/ex8.pdf b/examples/dynamic/Continuous/ex8.pdf
diff --git a/examples/dynamic/Continuous/los8.pdf b/examples/dynamic/Continuous/los8.pdf
diff --git a/examples/dynamic/Continuous/loss.m b/examples/dynamic/Continuous/loss.m
@@ -0,0 +1,13 @@
+%------------------------------------------------------------------------
+function err=loss(la0,A,B,x0,P,R,Q,T,n)
+%------------------------------------------------------------------------
+% Determine the error of the terminal condition as function of la0.
+% A and B are system matrices
+% Q, R and P are weight matrices in the objective function
+% n is number of states.
+% x0 is the initial state vector
+%------------------------------------------------------------------------
+z0=[x0;la0'];
+[time,zt]=ode45(@dlq,[0 T/2 T],z0,[],A,B,P,R,Q,n);
+zT=zt(end,:)';  xT=zT(1:n); laT=zT(n+1:end)';
+err=laT-xT'*P;             % Terminal condition
diff --git a/examples/dynamic/Continuous/lqsim.m b/examples/dynamic/Continuous/lqsim.m
@@ -0,0 +1,75 @@
+%------------------------------------------------------------------------
+function lqsim
+%------------------------------------------------------------------------
+% Program for solving the LQ problem
+%------------------------------------------------------------------------
+
+alf=0.05;
+b=-1;
+
+A=alf;                          % System matrix 
+B=b;        
+n=length(A);
+Q=alf^2;                        % Weight matrices in objective function
+R=Q;
+P=Q;
+
+T=10;                           % Final time
+x0=50000;                       % Initial state
+la0=131;                        % First guess on lambda
+% This is a good guess
+
+% Search for correct initial costates
+opt=optimset('fsolve');
+opt=optimset(opt,'Display','off');
+la0=fsolve(@loss,la0,opt,A,B,x0,P,R,Q,T,n)
+
+% Simulation with correct initial costate
+xp0=[x0;la0'];
+[time,xpt]=ode45(@dlq,[0 T],xp0,[],A,B,P,R,Q,n);
+xt=xpt(:,1:n); lat=xpt(:,n+1:end); 
+ut=-inv(R)*B'*lat'; ut=ut';
+
+%------------------------------------------------------------------------
+% The rest (until next function declaraion) is just plotting
+subplot(311);
+plot(time,xt); grid;
+xlabel('Time'); 
+ylabel('State');
+
+subplot(312);
+plot(time,lat); grid;
+xlabel('Time'); 
+ylabel('Costates ');
+
+subplot(313);
+plot(time,ut); grid;
+xlabel('Time'); ylabel('Control input ');
+%------------------------------------------------------------------------
+
+%------------------------------------------------------------------------
+function err=loss(la0,A,B,x0,P,R,Q,T,n)
+%------------------------------------------------------------------------
+% Determine the error of the terminal condition as function of la0.
+% A and B are system matrices
+% Q, R and P are weight matrices in the objective function
+% n is number of states.
+% x0 is the initial state vector
+%------------------------------------------------------------------------
+z0=[x0;la0'];
+[time,zt]=ode45(@dlq,[0 T/2 T],z0,[],A,B,P,R,Q,n);
+zT=zt(end,:)';  xT=zT(1:n); laT=zT(n+1:end)';
+err=laT-xT'*P;             % Terminal condition
+
+%------------------------------------------------------------------------
+function dxp=dlq(t,z,A,B,P,R,Q,n)
+%------------------------------------------------------------------------
+% Determine the derivative of x and la as function of t, x and la
+% A and B are system matrices
+% Q, R and P are weight matrices in the objective function
+% n is number of states.
+%------------------------------------------------------------------------
+x=z(1:n); la=z(n+1:end);
+u=-inv(R)*B'*la;
+dxp=[ A*x+B*u;
+     -Q*x-A'*la];
diff --git a/examples/dynamic/Continuous/runex1.m b/examples/dynamic/Continuous/runex1.m
@@ -0,0 +1,9 @@
+Tspan=0:0.1:30;
+x0=[1;0];
+a=7;
+
+[t,x]=ode45(@vdp,Tspan,x0,[],a);
+
+z=x(:,1);
+plot(t,z); grid; title('Van der Pool oscillator');
+xlabel('time'); ylabel('position');
diff --git a/examples/dynamic/Continuous/runex2.m b/examples/dynamic/Continuous/runex2.m
@@ -0,0 +1,46 @@
+%------------------------------------------------------------------------
+% Program for solving the LQ problem
+%------------------------------------------------------------------------
+
+alf=0.05;
+b=-1;
+
+A=alf;                          % System matrix 
+B=b;        
+n=length(A);
+Q=alf^2;                        % Weight matrices in objective function
+R=Q;
+P=Q;
+
+T=10;                           % Final time
+x0=50000;                       % Initial state
+la0=131;                        % First guess on lambda
+% This is a good guess
+
+% Search for correct initial costates
+opt=optimset('fsolve');
+opt=optimset(opt,'Display','off');
+la0=fsolve(@loss,la0,opt,A,B,x0,P,R,Q,T,n)
+
+% Simulation with correct initial costate
+xp0=[x0;la0'];
+[time,xpt]=ode45(@dlq,[0 T],xp0,[],A,B,P,R,Q,n);
+xt=xpt(:,1:n); lat=xpt(:,n+1:end); 
+ut=-inv(R)*B'*lat'; ut=ut';
+
+%------------------------------------------------------------------------
+% The rest (until next function declaraion) is just plotting
+subplot(311);
+plot(time,xt); grid;
+xlabel('Time'); 
+ylabel('State');
+
+subplot(312);
+plot(time,lat); grid;
+xlabel('Time'); 
+ylabel('Costates ');
+
+subplot(313);
+plot(time,ut); grid;
+xlabel('Time'); ylabel('Control input ');
+%------------------------------------------------------------------------
diff --git a/examples/dynamic/Continuous/startup.m b/examples/dynamic/Continuous/startup.m
@@ -0,0 +1 @@
+format compact
diff --git a/examples/dynamic/Continuous/vdp.m b/examples/dynamic/Continuous/vdp.m
@@ -0,0 +1,4 @@
+function dx=vdp(t,x,a)
+
+x1=x(1); x2=x(2);
+dx=[x2; a*(1-x1^2)*x2-x1];
diff --git a/examples/dynamic/Discrete/exer-solution-1.pdf b/examples/dynamic/Discrete/exer-solution-1.pdf
diff --git a/examples/dynamic/Discrete/fejlf.m b/examples/dynamic/Discrete/fejlf.m
@@ -0,0 +1,24 @@
+function [err,ut,xt,lat]=fejlf(la0)
+% --------------------------------------------------------------------
+% Usage: [err,ut,xt,lat]=fejlf(la0)
+%
+% err is the error in the end point condition (should be zero).
+% la0 is the guess on the initial value of the costate.
+%
+% ut is the optimal input sequence
+% xt is the resulting state trajectory
+% lat is the resulting costate trajectory
+% --------------------------------------------------------------------
+
+parms    % parameters in the problem
+
+la=la0; x=x0;
+ut=[]; lat=la; xt=x;
+for i=0:N-1,
+ la=(la-q*x)/a;
+ u=-b*la/r;
+ x=a*x+b*u;  
+ xt=[xt;x]; lat=[lat;la]; ut=[ut;u];
+end
+err=la-p*x;
+
diff --git a/examples/dynamic/Discrete/parms.m b/examples/dynamic/Discrete/parms.m
@@ -0,0 +1,12 @@
+% Constants etc.
+
+alf=0.05;
+a=1+alf; b=-1;
+x0=50000;
+N=10;
+q=alf^2; r=q; p=q;
+
+%r=10*q;
+%r=q/10;
+%p=0; 
+%p=100*q;
diff --git a/examples/dynamic/Discrete/runex1.m b/examples/dynamic/Discrete/runex1.m
@@ -0,0 +1,9 @@
+echo on
+
+opt=optimset;
+opt=optimset(opt,'Display','iter');
+
+z0=[1; 1]; 
+
+[z,val,flag]=fminsearch('fopt',z0,opt)
+echo off
diff --git a/examples/dynamic/Discrete/runex2.m b/examples/dynamic/Discrete/runex2.m
@@ -0,0 +1,10 @@
+echo on
+
+z0=[1;1];
+
+opt=optimset;
+opt=optimset(opt,'Display','iter');
+
+[z,val,flag]=fsolve('fz',z0,opt)
+
+echo off
diff --git a/examples/dynamic/Discrete/runex3.m b/examples/dynamic/Discrete/runex3.m
@@ -0,0 +1,19 @@
+format bank
+parms
+
+% The search for la0
+la0=10;
+
+opt=optimset('fsolve');
+opt=optimset(opt,'Display','iter');
+
+la=fsolve('fejlf',la0,opt)
+[err,ut,xt,lat]=fejlf(la);
+
+subplot(211);
+bar(ut); grid; title('Input sequence');
+axis([0 15 0 50000]);
+subplot(212);
+bar(xt); grid; title('Balance');
+axis([0 15 0 50000]);
+shg
diff --git a/examples/dynamic/Discrete/startup.m b/examples/dynamic/Discrete/startup.m
@@ -0,0 +1 @@
+format compact
diff --git a/examples/dynamic/README.tex.md b/examples/dynamic/README.tex.md
@@ -0,0 +1,45 @@
+
+## Free Dynamic Programming
+
+Consider the problem of payment of a (study) loan which at the start of the period is 50.000 DKK. Let us focus on the problem for a period of 10 years. We are going to determine the optimal pay back strategy for this loan, i.e. to determine how much we have to pay each year. Assume that the rate of interests is 5% per year ($\alpha = 0.05$) (and that the loan is credited each year), then the dynamics of the problem can be described by:
+
+$$
+x_{i+1} = f(x_i, u_i) = (1 + \alpha) x_{i} - u_{i}
+$$
+
+where $x_i$ is the actual size of the loan (including interests) and $u_i$ is the annual payment.
+
+On the one hand, we are interested in minimizing the amount we have to pay to the bank. On the other hand, we don't want to pay too much at one time. The objective function, which we might use in the minimization could be
+
+$$
+J = \frac{1}{2} p x_{N}^{2} + \sum_{i=0}^{N-1} \left(\frac{1}{2} q x_{i}^{2} + \frac{1}{2} r u_{i}^{2} \right)
+$$
+
+where $q = \alpha^2$. The weights $r$ and $p$ are at our disposal. Let us for a start choose $r = q$ and $p = q$ (but let the parameters be variable in your program in order to change them easily).
+
+### Solution
+
+$$
+\begin{align}
+	N &= 10 \\
+	x_0 &= 50000 \\
+	\phi(x_N) &= \frac{1}{2} p x_{N}^{2} \\
+	L_i(x_i, u_i) &= \frac{1}{2} q x_{i}^{2} + \frac{1}{2} r u_{i}^{2}
+\end{align}
+$$
+
+The Hamiltonian function of the problem is:
+
+$$
+H_i(x_i, u_i, \lambda_{i+1}) = \frac{1}{2} q x_{i}^{2} + \frac{1}{2} r u_{i}^{2} + \lambda_{i+1}^{T} \left[(1 + \alpha) x_{i} - u_{i}  \right]
+$$
+
+The Euler-Lagrange equations can be expressed as:
+
+$$
+\begin{align}
+	x_{i+1} &= (1 + \alpha) x_{i} - u_{i} \\
+	\lambda_{i} &= q x_{i} + (1 + \alpha) \lambda_{i+1} \\
+	0 &= r u_{i} - \lambda_{i+1}
+\end{align}
+$$
diff --git a/examples/dynamic/main.pdf b/examples/dynamic/main.pdf
diff --git a/src/Markov-Chain.md/main.jl b/src/Markov-Chain.md/main.jl
@@ -20,6 +20,8 @@ end
 
 "Calculate limiting probability distribution of a Markov chain family."
 function cal_vec_lpd(stpm::Array{Float64,2})
+
+    hcat([1; 1; 1])
     return vec_lpd
 end
 

diff --git a/src/dynamic/fopt.m b/src/dynamic/fopt.m
@@ -0,0 +1,5 @@
+
+
+function loss = fopt(u)
+	loss = sum((u - [1; 3]).^2) + 1;
+end
diff --git a/src/dynamic/fz.m b/src/dynamic/fz.m
@@ -0,0 +1,5 @@
+
+
+function f = fz(u)
+	f = u - [1; 3];
+end
diff --git a/src/dynamic/main.m b/src/dynamic/main.m
@@ -0,0 +1,12 @@
+
+
+addpath('~/GitHub/MatrixOptim.jl/src/dynamic')
+
+opt = optimset;
+opt = optimset(opt, 'Display', 'iter');
+fminsearch('fopt', [1; 1], opt)
+
+
+opt = optimset;
+opt = optimset(opt, 'Display', 'off');
+fsolve('fz', [1; 1], opt)