%% Dynamic programming using radial basis functions % Oliver Junge and Alex Schreiber, TUM, May 2014 %% an inverted pendulum on a cart M = 8; m = 2; mr = m/(m+M); l = 0.5; c = 9.8; h = 0.1; % ADAPTIV... theta=0.5; % ...ADAPTIV f = @(x,u) kron(x,ones(length(u),1))+h* ... [reshape(ones(length(u),1)*x(:,2)', length(u)*size(x,1),1), ... reshape((c/l*ones(length(u),1)*sin(x(:,1)') ... -1/2*mr*ones(length(u),1)*(x(:,2)'.^2.*sin(2*x(:,1)')) ... -mr/m/l*u*cos(x(:,1)'))./(4/3-mr*ones(length(u),1)*cos(x(:,1)').^2), ... length(u)*size(x,1),1)]; c = @(x,u) exp(-h*(1/2*reshape(0.1*ones(length(u),1)*x(:,1)'.^2 + ... 0.05*ones(length(u),1)*x(:,2)'.^2 + 0.01*u.^2*ones(1,size(x,1)), ... length(u)*size(x,1),1))); phi = @(r) max(spones(r)-r,0).^4.*(4*r+spones(r)); % Wendland function phi = @(r) max(spones(r)-r,0).^10.*(429*r.^4+450*r.^3+210*r.^2+50*r+5*spones(r)); T = [0 0]; v_T = 1; % boundary cond. shepard = @(A) spdiags(1./sum(A')',0,size(A,1),size(A,1))*A; % Shepard op. S = [8,10]; % radius of domain L = 33; U = linspace(-128,128,L)'; % control values N = 50; X1 = linspace(-1,1,N); [XX,YY] = meshgrid(X1*S(1),X1*S(2)); X = [XX(:) YY(:)]; % centers % ADAPTIV... h = 2/(N-1)/2*ones(N^2,1)*S; % local fill distance (approx.) % ...ADAPTIV ep = 1/sqrt((4*prod(S)*20/N^2)/pi); % shape parameter % ADAPTIV... for i=1:10, shapevec = getShapevec(X,ep); v = valueIteration(f,c,phi,shepard,X,shapevec,U,ep); [e,V] = residual(v,f,c,phi,shepard,X,[T;X],shapevec,U,ep); %% refining nodes I1 = find(V<9); size(I1); % sublevel set of V I2 = find(abs(e) > theta*max(abs(e(I1)))); % large residual Ilarge = intersect(I1,I2); Ismall = setdiff(1:length(X),Ilarge); length(Ilarge), length(Ismall) Xl = X(Ilarge,:); hl = h(Ilarge,:); [Xlr, hlr] = refine(Xl,hl); size(Xlr); X = [X(Ismall,:); Xlr]; h = [h(Ismall,:); hlr]; end shapevec = getShapevec(X,ep); v = valueIteration(f,c,phi,shepard,X,shapevec,U,ep); hold on; plot3(X(:,1),X(:,2),10*ones(size(X,1),1),'k.'); drawnow;