多状态量、多控制量，动态规划matlab例程

function varargout = dpm(varargin) %DPM Dynamic Programming using Matrix operations version 1.1.1 % % OPTIONS = DPM() returns the standard options structure. % % DPM(FUN,NX,NU) saves a random test model with the filename FUN.m % and NX number of states and NU number of inputs. % % [[OUT] DYN] = DPM(FUN,PAR,GRD,PRB,OPTIONS) whith FUN is a function % handle with the model function. The DPM-function requires that the % state and input grids are discretized and limited. The function assumes % equally spaced grids and the GRD structure needed is defined as: % GRD.X0{.} = 'initial state' (only used in forward simulation) % GRD.XN{.}.hi = 'final state upper constraint' % GRD.XN{.}.lo = 'final state lower constraint' % GRD.Nx{.} = 'number of elements in the state grid' % GRD.Xn{.}.hi = 'upper boundary of the state grid' % GRD.Xn{.}.lo = 'lower boundary of the state grid' % GRD.Nu{.} = 'number of elements in the input grid' % GRD.Un{.}.hi = 'upper boundary of the input grid' % GRD.Un{.}.lo = 'lower boundary of the input grid' % PRB contains the problem parameters % PRB.Ts = time step % PRB.N = number of time steps in problem (defines the problem % length) % [PRB.N0] = start time index (only used in forward simulation) % [PRB.W{i}] = (optional) vectors with length PRB.T containing % disturbances. % PAR is a user defined variable that is sent to the model function. % Useful if the model need some parameters defined outside the DPM % function. % OPTIONS is the options structure containing % OPTIONS.Waitbar = 'off'; (on/off to show or hide waitbars) % OPTIONS.Verbose = 'on' (on/off command window status % notification) % OPTIONS.Warnings = 'off'; (on/off to show or hide warnings) % OPTIONS.SaveMap = 'on'; (on/off to save cost-to-go map) % OPTIONS.MyInf = 1e4; (a big number for infeaible states % where out.I=1) % OPTIONS.Minimize = 1; (one/zero if minimizing or maximizing) % OPTIONS.InputType = 'cd' (string with the same number of % characters as number of inputs. % contains the character 'c' if input is % continuous or 'd' if discrete. % Default is all continuous.) % OPTIONS.BoundaryMethod = 'none', 'Line' or 'LevelSet' % if BoundaryMethod = 'Line' % OPTIONS.FixedGrid = 0; (zero/one use grid as defined in GRD or % adjust the according to boundary line) % OPTIONS.Iter = 10; (maximum number of iterations when % inverting model) % OPTIONS.Tol = 1e-8; (minimum tolerance when inverting % model) % endif % OPTIONS.gN{1} Cost matrix at the final time % (must be of size(OPTIONS.gN{1}) = % [GRD.Nx{1} GRD.Nx{2} ... GRD.Nx{.}]). % % % OUT = DPM(DYN,FUN,PAR,GRD,PRB,OPTIONS) only calculates the forward % simulation using DYN calculated earlier. % % OUT = DPM(N0,DYN,FUN,PAR,GRD,PRB,OPTIONS) only calculates the forward % simulation using DYN calculated earlier but from index N0 % (in the range 1->prb.N). % % Up to 5e6 grid points (prod([GRD.Nx{1} GRD.Nx{2} ... GRD.Nx{.} % GRD.Nu{1} GRD.Nu{2} ... GRD.Nu{.}])) works well. % % % EXAMPLES_______________________________________________________________ % DPM can be used to get standard options % options = dpm(); % % DPM can be used to generate a test model % dpm('model_test',3,2); % % FULL DYNAMIC PROGRAMMING EXAMPLES______________________________________ % To calculate the optimal control for a test model: % dpm('model_test',2,2); % grd.Nx{1} = 11; grd.Xn{1}.lo = -50; grd.Xn{1}.hi = 100; % grd.Nx{2} = 11; grd.Xn{2}.lo = -50; grd.Xn{2}.hi = 100; % grd.Nu{1} = 11; grd.Un{1}.lo = -5; grd.Un{1}.hi = 5; % grd.Nu{2} = 11; grd.Un{2}.lo = -5; grd.Un{2}.hi = 5; % % grd.X0{1} = 0; % grd.X0{2} = 0; % grd.XN{1}.lo = 0; grd.XN{1}.hi = 20; % grd.XN{2}.lo = 0; grd.XN{2}.hi = 20; % % prb.Ts = 1/5; % prb.N = 200*1/prb.Ts + 1; % prb.N0 = 1; % prb.W{1} = rand(1,prb.N); % options = dpm(); % [out dyn] = dpm('model_test',[],grd,prb,options); % % % HINTS FOR DEBUGGING____________________________________________________ % The model function is called twice before the actual DP algorithm is % used in order to determine the number of states, inputs, and outputs. % When adding breakpoints to the model function, please be aware of this. % % % REFERENCES_____________________________________________________________ % When using DPM please cite: % Sundstrom, O. and Guzzella, L., "A Generic Dynamic Programming Matlab % Function", In Proceedings of the 18th IEEE International Conference on % Control Applications, pages 1625-1630, Saint Petersburg, Russia, 2009 % % LICENSE________________________________________________________________ % This Source Code Form is subject to the terms of the Mozilla Public % License, v. 2.0. If a copy of the MPL was not distributed with this % file, You can obtain one at http://mozilla.org/MPL/2.0/. % % COPYRIGHT______________________________________________________________ % Copyright 2008- Institute for Dynamic Systems and Control, Department % of Mechanical and Process Engineering, ETH Zurich % Author: Olle L. Sundstrom % % CHANGELOG______________________________________________________________ % v.1.0.6 (2010) Useable for MATLAB 2007 -- CD % v.1.0.6 (15-Dec-2010): Added OPTIONS.VERBOSE 'on'|{'off'}. Prints % status of DPM in the command window. Output equivalent to Waitbar but % with significantly less computational effort---SE % v.1.0.6 (July 2011) removed use of roundn % v.1.1.0 (15-Dec-2011) Added Level-Set-Method as an option and new % forward simulation scheme as an option -- PE % v.1.1.1 (11-Oc-2012) fixed error handling in line 2394 --PE % v.1.1.2 (19-Juni-2013) added licence agreement header -- PE try varargout = {}; if nargin == 3 if ~exist([varargin{1} '.m'],'file') str{1} = ''; str{end+1} = ['function [X, C, I, signals] = ' varargin{1} '(inp,par)']; str{end+1} = '\n%% inp.X{i} states'; str{end+1} = '%% inp.U{i} inputs'; str{end+1} = '%% inp.W{i} disturbances (as defined in dis-struct)'; str{end+1} = '%% inp.Ts time step'; str{end+1} = '%% par struct including user defined parameters'; % str{end+1} = '%% lim.X{i} = ["lower" ; "upper"] (limits of each state)'; str{end+1} = '\n%% state update (out.X{i} must be set within model function)'; % str{end+1} = 'func = inp.Ts.*(par.a.*(inp.X{1}-inp.X{1}.^2/par.b)-inp.U{1});'; for i=1:varargin{2} x = round(rand*(varargin{2}-1))+1; u = round(rand*(varargin{3}-1))+1; str{end+1} = ['X{' num2str(i) '} = ' num2str(rand) '.*(inp.X{' num2str(x) '} + inp.U{' num2str(u) '} + inp.W{1})./inp.Ts + inp.X{' num2str(i) '};']; end str{end+1} = '\n%% cost (out.C{1} must be set within model function)'; str{end+1} = 'C{1} = -inp.Ts.*inp.U{1};'; str{end+1} = '\n%% Infeasibility (out.I [zero=feasible/one=infeasible] must be set within model function)'; str{end+1} = '\n%% for example if the state is outside the grid or infeasible combinations of inputs and states occur.';