├── dyntest.m ├── draw_drone.m ├── draw_update.m ├── README.md ├── costTest.m ├── costTest_long.m ├── LICENSE ├── quad_anim.m ├── droneDynamics.m ├── MAIN_flip_long.m ├── MAIN_flip.m ├── drone_params.m ├── ChebTest.m └── PseudoOptimal.m /dyntest.m: -------------------------------------------------------------------------------- 1 | function defects = dyntest(Xc,Uc,dynfcn,D,scale) 2 | % Xc = reshape(Xc,12,[]); 3 | % Uc = reshape(Uc,4,[]); 4 | Xc = reshape(Xc,12,numel(Xc)/12); 5 | Uc = reshape(Uc,4,numel(Uc)/4); 6 | 7 | defects = (D*(Xc.')/scale).' - dynfcn(0,Xc,Uc); 8 | defects = defects(:); 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /draw_drone.m: -------------------------------------------------------------------------------- 1 | function draw = draw_drone( drone ) 2 | draw.body=patch('faces',drone.body,'vertices',drone.pts(:,1:end-1)); 3 | draw.top=patch('faces',drone.top,'vertices',drone.pts(:,1:end-1)); 4 | draw.face=patch('faces',drone.face,'vertices',drone.pts(:,1:end-1)); 5 | draw.blade=patch('faces',drone.blade,'vertices',drone.rpts(:,1:end-1)); 6 | 7 | draw.x=plot3(drone.x(1,:),drone.x(2,:),drone.x(3,:),'linewidth',2,'color','r'); 8 | draw.y=plot3(drone.y(1,:),drone.y(2,:),drone.y(3,:),'linewidth',2,'color','g'); 9 | draw.z=plot3(drone.z(1,:),drone.z(2,:),drone.z(3,:),'linewidth',2,'color','b'); 10 | 11 | set(draw.body,'facecolor',[0.1,0.1,0.1],'facealpha',0.8); 12 | set(draw.top,'facecolor','r','facealpha',0.8); 13 | set(draw.face,'facecolor','b','facealpha',0.8); 14 | set(draw.blade,'facecolor',0.7*ones(1,3),'facealpha',0.9,'edgecolor','none'); 15 | 16 | 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /draw_update.m: -------------------------------------------------------------------------------- 1 | function [ drone ] = draw_update( drone,trans,quad,z ) 2 | X = z(1,:); 3 | Y = z(2,:); 4 | Z = z(3,:); 5 | ay = z(4,:); 6 | ax = z(5,:); 7 | az = z(6,:); 8 | % phi = z(4,:); 9 | % the = z(5,:); 10 | % psi = z(6,:); 11 | % R=[cos(phi) sin(phi) 0;-sin(phi) cos(phi) 0;0 0 1].'... 12 | % *[cos(the) 0 -sin(the);0 1 0;sin(the) 0 cos(the)].'... 13 | % *[1 0 0;0 cos(psi) sin(psi);0 -sin(psi) cos(psi)].'; 14 | R = [cos(ay) 0 -sin(ay);0 1 0;sin(ay) 0 cos(ay)].'... 15 | *[1 0 0;0 cos(ax) sin(ax);0 -sin(ax) cos(ax)].'... 16 | *[cos(az) sin(az) 0;-sin(az) cos(az) 0;0 0 1].'; 17 | T=[X;Y;Z]; 18 | trans=trans*[R,T;0 0 0 1]; 19 | 20 | 21 | drone.pts=drone.pts0*trans.'; 22 | drone.rpts=drone.rpts0*trans.'; 23 | drone.x=[trans(1:3,4),trans(1:3,4)+quad.d*trans(1:3,1)]; 24 | drone.y=[trans(1:3,4),trans(1:3,4)+quad.d*trans(1:3,2)]; 25 | drone.z=[trans(1:3,4),trans(1:3,4)+quad.d*trans(1:3,3)]; 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Flipping-Test 2 | Test for solving drone flipping problem using numerical optimal control (specific, pseudo optimal control) 3 | 4 | Fast example: run MAIN_flip.m (fast result) 5 | 6 | Long example: run MAIN_flip_long.m (result include stabilizing stage after flipping) 7 | 8 | dependent packages: chebfun-master(v5.7.0), casadi-matlabR2014b-v3.2.3(linux x64). 9 | 10 | File chain: MAIN_flip -- costTest, MAIN_flip_long -- costTest_long 11 | 12 | public files: 13 | 14 | (plotting) draw_drone, draw_update,quad_anim 15 | (dynamics) droneDynamics dyntest drone_params 16 | (optimal control) PseudoOptimal ChebTest 17 | 18 | Acknowlegment: 19 | 20 | Trajectory Optimization (package) - Matthew Kelly 21 | "Multirotor aerial vehicles." Mahony, Robert, Vijay Kumar, and Peter Corke. IEEE Robotics and Automation magazine 20.32 (2012) 22 | Rolling Spider software package for Education (MIT toolbox) 23 | ME290J 2016 spring lectures, UC Berkeley, Francesco Borrelli 24 | 25 | === 26 | 27 | License: BSD (https://github.com/yzhao334/Flipping-Test/blob/master/LICENSE) 28 | -------------------------------------------------------------------------------- /costTest.m: -------------------------------------------------------------------------------- 1 | function [cost] = costTest(Xc,Uc,dist_target,dist_time,tc,ww,scale,u0) 2 | % Xc = reshape(Xc,12,[]); 3 | Xc = reshape(Xc,12,numel(Xc)/12); 4 | Uc = reshape(Uc,4,numel(Uc)/4); 5 | % Uc = reshape(Uc,4,[]); 6 | target = interp1(dist_time(:),dist_target,tc(:)); 7 | target = target.'; 8 | temp = Xc - target; 9 | % temp = diag([100*ones(1,3),ones(1,3),... 10 | % 100*ones(1,3),ones(1,3)])*temp; 11 | 12 | % cost=sum(ww(:).'.*(sum(temp.^2,1)+sum(Uc.^2,1)/100))*scale; 13 | cost=sum(ww(:).'.*(sum(temp.^2,1)+sum(Uc.^2,1)/2000^2))*scale; 14 | % cost=sum(ww(:).'.*(sum(temp.^2,1)))*scale; 15 | % cost=sum(ww(:).'.*(sum(temp.^2,1)+sum(Uc.^2,1)/2000^2/1000))*scale; 16 | % temp = Uc - u0(:)*ones(1,numel(Uc)/4); 17 | % tercost = sum(sum(Uc(:,[1:3,numel(Uc)/4-10:numel(Uc)/4]).^2))*1000; 18 | % tercost2 = sum(sum(Xc(7:12,[numel(Xc)/12-10:numel(Xc)/12]).^2))*100*(2000^2); 19 | tercost2 = sum(sum(Xc(7:12,[numel(Xc)/12-4:numel(Xc)/12]).^2))*100*(2000^2); 20 | % tercost2 = sum(sum(Xc(7:12,[numel(Xc)/12-4:numel(Xc)/12]).^2))*100; 21 | % cost=sum( ww(:).'.*(sum(Uc.^2,1)+sum(Xc(7:12,:).^2*2000^2,1)) )*scale; 22 | % tercost = sum(sum(temp(:,[1:3,numel(Uc)/4-5:numel(Uc)/4]).^2))*1000; 23 | % cost=sum(ww(:).'.*(sum(temp.^2,1)))*scale; 24 | % cost = cost+tercost; 25 | 26 | % cost = cost+tercost2; 27 | 28 | % cost = cost+tercost+tercost2; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /costTest_long.m: -------------------------------------------------------------------------------- 1 | function [cost] = costTest_long(Xc,Uc,dist_target,dist_time,tc,ww,scale,u0) 2 | % Xc = reshape(Xc,12,[]); 3 | Xc = reshape(Xc,12,numel(Xc)/12); 4 | Uc = reshape(Uc,4,numel(Uc)/4); 5 | % Uc = reshape(Uc,4,[]); 6 | target = interp1(dist_time(:),dist_target,tc(:)); 7 | target = target.'; 8 | temp = Xc - target; 9 | % temp = diag([10000*ones(1,3),ones(1,3),... 10 | % 10000*ones(1,3),ones(1,3)])*temp; 11 | 12 | % cost=sum(ww(:).'.*(sum(temp.^2,1)+sum(Uc.^2,1)/100))*scale; 13 | cost=sum(ww(:).'.*(sum(temp.^2,1)+sum(Uc.^2,1)/2000^2/100))*scale; 14 | % cost=sum(ww(:).'.*(sum(temp.^2,1)))*scale; 15 | % cost=sum(ww(:).'.*(sum(temp.^2,1)+sum(Uc.^2,1)/2000^2/1000))*scale; 16 | % temp = Uc - u0(:)*ones(1,numel(Uc)/4); 17 | % tercost = sum(sum(Uc(:,[1:3,numel(Uc)/4-10:numel(Uc)/4]).^2))*1000; 18 | tercost2 = sum(sum(Xc(7:12,[floor(numel(Xc)/12/2):numel(Xc)/12]).^2))*10000*(2000^2); 19 | % tercost2 = sum(sum(Xc(7:12,[numel(Xc)/12-4:numel(Xc)/12]).^2))*100*(2000^2); 20 | % tercost2 = sum(sum(Xc(7:12,[numel(Xc)/12-4:numel(Xc)/12]).^2))*100; 21 | % cost=sum( ww(:).'.*(sum(Uc.^2,1)+sum(Xc(7:12,:).^2*2000^2,1)) )*scale; 22 | % tercost = sum(sum(temp(:,[1:3,numel(Uc)/4-5:numel(Uc)/4]).^2))*1000; 23 | % cost=sum(ww(:).'.*(sum(temp.^2,1)))*scale; 24 | % cost = cost+tercost; 25 | 26 | cost = cost+tercost2; 27 | 28 | % cost = cost+tercost+tercost2; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Yu Zhao 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /quad_anim.m: -------------------------------------------------------------------------------- 1 | function quad_anim( z,quad,freq ) 2 | % animation 3 | clf;pause(0.1); 4 | trans=[[1 0 0;0 -1 0; 0 0 -1].',[0;0;0]; 0 0 0 1]; 5 | T=size(z,2)-1;% total horizon 6 | d=quad.d/sqrt(2); 7 | drone.N=20;% blade discritization 8 | % faces 9 | drone.top=[1 2 3 4]; 10 | drone.body=[5 6 7 8;2 6 7 3;3 7 8 4;4 8 5 1]; 11 | drone.face=[1 5 6 2]; 12 | for i=1:4 13 | drone.blade(i,:)=(i-1)*drone.N+1:i*drone.N; 14 | end 15 | % vertices 16 | drone.pts0=[d -d quad.h;d d quad.h; -d d quad.h;-d -d quad.h;... 17 | d -d 0; d d 0; -d d 0; -d -d 0]; 18 | drone.pts0=[drone.pts0,ones(8,1)]; 19 | drone.rpts0=[ones(drone.N,1)*(drone.pts0(1,1:3)+[0,0,-0.005])+quad.r*[cos(linspace(0,-2*pi,drone.N)).',sin(linspace(0,-2*pi,drone.N)).',zeros(drone.N,1)];... 20 | ones(drone.N,1)*(drone.pts0(2,1:3)+[0,0,-0.005])+quad.r*[cos(linspace(0,-2*pi,drone.N)).',sin(linspace(0,-2*pi,drone.N)).',zeros(drone.N,1)];... 21 | ones(drone.N,1)*(drone.pts0(3,1:3)+[0,0,-0.005])+quad.r*[cos(linspace(0,-2*pi,drone.N)).',sin(linspace(0,-2*pi,drone.N)).',zeros(drone.N,1)];... 22 | ones(drone.N,1)*(drone.pts0(4,1:3)+[0,0,-0.005])+quad.r*[cos(linspace(0,-2*pi,drone.N)).',sin(linspace(0,-2*pi,drone.N)).',zeros(drone.N,1)]]; 23 | drone.rpts0=[drone.rpts0,ones(4*drone.N,1)]; 24 | % update data 25 | drone=draw_update(drone,trans,quad,z(:,1)); 26 | % initialize drawing 27 | hold on; 28 | view([37.5,30]); 29 | axis equal vis3d;box on;grid on; 30 | min_p=min(z(1:3,:),[],2); 31 | max_p=max(z(1:3,:),[],2); 32 | xlim([min_p(1)-quad.d-quad.r,max_p(1)+quad.d+quad.r]); 33 | ylim([-(max_p(2)+quad.d+quad.r),-(min_p(2)-quad.d-quad.r)]); 34 | zlim([-(max_p(3)+quad.d+quad.r),-(min_p(3)-quad.d-quad.r)]); 35 | xlabel('X');ylabel('Y');zlabel('Z'); 36 | 37 | 38 | draw=draw_drone(drone); 39 | 40 | for t=0:T 41 | drone=draw_update(drone,trans,quad,z(:,t+1)); 42 | set(draw.body,'vertices',drone.pts(:,1:end-1)); 43 | set(draw.top,'vertices',drone.pts(:,1:end-1)); 44 | set(draw.face,'vertices',drone.pts(:,1:end-1)); 45 | set(draw.blade,'vertices',drone.rpts(:,1:end-1)); 46 | set(draw.x,'xdata',drone.x(1,:),'ydata',drone.x(2,:),'zdata',drone.x(3,:)); 47 | set(draw.y,'xdata',drone.y(1,:),'ydata',drone.y(2,:),'zdata',drone.y(3,:)); 48 | set(draw.z,'xdata',drone.z(1,:),'ydata',drone.z(2,:),'zdata',drone.z(3,:)); 49 | pause(1/freq); % show slow animation 50 | end 51 | 52 | 53 | end 54 | 55 | -------------------------------------------------------------------------------- /droneDynamics.m: -------------------------------------------------------------------------------- 1 | function dz = droneDynamics(z,u,quadpar) 2 | % drone dynamics -- customized version 3 | % dz = droneDynamics(z,u,p) 4 | % 5 | % This function computes the first-order dynamics of the quadrator/drone. 6 | % 7 | % INPUTS: 8 | % x = [12, n] = [P;O;dp;do] = state of the system 9 | % P = [x;y;z], position in inertia frame 10 | % O = [ay;ax;az], euler angles 11 | % dp = [dx;dy;dz], vel in inertia frame 12 | % do = [wx;wy;wz], anguler velocity 13 | % u = [4, n] = control to the drone 14 | % quad = parameter struct 15 | % orientation: y-x-z angle 16 | % angles: ay, ax, az 17 | % 18 | % OUTPUTS: 19 | % dz = dz/dt = time derivative of state 20 | % 21 | % 22 | % R = 23 | % [ cos(ay)*cos(az) + sin(ax)*sin(ay)*sin(az), cos(az)*sin(ax)*sin(ay) - cos(ay)*sin(az), cos(ax)*sin(ay)] 24 | % [ cos(ax)*sin(az), cos(ax)*cos(az), -sin(ax)] 25 | % [ cos(ay)*sin(ax)*sin(az) - cos(az)*sin(ay), sin(ay)*sin(az) + cos(ay)*cos(az)*sin(ax), cos(ax)*cos(ay)] 26 | % 27 | % X = z(1,:);% x pos 28 | % Y = z(2,:);% y pos 29 | % Z = z(3,:);% z pos 30 | ay = z(4,:);% x vel 31 | ax = z(5,:);% y vel 32 | az = z(6,:);% z vel 33 | dx = z(7,:); 34 | dy = z(8,:); 35 | dz = z(9,:); 36 | wx = z(10,:); 37 | wy = z(11,:); 38 | wz = z(12,:); 39 | 40 | w1=u(1,:); 41 | w2=u(2,:); 42 | w3=u(3,:); 43 | w4=u(4,:); 44 | 45 | ct=quadpar.Ct*quadpar.rho*quadpar.A*quadpar.r^2; 46 | cQ=quadpar.Cq*quadpar.rho*quadpar.A*quadpar.r^3; 47 | d=sqrt(2)/2*quadpar.d; 48 | M =... 49 | [ -ct, -ct, -ct, -ct;... 50 | ct*d, -ct*d, -ct*d, ct*d;... 51 | ct*d, ct*d, -ct*d, -ct*d;... 52 | -cQ, cQ, -cQ, cQ]; 53 | 54 | Trqs = M*[w1.^2;w2.^2;w3.^2;w4.^2]; 55 | 56 | Tz=Trqs(1,:); % force, body frame 57 | Tay=Trqs(2,:); % torque 1, body frame 58 | Tax=Trqs(3,:); % torque 2, body frame 59 | Taz=Trqs(4,:); % torque 3, body frame 60 | 61 | % in inertia frame 62 | % tau = 63 | % Tay*(cos(ay)*cos(az) + sin(ax)*sin(ay)*sin(az)) - Tax*(cos(ay)*sin(az) - cos(az)*sin(ax)*sin(ay)) + Taz*cos(ax)*sin(ay) 64 | % Tax*cos(ax)*cos(az) - Taz*sin(ax) + Tay*cos(ax)*sin(az) 65 | % Tax*(sin(ay)*sin(az) + cos(ay)*cos(az)*sin(ax)) - Tay*(cos(az)*sin(ay) - cos(ay)*sin(ax)*sin(az)) + Taz*cos(ax)*cos(ay) 66 | % tz = 67 | % cos(ax)*sin(ay)*Tz 68 | % -sin(ax)*Tz 69 | % cos(ax)*cos(ay)*Tz 70 | 71 | 72 | ddx = 1/quadpar.M*(cos(ax).*sin(ay).*Tz); 73 | ddy = 1/quadpar.M*(-sin(ax).*Tz); 74 | ddz = 1/quadpar.M*(cos(ax).*cos(ay).*Tz)+quadpar.g; 75 | 76 | day = (wy.*cos(ax) + wz.*cos(ay).*sin(ax) + wx.*sin(ax).*sin(ay))./cos(ax); 77 | dax = wx.*cos(ay) - wz.*sin(ay); 78 | daz = (wz.*cos(ay) + wx.*sin(ay))./cos(ax); 79 | 80 | J1 = quadpar.J(1,1); 81 | J2 = quadpar.J(2,2); 82 | J3 = quadpar.J(3,3); 83 | 84 | % -Omega x I Omega 85 | % -(dax*sin(ay) - daz*cos(ax)*cos(ay))*(J2 - J3)*(day - daz*sin(ax)) 86 | % (J1 - J3)*(dax^2*cos(ay)*sin(ay) + dax*daz*cos(ax) - 2*dax*daz*cos(ax)*cos(ay)^2 - daz^2*cos(ax)^2*cos(ay)*sin(ay)) 87 | % (dax*cos(ay) + daz*cos(ax)*sin(ay))*(J1 - J2)*(day - daz*sin(ax)) 88 | 89 | dw = [-(dax.*sin(ay) - daz.*cos(ax).*cos(ay)).*(day - daz.*sin(ax))*(J2 - J3) + ... 90 | Tay.*(cos(ay).*cos(az) + sin(ax).*sin(ay).*sin(az)) - Tax.*(cos(ay).*sin(az) - cos(az).*sin(ax).*sin(ay)) + Taz.*cos(ax).*sin(ay);... 91 | (J1 - J3)*(dax.^2.*cos(ay).*sin(ay) + dax.*daz.*cos(ax) - 2*dax.*daz.*cos(ax).*cos(ay).^2 - daz.^2.*cos(ax).^2.*cos(ay).*sin(ay)) + ... 92 | Tax.*cos(ax).*cos(az) - Taz.*sin(ax) + Tay.*cos(ax).*sin(az);... 93 | (J1 - J2)*(dax.*cos(ay) + daz.*cos(ax).*sin(ay)).*(day - daz.*sin(ax)) + ... 94 | Tax.*(sin(ay).*sin(az) + cos(ay).*cos(az).*sin(ax)) - Tay.*(cos(az).*sin(ay) - cos(ay).*sin(ax).*sin(az)) + Taz.*cos(ax).*cos(ay)]; 95 | % dw = quadpar.J\dw; % dw = [dwx;dwy;dwz] 96 | dwx = dw(1,:)/J1; 97 | dwy = dw(2,:)/J2; 98 | dwz = dw(3,:)/J3; 99 | 100 | 101 | 102 | 103 | dz = [dx;dy;dz;... 104 | day;dax;daz;... 105 | ddx;ddy;ddz;... 106 | dwx;dwy;dwz]; 107 | 108 | % [dX,dY,dZ,dphi,dthe,dpsi,ddx,ddy,ddz,dp,dq,dr] = autoGen_droneDynamics(phi,the,psi,dx,dy,dz,p,q,r, w1,w2,w3,w4, quadpar); 109 | 110 | % dz = [dX;dY;dZ;dphi;dthe;dpsi;ddx;ddy;ddz;dp;dq;dr]; 111 | 112 | end -------------------------------------------------------------------------------- /MAIN_flip_long.m: -------------------------------------------------------------------------------- 1 | % MAIN_flip.m 2 | % remember to add casadi into path 3 | 4 | clc; clear; 5 | % setting parameters 6 | drone_params; 7 | % Trajectory Parameters: 8 | % global N z0 zF state_target control_target; 9 | N=100; % N should be even 10 | duration = 1/200*N; 11 | % duration = 1.5; 12 | 13 | % Initial State: 14 | z0 = [0 ;0; -0.46; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % initial state 15 | 16 | % zF = [0 ;0; -0.46; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % final state 17 | zF = [0 ;0; -0.46; 2*pi; 0; 0; 0; 0; 0; 0; 0; 0]; % final state 18 | 19 | state_target=nan(12,N*4); 20 | temptime=linspace(0,duration,N*4); 21 | for i=1:6 22 | state_target(i,1:N/2)=z0(i)*ones(1,N/2); 23 | state_target(i,N/2+1:3/2*N)=linspace(z0(i),zF(i),N); 24 | %state_target(i,3/2*N+1:2*N)=zF(i)*ones(1,N/2); 25 | state_target(i,3/2*N+1:4*N)=zF(i)*ones(1,3/2*N+N); 26 | end 27 | for i=7:9 28 | state_target(i,:)=gradient(state_target(i-6,:))./gradient(temptime); 29 | end 30 | % state_target(10,:)=gradient(state_target(6,:))./gradient(temptime); 31 | % state_target(11,:)=gradient(state_target(5,:))./gradient(temptime); 32 | % state_target(12,:)=gradient(state_target(4,:))./gradient(temptime); 33 | state_target(10,:)=gradient(state_target(5,:))./gradient(temptime); 34 | state_target(11,:)=gradient(state_target(4,:))./gradient(temptime); 35 | state_target(12,:)=gradient(state_target(6,:))./gradient(temptime); 36 | 37 | % state_target(10,:)=0*gradient(state_target(6,:))./gradient(temptime); 38 | % state_target(11,:)=0*gradient(state_target(5,:))./gradient(temptime); 39 | % state_target(12,:)=0*gradient(state_target(4,:))./gradient(temptime); 40 | 41 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 42 | % Set up function handles % 43 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 44 | 45 | problem.func.dynamics = @(t,x,u)( droneDynamics(x,u,quad) ); 46 | problem.func.pathObj = @(t,x,u)( objective(x,u) ); 47 | 48 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 49 | % Set up bounds on state and control % 50 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 51 | 52 | problem.bounds.initialTime.low = 0; 53 | problem.bounds.initialTime.upp = 0; 54 | problem.bounds.finalTime.low = duration; 55 | problem.bounds.finalTime.upp = duration; 56 | 57 | problem.bounds.initialState.low = z0; 58 | problem.bounds.initialState.upp = z0; 59 | problem.bounds.finalState.low = zF; 60 | problem.bounds.finalState.upp = zF; 61 | 62 | 63 | problem.bounds.control.low = sqrt(10*quadEDT.motorcommandToW2_gain)*[1;1;1;1]*0; 64 | problem.bounds.control.upp = sqrt(quadEDT.motors_max*quadEDT.motorcommandToW2_gain)*[1;1;1;1]; 65 | 66 | 67 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 68 | % Initialize trajectory with guess % 69 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 70 | 71 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 72 | % Solver options % 73 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 74 | 75 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 76 | % Solve! % 77 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 78 | sim.u0=[-0.66708 0 0 0].'; 79 | 80 | neo.tspan=[0,1.5]; 81 | neo.dt=1e-3; 82 | neo.time=0:neo.dt:neo.tspan(end); 83 | neo.init=state_target(:,1); 84 | neo.terminal=state_target(:,end); 85 | neo.t_tar=linspace(neo.tspan(1),neo.tspan(end),size(state_target,2)); 86 | neo.target=state_target.'; 87 | neo.dyn=@(Xc,Uc,D,nS,nU,scale,P)dyntest(Xc,Uc,problem.func.dynamics,D,scale); 88 | neo.cost=@(Xc,Uc,tc,D,nS,nU,ww,scale)costTest_long(Xc,Uc,... 89 | neo.target,neo.t_tar,tc,ww,scale,sim.u0); 90 | neo.constr=@(Xc,Uc,D,scale)Uc; 91 | neo.conlb=problem.bounds.control.low; 92 | % neo.conub=problem.bounds.control.upp; 93 | % neo.conlb=problem.bounds.control.upp*0.03; 94 | % neo.conub=problem.bounds.control.upp*0.85; 95 | neo.conub=problem.bounds.control.upp*1; 96 | % neo.conlb=-sqrt(quadEDT.motorcommandToW2_gain*500)*ones(4,1)*0.85; 97 | % neo.conub=+sqrt(quadEDT.motorcommandToW2_gain*500)*ones(4,1)*0.85; 98 | 99 | ct=quad.Ct*quad.rho*quad.A*quad.r^2; 100 | cQ=quad.Cq*quad.rho*quad.A*quad.r^3; 101 | d=sqrt(2)/2*quad.d; 102 | M =... 103 | [ -ct, -ct, -ct, -ct;... 104 | ct*d, -ct*d, -ct*d, ct*d;... 105 | ct*d, ct*d, -ct*d, -ct*d;... 106 | -cQ, cQ, -cQ, cQ]; 107 | % M_inv_neg=-inv(M); 108 | 109 | sim.u_init=[(sim.u0+[-0.63;0;2e-2;0])*ones(1,34),... 110 | (sim.u0+[-0.63;0;-2e-2;0])*ones(1,34),... 111 | (sim.u0+[-0.63;0;0;0])*ones(1,40),... 112 | (sim.u0+[0.2;0;0;0])*ones(1,42)]; 113 | sim.u_init=M\sim.u_init; 114 | sim.u_init=sqrt(sim.u_init); 115 | sim.t_temp=linspace(neo.tspan(1),neo.tspan(end),size(sim.u_init,2)); 116 | 117 | sim.u_init=sqrt(quad.M*quad.g/ct/4)*ones(size(sim.u_init)); 118 | neo.u_init=interp1(sim.t_temp(:),sim.u_init.',neo.time(:)); 119 | 120 | %% 121 | ps=PseudoOptimal; 122 | ps.npts=20; 123 | ps.nS=12; 124 | ps.nU=4; 125 | ps.sGuess=interp1(neo.t_tar(:),neo.target,neo.time(:)); 126 | %% 127 | [Xc,Uc,Xopt,Uopt] = ps.optimalControlTerminal(neo.dyn,neo.cost,neo.constr,... 128 | neo.conlb,neo.conub,neo.time,neo.u_init,... 129 | neo.init,neo.terminal,1000,5); 130 | 131 | %% 132 | ps.npts=30; 133 | 134 | %% 135 | ps.sGuess=Xopt; 136 | neo.u_init=Uopt; 137 | 138 | [Xc,Uc,Xopt,Uopt] = ps.optimalControlTerminal(neo.dyn,neo.cost,neo.constr,... 139 | neo.conlb,neo.conub,neo.time,neo.u_init,... 140 | neo.init,neo.terminal,2000,5); 141 | 142 | %% simulate 143 | % load staticGain; 144 | % K=static.K; 145 | sim.dt=1/200; 146 | sim.soln.time=0; 147 | sim.soln.state=neo.init; 148 | sim.T=neo.time(end)/sim.dt; 149 | sim.dyn=@(t,x,u,p)droneDynamics(x,u,p); 150 | sim.soln.u=[]; 151 | 152 | [t,y]=ode23tb(@(t,x)sim.dyn(t,x,(interp1(neo.time,Uopt,t)).',quad),neo.tspan,neo.init); 153 | sim.time=neo.tspan(1):sim.dt:neo.tspan(end); 154 | sim.soln.state=(interp1(t(:),y,sim.time(:))).'; 155 | 156 | %% plot 157 | sim.anim_freq=100; % refresh freq for animation 158 | quad_anim(sim.soln.state,quad,sim.anim_freq);% animation 159 | -------------------------------------------------------------------------------- /MAIN_flip.m: -------------------------------------------------------------------------------- 1 | % MAIN_flip.m 2 | % remember to add casadi into path 3 | 4 | clc; clear; 5 | % setting parameters 6 | drone_params; 7 | % Trajectory Parameters: 8 | % global N z0 zF state_target control_target; 9 | N=10; % N should be even 10 | duration = 1/200*N*2*10; 11 | % duration = 1/200*N; 12 | 13 | % Initial State: 14 | z0 = [0 ;0; -0.46; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % initial state 15 | 16 | % zF = [0 ;0; -0.46; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % final state 17 | zF = [0 ;0; -0.46; 2*pi; 0; 0; 0; 0; 0; 0; 0; 0]; % final state 18 | 19 | state_target=nan(12,N*2); 20 | temptime=linspace(0,duration,N*2); 21 | for i=1:6 22 | state_target(i,1:N/2)=z0(i)*ones(1,N/2); 23 | state_target(i,N/2+1:3/2*N)=linspace(z0(i),zF(i),N); 24 | state_target(i,3/2*N+1:2*N)=zF(i)*ones(1,N/2); 25 | end 26 | for i=7:9 27 | state_target(i,:)=gradient(state_target(i-6,:))./gradient(temptime); 28 | end 29 | % state_target(10,:)=gradient(state_target(6,:))./gradient(temptime); 30 | % state_target(11,:)=gradient(state_target(5,:))./gradient(temptime); 31 | % state_target(12,:)=gradient(state_target(4,:))./gradient(temptime); 32 | state_target(10,:)=gradient(state_target(5,:))./gradient(temptime); 33 | state_target(11,:)=gradient(state_target(4,:))./gradient(temptime); 34 | state_target(12,:)=gradient(state_target(6,:))./gradient(temptime); 35 | 36 | 37 | ct=quad.Ct*quad.rho*quad.A*quad.r^2; 38 | control_target=ones(4,N*2)*sqrt(quad.M*quad.g/ct/4); 39 | % control_target=inv_dyn(state_target,temptime,quad); 40 | 41 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 42 | % Set up function handles % 43 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 44 | 45 | problem.func.dynamics = @(t,x,u)( droneDynamics(x,u,quad) ); 46 | problem.func.pathObj = @(t,x,u)( objective(x,u) ); 47 | 48 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 49 | % Set up bounds on state and control % 50 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 51 | 52 | problem.bounds.initialTime.low = 0; 53 | problem.bounds.initialTime.upp = 0; 54 | problem.bounds.finalTime.low = duration; 55 | problem.bounds.finalTime.upp = duration; 56 | 57 | problem.bounds.initialState.low = z0; 58 | problem.bounds.initialState.upp = z0; 59 | problem.bounds.finalState.low = zF; 60 | problem.bounds.finalState.upp = zF; 61 | 62 | 63 | problem.bounds.control.low = sqrt(10*quadEDT.motorcommandToW2_gain)*[1;1;1;1]*0; 64 | problem.bounds.control.upp = sqrt(quadEDT.motors_max*quadEDT.motorcommandToW2_gain)*[1;1;1;1]; 65 | 66 | 67 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 68 | % Initialize trajectory with guess % 69 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 70 | % Car travels at a speed of one, and drives in a straight line from start 71 | % to finish point. 72 | 73 | % problem.guess.time = [0,duration]; 74 | % problem.guess.state = [problem.bounds.initialState.low, problem.bounds.finalState.low]; 75 | % problem.guess.control = [zeros(4,1),zeros(4,1)]; 76 | 77 | % problem.guess.time = linspace(0,duration,N*2); 78 | % problem.guess.state=state_target; 79 | % problem.guess.control=control_target; 80 | 81 | problem.guess.time = [0,duration/4,duration/4*3,duration]; 82 | problem.guess.state=[z0,z0,zF,zF]; 83 | problem.guess.control=control_target(:,1)*ones(1,4); 84 | 85 | % problem.guess.time = [0,duration]; 86 | % problem.guess.state=[z0,z0]; 87 | % problem.guess.control=control_target(:,1)*ones(1,2); 88 | 89 | 90 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 91 | % Solver options % 92 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 93 | 94 | problem.options.nlpOpt = optimset(... 95 | 'Display','iter',... 96 | 'MaxFunEvals',1e5); 97 | 98 | 99 | problem.options.method = 'trapezoid'; 100 | % problem.options.method = 'hermiteSimpson'; 101 | % problem.options.method = 'rungeKutta'; 102 | % problem.options.method = 'chebyshev'; 103 | 104 | % problem.options.method = 'chebyshev'; 105 | % problem.options.chebyshev.nColPts = N*2; 106 | problem.options.trapezoid.nGrid = N*2; 107 | 108 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 109 | % Solve! % 110 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 111 | sim.u0=[-0.66708 0 0 0].'; 112 | 113 | neo.tspan=[0,0.95]; 114 | neo.dt=1e-3; 115 | neo.time=0:neo.dt:neo.tspan(end); 116 | neo.init=state_target(:,1); 117 | neo.terminal=state_target(:,end); 118 | neo.t_tar=linspace(neo.tspan(1),neo.tspan(end),size(state_target,2)); 119 | neo.target=state_target.'; 120 | neo.dyn=@(Xc,Uc,D,nS,nU,scale,P)dyntest(Xc,Uc,problem.func.dynamics,D,scale); 121 | neo.cost=@(Xc,Uc,tc,D,nS,nU,ww,scale)costTest(Xc,Uc,... 122 | neo.target,neo.t_tar,tc,ww,scale,sim.u0); 123 | neo.constr=@(Xc,Uc,D,scale)Uc; 124 | neo.conlb=problem.bounds.control.low; 125 | % neo.conub=problem.bounds.control.upp; 126 | % neo.conlb=problem.bounds.control.upp*0.03; 127 | neo.conub=problem.bounds.control.upp*0.85; 128 | % neo.conlb=-sqrt(quadEDT.motorcommandToW2_gain*500)*ones(4,1)*0.85; 129 | % neo.conub=+sqrt(quadEDT.motorcommandToW2_gain*500)*ones(4,1)*0.85; 130 | 131 | ct=quad.Ct*quad.rho*quad.A*quad.r^2; 132 | cQ=quad.Cq*quad.rho*quad.A*quad.r^3; 133 | d=sqrt(2)/2*quad.d; 134 | M =... 135 | [ -ct, -ct, -ct, -ct;... 136 | ct*d, -ct*d, -ct*d, ct*d;... 137 | ct*d, ct*d, -ct*d, -ct*d;... 138 | -cQ, cQ, -cQ, cQ]; 139 | % M_inv_neg=-inv(M); 140 | 141 | sim.u_init=[(sim.u0+[-0.63;0;2e-2;0])*ones(1,34),... 142 | (sim.u0+[-0.63;0;-2e-2;0])*ones(1,34),... 143 | (sim.u0+[-0.63;0;0;0])*ones(1,40),... 144 | (sim.u0+[0.2;0;0;0])*ones(1,42)]; 145 | sim.u_init=M\sim.u_init; 146 | sim.u_init=sqrt(sim.u_init); 147 | sim.t_temp=linspace(neo.tspan(1),neo.tspan(end),size(sim.u_init,2)); 148 | 149 | sim.u_init=sqrt(quad.M*quad.g/ct/4)*ones(size(sim.u_init)); 150 | neo.u_init=interp1(sim.t_temp(:),sim.u_init.',neo.time(:)); 151 | 152 | %% 153 | ps=PseudoOptimal; 154 | ps.npts=20; 155 | ps.nS=12; 156 | ps.nU=4; 157 | ps.sGuess=interp1(neo.t_tar(:),neo.target,neo.time(:)); 158 | %% 159 | [Xc,Uc,Xopt,Uopt] = ps.optimalControlTerminal(neo.dyn,neo.cost,neo.constr,... 160 | neo.conlb,neo.conub,neo.time,neo.u_init,... 161 | neo.init,neo.terminal,1000,5); 162 | 163 | %% 164 | % ps.npts=30; 165 | 166 | %% 167 | ps.sGuess=Xopt; 168 | neo.u_init=Uopt; 169 | 170 | [Xc,Uc,Xopt,Uopt] = ps.optimalControlTerminal(neo.dyn,neo.cost,neo.constr,... 171 | neo.conlb,neo.conub,neo.time,neo.u_init,... 172 | neo.init,neo.terminal,1000,5); 173 | 174 | % soln = trajOpt(problem); 175 | 176 | % get results 177 | % t=linspace(soln.grid.time(1),soln.grid.time(end),2*N); 178 | % z=soln.interp.state(t); 179 | % u=soln.interp.control(t); 180 | % t=soln.grid.time; 181 | % z=soln.grid.state; 182 | % u=soln.grid.control; 183 | %% simulate 184 | % load staticGain; 185 | % K=static.K; 186 | sim.dt=1/200; 187 | sim.soln.time=0; 188 | sim.soln.state=neo.init; 189 | sim.T=neo.time(end)/sim.dt; 190 | sim.dyn=@(t,x,u,p)droneDynamics(x,u,p); 191 | sim.soln.u=[]; 192 | 193 | [t,y]=ode23tb(@(t,x)sim.dyn(t,x,(interp1(neo.time,Uopt,t)).',quad),neo.tspan,neo.init); 194 | sim.time=neo.tspan(1):sim.dt:neo.tspan(end); 195 | sim.soln.state=(interp1(t(:),y,sim.time(:))).'; 196 | 197 | % for k=0:sim.T-1 198 | % sim.soln.time=[sim.soln.time,(k+1)*sim.dt]; 199 | % % use opt control, Euler diff 200 | % %temp_s=(interp1(neo.time,Xopt,k*sim.dt)).'; 201 | % %u_fb=sqrt(0*abs(quadEDT.motorcommandToW2_gain*(K*(temp_s-sim.soln.state(:,end))))); 202 | % %u_fb=0*([5*ones(4,6),10*ones(4,6)]*(temp_s-sim.soln.state(:,end))); 203 | % u_fb=0; 204 | % sim.soln.u=[sim.soln.u,(interp1(neo.time,Uopt,k*sim.dt)).'+u_fb]; 205 | % temp.y=sim.soln.state(:,end)+sim.dt*feval(sim.dyn,0,sim.soln.state(:,end),sim.soln.u(:,end),quad); 206 | % sim.soln.state=[sim.soln.state,temp.y(:,end)]; 207 | % end 208 | % clear k temp; 209 | %% plot 210 | sim.anim_freq=100; % refresh freq for animation 211 | quad_anim(sim.soln.state,quad,sim.anim_freq);% animation 212 | -------------------------------------------------------------------------------- /drone_params.m: -------------------------------------------------------------------------------- 1 | % setup drone parameters 2 | % from mdl_quadrotor.m in the MIT toolbox of the drone 3 | %MDL_QUADCOPTER Dynamic parameters for a quadrotor. 4 | % 5 | % MDL_QUADCOPTER is a script creates the workspace variable quad which 6 | % describes the dynamic characterstics of a quadrotor flying robot. 7 | % 8 | % Properties:: 9 | % 10 | % This is a structure with the following elements: 11 | % 12 | % nrotors Number of rotors (1x1) 13 | % J Flyer rotational inertia matrix (3x3) 14 | % h Height of rotors above CoG (1x1) 15 | % d Length of flyer arms (1x1) 16 | % nb Number of blades per rotor (1x1) 17 | % r Rotor radius (1x1) 18 | % c Blade chord (1x1) 19 | % e Flapping hinge offset (1x1) 20 | % Mb Rotor blade mass (1x1) 21 | % Mc Estimated hub clamp mass (1x1) 22 | % ec Blade root clamp displacement (1x1) 23 | % Ib Rotor blade rotational inertia (1x1) 24 | % Ic Estimated root clamp inertia (1x1) 25 | % mb Static blade moment (1x1) 26 | % Ir Total rotor inertia (1x1) 27 | % Ct Non-dim. thrust coefficient (1x1) 28 | % Cq Non-dim. torque coefficient (1x1) 29 | % sigma Rotor solidity ratio (1x1) 30 | % thetat Blade tip angle (1x1) 31 | % theta0 Blade root angle (1x1) 32 | % theta1 Blade twist angle (1x1) 33 | % theta75 3/4 blade angle (1x1) 34 | % thetai Blade ideal root approximation (1x1) 35 | % a Lift slope gradient (1x1) 36 | % A Rotor disc area (1x1) 37 | % gamma Lock number (1x1) 38 | % 39 | % 40 | % Notes:: 41 | % - SI units are used. 42 | % 43 | % References:: 44 | % - Design, Construction and Control of a Large Quadrotor micro air vehicle. 45 | % P.Pounds, PhD thesis, 46 | % Australian National University, 2007. 47 | % http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf 48 | % - This is a heavy lift quadrotor 49 | % 50 | % See also sl_quadrotor. 51 | 52 | % MODEL: quadrotor 53 | 54 | % Copyright (C) 1993-2015, by Peter I. Corke 55 | % 56 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 57 | % 58 | % RTB is free software: you can redistribute it and/or modify 59 | % it under the terms of the GNU Lesser General Public License as published by 60 | % the Free Software Foundation, either version 3 of the License, or 61 | % (at your option) any later version. 62 | % 63 | % RTB is distributed in the hope that it will be useful, 64 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 65 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 66 | % GNU Lesser General Public License for more details. 67 | % 68 | % You should have received a copy of the GNU Leser General Public License 69 | % along with RTB. If not, see . 70 | % 71 | % http://www.petercorke.com 72 | 73 | %-------------------- 74 | % Update: 75 | % date: 2015/08/23 76 | % editor: Fabian Riether 77 | % comment: This updated version now contains parameters for Peter Corke's 78 | % Robotics Toolbox that match Parrot's Rolling Spider, parameters for 79 | % the EducationalDroneToolbox are appended 80 | %-------------------- 81 | 82 | quad.nrotors = 4; % 4 rotors 83 | quad.g = 9.81; % g Gravity 1x1 84 | quad.rho = 1.184; % rho Density of air 1x1 85 | quad.muv = 1.5e-5; % muv Viscosity of air 1x1 86 | 87 | % Airframe 88 | quad.M = 0.068; % M Mass 1x1 89 | 90 | % Ixx,yy,zz Flyer rotational inertia matrix 3x3 91 | quad.J = diag([0.0686e-3 0.092e-3 0.1366e-3]); 92 | 93 | quad.h = -(6.5+9.376)/1000; % h Height of rotors above CoG 1x1 94 | quad.d = 0.0624; % d Length of flyer arms 1x1 95 | 96 | %Rotor 97 | quad.nb = 2; % b Number of blades per rotor 1x1 98 | quad.r = 33/1000; % r Rotor radius 1x1 99 | 100 | quad.c = 0.008; % c Blade chord 1x1 101 | 102 | quad.e = 0.0; % e Flapping hinge offset 1x1 103 | quad.Mb = 0.0015/4; % Mb Rotor blade mass 1x1 104 | quad.Mc = 0; % Mc Estimated hub clamp mass 1x1 105 | quad.ec = 0; % ec Blade root clamp displacement 1x1 106 | quad.Ib = quad.Mb*(quad.r-quad.ec)^2/4 ; % Ib Rotor blade rotational inertia 1x1 107 | quad.Ic = quad.Mc*(quad.ec)^2/4; % Ic Estimated root clamp inertia 1x1 108 | quad.mb = quad.g*(quad.Mc*quad.ec/2+quad.Mb*quad.r/2); % mb Static blade moment 1x1 109 | quad.Ir = quad.nb*(quad.Ib+quad.Ic); % Ir Total rotor inertia 1x1 110 | 111 | quad.Ct = 0.0107; % Ct Non-dim. thrust coefficient 1x1 112 | quad.Cq = quad.Ct*sqrt(quad.Ct/2); % Cq Non-dim. torque coefficient 1x1 113 | 114 | quad.sigma = quad.c*quad.nb/(pi*quad.r); % sigma Rotor solidity ratio 1x1 115 | quad.thetat = 6.8*(pi/180); % thetat Blade tip angle @TODO 1x1 116 | quad.theta0 = 14.6*(pi/180); % theta0 Blade root angle @TODO 1x1 117 | quad.theta1 = quad.thetat - quad.theta0; % theta1 Blade twist angle 1x1 118 | quad.theta75 = quad.theta0 + 0.75*quad.theta1; % theta76 3/4 blade angle 1x1 119 | quad.thetai = quad.thetat*(quad.r/quad.e); % thetai Blade ideal root approximation 1x1 120 | if isinf(quad.thetai) quad.thetai = 10000; end; 121 | quad.a = 5.5; % a Lift slope gradient @TODO 1x1 122 | 123 | % derived constants 124 | quad.A = pi*quad.r^2; % A Rotor disc area 1x1 125 | quad.gamma = quad.rho*quad.a*quad.c*quad.r^4/(quad.Ib+quad.Ic);% gamma Lock number 1x1 126 | 127 | quad.b = quad.Ct*quad.rho*quad.A*quad.r^2; % T = b w^2 128 | quad.k = quad.Cq*quad.rho*quad.A*quad.r^3; % Q = k w^2 129 | 130 | quad.verbose = false; 131 | 132 | quadEDT.w2ToThrust_gain = quad.Ct*quad.rho*quad.A*quad.r^2; 133 | 134 | %% Motors 135 | quadEDT.motors_max = 500; %max command to control motors 136 | quadEDT.motorcommandToW2_gain = 13840.8; %motor command for Rolling Spider (0-500) to motorspeed^2 137 | 138 | quadEDT.thrustToMotorcommand = 1/(quadEDT.w2ToThrust_gain*quadEDT.motorcommandToW2_gain); 139 | 140 | 141 | %% Sensors 142 | %Noise on all states from simulation 143 | quadEDT.noiseStatesSensed_std = [1 1 1 1 1 1 0.0165195073635001 0.0152648883285633 0.0215786550496705 0.000652733165165932 0.000721701528439517 0.000690781425279554]; 144 | quadEDT.noiseStatesSensed_weights = [0 0 0 0 0 0 0.05 0.05 0.05 1 1 1]; 145 | 146 | %Delay (all sensor data) 147 | quadEDT.sensordelay = 1; %in samples of 200Hz; 148 | 149 | %Bias (some assumed, simulated default) 150 | quadEDT.sensordataCalib = [0.09 -0.06 -9.4730 -0.0095 -0.0075 0.0015 101270.95]; 151 | quadEDT.IMUaccel_gain = [+1.00596 +1.00383 +0.99454]; 152 | quadEDT.IMUgyro_gain = [0.99861 1.00644 0.99997]; 153 | 154 | %Gains 155 | quadEDT.airDensity = 1.225; 156 | quadEDT.altToPrs_gain = quad.g*quadEDT.airDensity ; 157 | quadEDT.inverseIMU_gain = [1./quadEDT.IMUaccel_gain 1./quadEDT.IMUgyro_gain]; 158 | 159 | %Saturations 160 | quadEDT.altSenor_min = 0.44; 161 | 162 | %Battery DUMMY 163 | quadEDT.dummy.batteryStatus = [3.5 70]; 164 | 165 | %Vision 166 | quadEDT.velocityToOpticalFlow_gain = 1/20; 167 | 168 | %% Simulation Parameters for EducationalDroneToolbox (apart from drone dynamics subsystem) 169 | quadEDT.sampletime = 0.005; 170 | 171 | %reference values 172 | %---- 173 | %yaw 174 | quadEDT.yawStep_amplitude = 0.1; 175 | quadEDT.yawStep_time = 5.5; 176 | quadEDT.yawStep_duration = 2.5; 177 | %pitch 178 | quadEDT.pitchStep_amplitude = 0.1; %0.1 179 | quadEDT.pitchStep_time = 3; 180 | quadEDT.pitchStep_duration = 1; 181 | %roll 182 | quadEDT.rollStep_amplitude = 0.0; %0.1 183 | quadEDT.rollStep_time = 3; 184 | quadEDT.rollStep_duration = 1; 185 | %altitude 186 | quadEDT.takeoff_duration = 1; 187 | quadEDT.altitude = -1.1; 188 | %---- 189 | 190 | %Vision 191 | quadEDT.NO_VIS_X = -99.0; 192 | quadEDT.NO_VIS_YAW = -9.0; 193 | quadEDT.dummy.posVIS_novisionavail = [quadEDT.NO_VIS_X;0.0;0.0;quadEDT.NO_VIS_YAW ]; %drone sends this position to Drone_compensator when no marker to reconstruct position found 194 | quadEDT.dummy.usePosVIS_flag = 0; 195 | 196 | 197 | %Motor Failure Simulation 198 | quadEDT.motorFailure_time = 10;%0.1; 199 | quadEDT.motorFailure_duration = 0.05; 200 | quadEDT.motorFailure_m1 = 0; 201 | quadEDT.motorFailure_m2 = 0; 202 | quadEDT.motorFailure_m3 = -30; 203 | quadEDT.motorFailure_m4 = 20; 204 | 205 | quadEDT.motorUnknownGain_gain = 1; %amplifies the TF from motors_datin (command 0-500) to acutal rotor speed (rad/s) used in the simulation; use as "model uncertainty" 206 | -------------------------------------------------------------------------------- /ChebTest.m: -------------------------------------------------------------------------------- 1 | classdef ChebTest 2 | % functions for chebyshev collocation 3 | % dependency: chebfun, casadi 4 | 5 | properties 6 | end 7 | 8 | methods 9 | end 10 | 11 | methods(Static) 12 | function D = getDifferentiationMatrix(x,v,d) 13 | % D = getDifferentiationMatrix(x,v,d) 14 | % 15 | % 16 | % INPUTS: 17 | % x = [n,1] = vector of roots of the orthogonal polynomial of interest 18 | % v = [n,1] = vector of barycentric weights corresponding to each root 19 | % d = [1,2] = domain of the polynomial (optional) 20 | % 21 | % OUTPUTS: 22 | % D = [n,n] = differentiation matrix such that dy/dx = D*y @ points in x 23 | % 24 | % NOTES: 25 | % Reference: 26 | % 1) ChebFun (http://www.chebfun.org/) 27 | % 2) "Barycentric Lagrange Interpolation" SIAM Review 2004 28 | % Jean-Paul Berrut and Lloyd N. Trefethen 29 | % 30 | % Inputs: x and v are typically produced by a call to any of: 31 | % chebpts, trigpts, legpts, jacpts, lagpts, hermpts, lobpts, radaupts 32 | % 33 | 34 | if nargin == 2 35 | d = [-1,1]; 36 | end 37 | 38 | n = length(x); 39 | D = zeros(n,n); 40 | for i=1:n 41 | D(i,:) = (v/v(i))./(x(i)-x); 42 | D(i,i) = 0; 43 | D(i,i) = -sum(D(i,:)); 44 | end 45 | 46 | D = 2*D/(d(2)-d(1)); 47 | end 48 | 49 | function y = barycentricInterpolate(x,yk,xk,vk) 50 | % y = barycentricInterpolate(x,yk,xk,vk) 51 | % 52 | % Interpolates an orthogonal polynomial using barycentric interpolation 53 | % 54 | % INPUTS: 55 | % x = [nTime, 1] = vector of points to evaluate polynomial at 56 | % yk = [nGrid, nOut] = value of the function to be interpolated at each 57 | % grid point 58 | % xk = [nGrid, 1] = roots of orthogonal polynomial 59 | % vk = [nGrid, 1] = barycentric interpolation weights 60 | % 61 | % OUTPUTS: 62 | % y = [nTime, nOut] = value of the function at the desired points 63 | % 64 | % NOTES: 65 | % xk and yk should be produced by chebfun (help chebpts) 66 | % 67 | 68 | nOut = size(yk,2); 69 | nTime = length(x); 70 | y = zeros(nTime, nOut); 71 | 72 | for i=1:nOut 73 | y(:,i) = bary(x,yk(:,i),xk,vk); 74 | end 75 | end 76 | 77 | 78 | function [x,w,D] = orthScale(orth,d) 79 | % [x,w,D] = orthScale(orth,d) 80 | % 81 | % This function scales the chebyshev points to an arbitrary interval 82 | % 83 | % INPUTS: 84 | % xx = chebyshev points on the domain [-1,1] 85 | % ww = chebysehv weights on the domain [-1,1] 86 | % d = [low, upp] = new domain 87 | % 88 | % OUTPUTS: 89 | % x = chebyshev points on the new domain d 90 | % w = chebyshev weights on the new domain d 91 | % 92 | 93 | shift = 0.5*(d(1) + d(2)); 94 | scale = 0.5*(d(2) - d(1)); 95 | 96 | x = scale*orth.xx + shift; 97 | 98 | if nargout > 1 99 | w = orth.ww*scale; 100 | end 101 | 102 | if nargout > 2 103 | D = orth.D/scale; 104 | end 105 | 106 | end 107 | 108 | 109 | function [t,x,u,w] = unPackDecVar(z,pack,orth) 110 | % 111 | % This function unpacks the decision variables for 112 | % trajectory optimization into the time (t), 113 | % state (x), and control (u) matricies 114 | % 115 | % INPUTS: 116 | % z = column vector of 2 + nTime*(nState+nControl) decision variables 117 | % pack = details about how to convert z back into t,x, and u 118 | % .nTime 119 | % .nState 120 | % .nControl 121 | % 122 | % OUTPUTS: 123 | % t = [1, nTime] = time vector (grid points) 124 | % x = [nState, nTime] = state vector at each grid point 125 | % u = [nControl, nTime] = control vector at each grid point 126 | % w = [1, nTime] = weights for clenshaw-curtis quadrature 127 | % 128 | 129 | nTime = pack.nTime; 130 | nState = pack.nState; 131 | nControl = pack.nControl; 132 | nx = nState*nTime; 133 | nu = nControl*nTime; 134 | 135 | [t, w] = ChebTest.orthScale(orth,[z(1),z(2)]); 136 | t = t'; 137 | x = reshape(z((2+1):(2+nx)),nState,nTime); 138 | u = reshape(z((2+nx+1):(2+nx+nu)),nControl,nTime); 139 | end 140 | 141 | function [z,pack] = packDecVar(t,x,u) 142 | % 143 | % This function collapses the time (t), state (x) 144 | % and control (u) matricies into a single vector 145 | % 146 | % INPUTS: 147 | % t = [1, nTime] = time vector (grid points) 148 | % x = [nState, nTime] = state vector at each grid point 149 | % u = [nControl, nTime] = control vector at each grid point 150 | % 151 | % OUTPUTS: 152 | % z = column vector of 2 + nTime*(nState+nControl) decision variables 153 | % pack = details about how to convert z back into t,x, and u 154 | % .nTime 155 | % .nState 156 | % .nControl 157 | % 158 | 159 | nTime = length(t); 160 | nState = size(x,1); 161 | nControl = size(u,1); 162 | 163 | tSpan = [t(1); t(end)]; 164 | xCol = reshape(x, nState*nTime, 1); 165 | uCol = reshape(u, nControl*nTime, 1); 166 | 167 | z = [tSpan;xCol;uCol]; 168 | 169 | pack.nTime = nTime; 170 | pack.nState = nState; 171 | pack.nControl = nControl; 172 | 173 | end 174 | 175 | % check how many points could approximate function/signal, 1-D 176 | function [ ucheb,udcheb ] = chebinterPosVel( u,time,npts ) 177 | %chebshev interpolation to get pos and vel for u 178 | % u: original pos signal, 1-D 179 | % time: time of u 180 | % npts: # of points for interpolation 181 | Uc=u(:); 182 | time=time(:); 183 | [orth.xx,orth.ww,orth.vv]=chebpts(npts);% cheb pnts and weights 184 | orth.D=ChebTest.getDifferentiationMatrix(orth.xx,orth.vv); 185 | tc=chebpts(npts,[time(1),time(end)]); 186 | Uc=interp1(time,Uc,tc); 187 | scale=(time(end)-time(1))/2; 188 | Ucd=orth.D/scale*Uc(:); 189 | 190 | ucheb=ChebTest.barycentricInterpolate(time(:),Uc(:),tc(:),orth.vv(:)); 191 | udcheb=ChebTest.barycentricInterpolate(time(:),Ucd(:),tc(:),orth.vv(:)); 192 | 193 | 194 | end 195 | 196 | 197 | function Xsim = test(nColPts,time,posvelm,rob,initCond) 198 | [orth.xx, orth.ww, orth.vv] = chebpts(nColPts); 199 | orth.D = ChebTest.getDifferentiationMatrix(orth.xx,orth.vv); 200 | guess.tSpan = time([1,end]); 201 | guess.time = chebpts(nColPts,guess.tSpan)'; 202 | guess.state = interp1(time(:), posvelm, guess.time')'; 203 | dynfcn=@(x)rob.linkDyn210f(x,guess.time,time,posvelm); 204 | 205 | x0=guess.state(:); 206 | x0=x0(13:end); 207 | scale=(guess.tSpan(2)-guess.tSpan(1))/2; 208 | eqn=@(x)ChebTest.testEqn(x,orth.D,dynfcn,12,initCond,scale); 209 | opt=optimoptions('fsolve','Display','iter','Algorithm','levenberg-marquardt'); 210 | xsoln=fsolve(eqn,x0,opt); 211 | xsoln=[initCond(:);xsoln]; 212 | pack.nTime = nColPts; 213 | pack.nState = 12; 214 | pack.nControl = 0; 215 | [tSoln,xSoln,uSoln] = ChebTest.unPackDecVar([guess.tSpan(:);xsoln],pack,orth); 216 | %%%% Store the results: 217 | soln.grid.time = tSoln; 218 | soln.grid.state = xSoln; 219 | soln.grid.control = uSoln; 220 | 221 | %%%% Rescale the points: 222 | dSoln = tSoln([1,end]); %Domain of the final solution 223 | xxSoln = ChebTest.orthScale(orth,dSoln); 224 | soln.interp.state = @(t)( ChebTest.barycentricInterpolate(t', xSoln',xxSoln,orth.vv)' ); 225 | 226 | Xsim = soln.interp.state(time(:).'); 227 | 228 | end 229 | 230 | 231 | function ret = testEqn(x,D,dynfcn,nS,x0,scale) 232 | x=reshape(x,nS,[]); 233 | x=[x0(:),x]; 234 | dxFun = (D/scale*(x'))'; %Differentiate trajectory 235 | dxDyn = dynfcn(x); 236 | % Add a constraint that both versions of the derivative must match: 237 | defects = dxFun - dxDyn; 238 | ret = defects(:); 239 | 240 | end 241 | 242 | 243 | end 244 | 245 | end 246 | 247 | -------------------------------------------------------------------------------- /PseudoOptimal.m: -------------------------------------------------------------------------------- 1 | classdef PseudoOptimal < handle 2 | %PSEUDOOPTIMAL pseudo optimal control for simulation, parameter estimation, and optimal control 3 | % dependency: chebfun, casadi, ChebTest 4 | 5 | properties 6 | npts; % number of nodes 7 | nS; % dim of states 8 | nU; % dim of control 9 | nO; % dim of output 10 | sGuess; % initial guess of states 11 | Pv0; % initial value of variable parameters 12 | getP; % function handle to get full parameter 13 | P; % actually used parameter 14 | S; % casadi fcn 15 | end 16 | 17 | properties (Hidden) 18 | orth; % orthogonal nodes, weights, differential matrix 19 | guess; % guess of initial states, states and parameter, or states and control 20 | % also time for nodes 21 | scale; % time scale 22 | x; % casadivariable for variable at nodes 23 | 24 | 25 | %Dynfcn; % Dynfcn: dynamics function handle for vectorized discretized states and control 26 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 27 | % Xc: state at nodes 28 | % Uc: control at nodes 29 | % D: differential matrix 30 | % nS: dim of states 31 | % scale: time scale 32 | % P: parameter 33 | %Outfcn; % function handle of difference between output and target 34 | % format: Outfcn( Xc ) 35 | % output nOutxN matrix(row for out, col for time) 36 | 37 | %target; % target of output, NxnO(row for time, col for var) 38 | %Pmin; % lower bound of variable parameter 39 | %Pmax; % upper bound of variable parameter 40 | %outerr; % acceptable difference between output and target 41 | end 42 | 43 | methods 44 | % function for simulation 45 | function [Xc,Xsim] = simulate(obj,Dynfcn,time,u,init,maxiter,displev) 46 | % simulate use collocation 47 | % requires: 48 | % Dynfcn: dynamics function handle for vectorized discretized states and control 49 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 50 | % Xc: state at nodes 51 | % Uc: control at nodes 52 | % D: differential matrix 53 | % nS: dim of states 54 | % scale: time scale 55 | % P: parameter 56 | % time: simulation time 57 | % u: input, corresponding to time(col for control, row for time) 58 | % init: initial condition for states 59 | % maxiter: maximum iteration for optimization 60 | % displev: disp level for optimization 61 | % output: 62 | % Xc: discretized states(col for state, row for time) 63 | % Xsim: simulated states corresponding to time(col for state, row for time) 64 | type = 'Simulation'; 65 | import casadi.*; 66 | disp([type,': Initialize optimization problem ......']); 67 | init=init(:);% reshape to col vec 68 | time=time(:).';% reshape to row vec 69 | fullx=@(x)[init;x];% from xc to get full x with init cond 70 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 71 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 72 | obj.guess.tSpan=[time(1),time(end)];% get time span 73 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 74 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 75 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 76 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 77 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 78 | Uc=reshape(Uc.',numel(Uc),1); 79 | eqn = @(x)Dynfcn(fullx(x),Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.P);% dynamics constraint 80 | 81 | obj.x=SX.sym('x',(obj.npts-1)*obj.nS,1);% casadi variable for state at node 82 | nlp=struct('x',obj.x,'f',dot(eqn(obj.x),eqn(obj.x)));% objective = minimize constraints 83 | opts.ipopt.max_iter=maxiter;% set maximum iteration number 84 | opts.ipopt.print_level=displev;% set display level 85 | obj.S=nlpsol('S','ipopt',nlp,opts);% optimization problem 86 | disp([type,': Initialize over, solving ......']); 87 | r=obj.S('x0',obj.guess.state);% solve optimization problem 88 | xopt=full(r.x);% get numerical result 89 | 90 | Xc = fullx(xopt); 91 | Xc = reshape(Xc,obj.nS,[]); 92 | Xc = Xc.'; 93 | 94 | Xsim=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 95 | end 96 | 97 | % function for simulation 98 | function [Xc,Xsim] = simulatePre(obj,Dynfcn,time,u,init,maxiter,displev) 99 | % simulate use collocation 100 | % requires: 101 | % Dynfcn: dynamics function handle for vectorized discretized states and control 102 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 103 | % Xc: state at nodes 104 | % Uc: control at nodes 105 | % D: differential matrix 106 | % nS: dim of states 107 | % scale: time scale 108 | % P: parameter 109 | % time: simulation time 110 | % u: input, corresponding to time(col for control, row for time) 111 | % init: initial condition for states 112 | % maxiter: maximum iteration for optimization 113 | % displev: disp level for optimization 114 | % output: 115 | % Xc: discretized states(col for state, row for time) 116 | % Xsim: simulated states corresponding to time(col for state, row for time) 117 | type = 'Simulation'; 118 | import casadi.*; 119 | disp([type,': Initialize optimization problem ......']); 120 | init=init(:);% reshape to col vec 121 | time=time(:).';% reshape to row vec 122 | fullx=@(x)[init;x];% from xc to get full x with init cond 123 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 124 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 125 | obj.guess.tSpan=[time(1),time(end)];% get time span 126 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 127 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 128 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 129 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 130 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 131 | Uc=reshape(Uc.',numel(Uc),1); 132 | eqn = @(x)Dynfcn(fullx(x),Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.P);% dynamics constraint 133 | 134 | %obj.x=MX.sym('x',(obj.npts-1)*obj.nS,1);% casadi variable for state at node 135 | %nlp=struct('x',obj.x,'f',dot(eqn(obj.x),eqn(obj.x)));% objective = minimize constraints 136 | %opts.ipopt.max_iter=maxiter;% set maximum iteration number 137 | %opts.ipopt.print_level=displev;% set display level 138 | %S=nlpsol('S','ipopt',nlp,opts);% optimization problem 139 | disp([type,': Initialize over, solving ......']); 140 | r=obj.S('x0',obj.guess.state);% solve optimization problem 141 | xopt=full(r.x);% get numerical result 142 | 143 | Xc = fullx(xopt); 144 | Xc = reshape(Xc,obj.nS,[]); 145 | Xc = Xc.'; 146 | 147 | Xsim=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 148 | end 149 | 150 | % function for parameter estimation 151 | function [Pvid,Xsim] = estimateParam(obj,Dynfcn,Outfcn,target,time,u,init,Pmin,Pmax,outerr,maxiter,displev) 152 | % parameter estimation use collocation 153 | % input: 154 | % Dynfcn: dynamics function handle for vectorized discretized states and control 155 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 156 | % Xc: state at nodes 157 | % Uc: control at nodes 158 | % D: differential matrix 159 | % nS: dim of states 160 | % scale: time scale 161 | % P: parameter 162 | % Outfcn: function handle output function 163 | % format: Outfcn( Xc ) 164 | % output nOutxN matrix(row for out, col for time) 165 | % target: target of output, NxnO(row for time, col for var) 166 | % time: simulation time 167 | % u: input, corresponding to time(col for control, row for time) 168 | % init: initial condition for states 169 | % outerr: acceptable output error range 170 | % Pmin: lower bound of variable parameter 171 | % Pmax: upper bound of variable parameter 172 | % maxiter: maximum iteration for optimization 173 | % displev: disp level for optimization 174 | % output: 175 | % Pvid: discretized states(col for state, row for time) 176 | % Xsim: simulated states corresponding to time(col for state, row for time) 177 | type = 'Parameter Estimation'; 178 | import casadi.*; 179 | disp([type,': Initialize optimization problem ......']); 180 | init=init(:);% reshape to col vec 181 | time=time(:).';% reshape to row vec 182 | fullx=@(x)[init;x];% from xc to get full x with init cond 183 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 184 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 185 | obj.guess.tSpan=[time(1),time(end)];% get time span 186 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 187 | Targc=interp1(time,target,obj.guess.time(:)); 188 | Targc=Targc.';% target at node 189 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 190 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 191 | obj.guess.state=[obj.guess.state;obj.Pv0(:)];% get initial state and parameter vec 192 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 193 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 194 | Uc=reshape(Uc.',numel(Uc),1); 195 | ind=obj.nS*(obj.npts-1);% index seperation of state and parameter 196 | %controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 197 | % Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 198 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 199 | Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 200 | 201 | cost = @(x)dot(obj.orth.ww(:),1000*1/2*diag(x.'*x))*obj.scale;% cost for id 202 | %cost = @(x)dot(obj.orth.ww,sum(abs(x),1))*obj.scale;% 1-norm cost for id 203 | %cost = @(x)max(sum(abs(x),1));% inf-norm cost for id 204 | %cost = @(x)dot(obj.orth.ww,1/2*sum(x.^2,1))*obj.scale;% 2-norm cost for id 205 | nP = length(obj.Pv0);% dim of variable parameter 206 | 207 | controlieq = @(x)eye(nP)*x(ind+1:end);% parameter bound 208 | %controlieq2 = @(x)reshape((Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc)*diag(1./obj.orth.ww),... 209 | % obj.nO*obj.npts,1)/obj.scale;% output defect 210 | controlieq2 = @(x)reshape(Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc,... 211 | obj.nO*obj.npts,1);% output defect 212 | 213 | obj.x=SX.sym('x',(obj.npts-1)*obj.nS+nP,1);% casadi variable for state at node and parameter 214 | nlp=struct('x',obj.x,'f',cost(Outfcn(reshape(fullx(obj.x(1:ind)),obj.nS,obj.npts))-Targc),...% objective = minimize difference between output and target 215 | 'g',[controlieq(obj.x);controleqn(obj.x);controlieq2(obj.x)]);% constraints = parameter bound, dynamics, output defect 216 | 217 | opts.ipopt.max_iter=maxiter;% set maximum iteration number 218 | opts.ipopt.print_level=displev;% set display level 219 | obj.S=nlpsol('S','ipopt',nlp,opts);% optimization problem 220 | disp([type,': Initialize over, solving ......']); 221 | r=obj.S('x0',obj.guess.state,'lbg',[Pmin(:);-1e-5*ones(obj.npts*obj.nS,1);-outerr*ones(obj.nO*obj.npts,1)],... 222 | 'ubg',[Pmax(:);1e-5*ones(obj.npts*obj.nS,1);outerr*ones(obj.nO*obj.npts,1)]);% solve optimization problem 223 | xopt=full(r.x);% get numerical result 224 | 225 | Xc = fullx(xopt(1:ind)); 226 | Xc = reshape(Xc,obj.nS,[]); 227 | Xc = Xc.'; 228 | 229 | Xsim=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 230 | Pvid = xopt(ind+1:end); 231 | end 232 | 233 | % function for parameter estimation 234 | function [Pvid,Xsim] = estimateParamInfn(obj,Dynfcn,Outfcn,target,time,u,init,Pmin,Pmax,outerr,maxiter,displev) 235 | % parameter estimation use collocation 236 | % input: 237 | % Dynfcn: dynamics function handle for vectorized discretized states and control 238 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 239 | % Xc: state at nodes 240 | % Uc: control at nodes 241 | % D: differential matrix 242 | % nS: dim of states 243 | % scale: time scale 244 | % P: parameter 245 | % Outfcn: function handle output function 246 | % format: Outfcn( Xc ) 247 | % output nOutxN matrix(row for out, col for time) 248 | % target: target of output, NxnO(row for time, col for var) 249 | % time: simulation time 250 | % u: input, corresponding to time(col for control, row for time) 251 | % init: initial condition for states 252 | % outerr: acceptable output error range 253 | % Pmin: lower bound of variable parameter 254 | % Pmax: upper bound of variable parameter 255 | % maxiter: maximum iteration for optimization 256 | % displev: disp level for optimization 257 | % output: 258 | % Pvid: discretized states(col for state, row for time) 259 | % Xsim: simulated states corresponding to time(col for state, row for time) 260 | type = 'Parameter Estimation'; 261 | import casadi.*; 262 | disp([type,': Initialize optimization problem ......']); 263 | init=init(:);% reshape to col vec 264 | time=time(:).';% reshape to row vec 265 | fullx=@(x)[init;x];% from xc to get full x with init cond 266 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 267 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 268 | obj.guess.tSpan=[time(1),time(end)];% get time span 269 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 270 | Targc=interp1(time,target,obj.guess.time(:)); 271 | Targc=Targc.';% target at node 272 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 273 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 274 | obj.guess.state=[obj.guess.state;obj.Pv0(:)];% get initial state and parameter vec 275 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 276 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 277 | Uc=reshape(Uc.',numel(Uc),1); 278 | ind=obj.nS*(obj.npts-1);% index seperation of state and parameter 279 | %controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 280 | % Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 281 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 282 | Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 283 | 284 | %cost = @(x)dot(obj.orth.ww(:),1000*1/2*diag(x.'*x))*obj.scale;% cost for id 285 | %cost = @(x)dot(obj.orth.ww,sum(abs(x),1))*obj.scale;% 1-norm cost for id 286 | cost = @(x)norm(sum(abs(x),1),'inf');% inf-norm cost for id 287 | %cost = @(x)dot(obj.orth.ww,1/2*sum(x.^2,1))*obj.scale;% 2-norm cost for id 288 | nP = length(obj.Pv0);% dim of variable parameter 289 | 290 | controlieq = @(x)eye(nP)*x(ind+1:end);% parameter bound 291 | %controlieq2 = @(x)reshape((Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc)*diag(1./obj.orth.ww),... 292 | % obj.nO*obj.npts,1)/obj.scale;% output defect 293 | controlieq2 = @(x)reshape(Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc,... 294 | obj.nO*obj.npts,1);% output defect 295 | 296 | obj.x=SX.sym('x',(obj.npts-1)*obj.nS+nP,1);% casadi variable for state at node and parameter 297 | nlp=struct('x',obj.x,'f',cost(Outfcn(reshape(fullx(obj.x(1:ind)),obj.nS,obj.npts))-Targc),...% objective = minimize difference between output and target 298 | 'g',[controlieq(obj.x);controleqn(obj.x);controlieq2(obj.x)]);% constraints = parameter bound, dynamics, output defect 299 | 300 | opts.ipopt.max_iter=maxiter;% set maximum iteration number 301 | opts.ipopt.print_level=displev;% set display level 302 | obj.S=nlpsol('S','ipopt',nlp,opts);% optimization problem 303 | disp([type,': Initialize over, solving ......']); 304 | r=obj.S('x0',obj.guess.state,'lbg',[Pmin(:);-1e-5*ones(obj.npts*obj.nS,1);-outerr*ones(obj.nO*obj.npts,1)],... 305 | 'ubg',[Pmax(:);1e-5*ones(obj.npts*obj.nS,1);outerr*ones(obj.nO*obj.npts,1)]);% solve optimization problem 306 | xopt=full(r.x);% get numerical result 307 | 308 | Xc = fullx(xopt(1:ind)); 309 | Xc = reshape(Xc,obj.nS,[]); 310 | Xc = Xc.'; 311 | 312 | Xsim=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 313 | Pvid = xopt(ind+1:end); 314 | end 315 | 316 | % function for parameter estimation 317 | function [Pvid,Xsim] = estimateParamPre(obj,Dynfcn,Outfcn,target,time,u,init,Pmin,Pmax,outerr,Pv0) 318 | % parameter estimation use collocation 319 | % input: 320 | % Dynfcn: dynamics function handle for vectorized discretized states and control 321 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 322 | % Xc: state at nodes 323 | % Uc: control at nodes 324 | % D: differential matrix 325 | % nS: dim of states 326 | % scale: time scale 327 | % P: parameter 328 | % Outfcn: function handle output function 329 | % format: Outfcn( Xc ) 330 | % output nOutxN matrix(row for out, col for time) 331 | % target: target of output, NxnO(row for time, col for var) 332 | % time: simulation time 333 | % u: input, corresponding to time(col for control, row for time) 334 | % init: initial condition for states 335 | % outerr: acceptable output error range 336 | % Pmin: lower bound of variable parameter 337 | % Pmax: upper bound of variable parameter 338 | % maxiter: maximum iteration for optimization 339 | % displev: disp level for optimization 340 | % output: 341 | % Pvid: discretized states(col for state, row for time) 342 | % Xsim: simulated states corresponding to time(col for state, row for time) 343 | type = 'Parameter Estimation'; 344 | import casadi.*; 345 | disp([type,': Initialize optimization problem ......']); 346 | init=init(:);% reshape to col vec 347 | time=time(:).';% reshape to row vec 348 | fullx=@(x)[init;x];% from xc to get full x with init cond 349 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 350 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 351 | obj.guess.tSpan=[time(1),time(end)];% get time span 352 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 353 | Targc=interp1(time,target,obj.guess.time(:)); 354 | Targc=Targc.';% target at node 355 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 356 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 357 | %obj.guess.state=[obj.guess.state;obj.Pv0(:)];% get initial state and parameter vec 358 | obj.guess.state=[obj.guess.state;Pv0(:)];% get initial state and parameter vec 359 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 360 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 361 | Uc=reshape(Uc.',numel(Uc),1); 362 | ind=obj.nS*(obj.npts-1);% index seperation of state and parameter 363 | %controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 364 | % Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 365 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 366 | Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 367 | 368 | %cost = @(x)dot(obj.orth.ww(:),1000*1/2*diag(x.'*x))*obj.scale;% cost for id 369 | cost = @(x)dot(obj.orth.ww,sum(abs(x),1))*obj.scale;% 1-norm cost for id 370 | %cost = @(x)dot(obj.orth.ww,1/2*sum(x.^2,1))*obj.scale;% 2-norm cost for id 371 | nP = length(obj.Pv0);% dim of variable parameter 372 | 373 | controlieq = @(x)eye(nP)*x(ind+1:end);% parameter bound 374 | %controlieq2 = @(x)reshape((Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc)*diag(1./obj.orth.ww),... 375 | % obj.nO*obj.npts,1)/obj.scale;% output defect 376 | controlieq2 = @(x)reshape(Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc,... 377 | obj.nO*obj.npts,1);% output defect 378 | 379 | %obj.x=MX.sym('x',(obj.npts-1)*obj.nS+nP,1);% casadi variable for state at node and parameter 380 | %nlp=struct('x',obj.x,'f',cost(Outfcn(reshape(fullx(obj.x(1:ind)),obj.nS,obj.npts))-Targc),...% objective = minimize difference between output and target 381 | % 'g',[controlieq(obj.x);controleqn(obj.x);controlieq2(obj.x)]);% constraints = parameter bound, dynamics, output defect 382 | 383 | %opts.ipopt.max_iter=maxiter;% set maximum iteration number 384 | %opts.ipopt.print_level=displev;% set display level 385 | %S=nlpsol('S','ipopt',nlp,opts);% optimization problem 386 | disp([type,': Initialize over, solving ......']); 387 | r=obj.S('x0',obj.guess.state,'lbg',[Pmin(:);-1e-5*ones(obj.npts*obj.nS,1);-outerr*ones(obj.nO*obj.npts,1)],... 388 | 'ubg',[Pmax(:);1e-5*ones(obj.npts*obj.nS,1);outerr*ones(obj.nO*obj.npts,1)]);% solve optimization problem 389 | xopt=full(r.x);% get numerical result 390 | 391 | Xc = fullx(xopt(1:ind)); 392 | Xc = reshape(Xc,obj.nS,[]); 393 | Xc = Xc.'; 394 | 395 | Xsim=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 396 | Pvid = xopt(ind+1:end); 397 | end 398 | 399 | % function for parameter estimation 400 | function [Pvid,Xsim] = estimateParamPreInit(obj,Dynfcn,Outfcn,target,time,u,init,Pmin,Pmax,outerr,sGuess,Pv0) 401 | % parameter estimation use collocation 402 | % input: 403 | % Dynfcn: dynamics function handle for vectorized discretized states and control 404 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 405 | % Xc: state at nodes 406 | % Uc: control at nodes 407 | % D: differential matrix 408 | % nS: dim of states 409 | % scale: time scale 410 | % P: parameter 411 | % Outfcn: function handle output function 412 | % format: Outfcn( Xc ) 413 | % output nOutxN matrix(row for out, col for time) 414 | % target: target of output, NxnO(row for time, col for var) 415 | % time: simulation time 416 | % u: input, corresponding to time(col for control, row for time) 417 | % init: initial condition for states 418 | % outerr: acceptable output error range 419 | % Pmin: lower bound of variable parameter 420 | % Pmax: upper bound of variable parameter 421 | % maxiter: maximum iteration for optimization 422 | % displev: disp level for optimization 423 | % output: 424 | % Pvid: discretized states(col for state, row for time) 425 | % Xsim: simulated states corresponding to time(col for state, row for time) 426 | type = 'Parameter Estimation'; 427 | import casadi.*; 428 | disp([type,': Initialize optimization problem ......']); 429 | init=init(:);% reshape to col vec 430 | time=time(:).';% reshape to row vec 431 | fullx=@(x)[init;x];% from xc to get full x with init cond 432 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 433 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 434 | obj.guess.tSpan=[time(1),time(end)];% get time span 435 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 436 | Targc=interp1(time,target,obj.guess.time(:)); 437 | Targc=Targc.';% target at node 438 | %ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 439 | ls = interp1(time(:),sGuess,obj.guess.time(2:end).');% initial of states at nodes 440 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 441 | %obj.guess.state=[obj.guess.state;obj.Pv0(:)];% get initial state and parameter vec 442 | obj.guess.state=[obj.guess.state;Pv0(:)];% get initial state and parameter vec 443 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 444 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 445 | Uc=reshape(Uc.',numel(Uc),1); 446 | ind=obj.nS*(obj.npts-1);% index seperation of state and parameter 447 | %controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 448 | % Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 449 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 450 | Uc,obj.orth.D,obj.nS,obj.nU,obj.scale,obj.getP(x(ind+1:end)));% dynamics constraint 451 | 452 | %cost = @(x)dot(obj.orth.ww(:),1000*1/2*diag(x.'*x))*obj.scale;% cost for id 453 | cost = @(x)dot(obj.orth.ww,sum(abs(x),1))*obj.scale;% 1-norm cost for id 454 | %cost = @(x)dot(obj.orth.ww,1/2*sum(x.^2,1))*obj.scale;% 2-norm cost for id 455 | nP = length(obj.Pv0);% dim of variable parameter 456 | 457 | controlieq = @(x)eye(nP)*x(ind+1:end);% parameter bound 458 | %controlieq2 = @(x)reshape((Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc)*diag(1./obj.orth.ww),... 459 | % obj.nO*obj.npts,1)/obj.scale;% output defect 460 | controlieq2 = @(x)reshape(Outfcn(reshape(fullx(x(1:ind)),obj.nS,obj.npts))-Targc,... 461 | obj.nO*obj.npts,1);% output defect 462 | 463 | %obj.x=MX.sym('x',(obj.npts-1)*obj.nS+nP,1);% casadi variable for state at node and parameter 464 | %nlp=struct('x',obj.x,'f',cost(Outfcn(reshape(fullx(obj.x(1:ind)),obj.nS,obj.npts))-Targc),...% objective = minimize difference between output and target 465 | % 'g',[controlieq(obj.x);controleqn(obj.x);controlieq2(obj.x)]);% constraints = parameter bound, dynamics, output defect 466 | 467 | %opts.ipopt.max_iter=maxiter;% set maximum iteration number 468 | %opts.ipopt.print_level=displev;% set display level 469 | %S=nlpsol('S','ipopt',nlp,opts);% optimization problem 470 | disp([type,': Initialize over, solving ......']); 471 | %r=obj.S('x0',obj.guess.state,'lbg',[Pmin(:);-1e-5*ones(obj.npts*obj.nS,1);-outerr*ones(obj.nO*obj.npts,1)],... 472 | % 'ubg',[Pmax(:);1e-5*ones(obj.npts*obj.nS,1);outerr*ones(obj.nO*obj.npts,1)]);% solve optimization problem 473 | r=obj.S('x0',obj.guess.state,'lbg',[Pmin(:);-0*1e-5*ones(obj.npts*obj.nS,1);kron(ones(obj.npts,1),-outerr)],... 474 | 'ubg',[Pmax(:);0*1e-5*ones(obj.npts*obj.nS,1);kron(ones(obj.npts,1),outerr)]);% solve optimization problem 475 | xopt=full(r.x);% get numerical result 476 | 477 | Xc = fullx(xopt(1:ind)); 478 | Xc = reshape(Xc,obj.nS,[]); 479 | Xc = Xc.'; 480 | 481 | Xsim=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 482 | Pvid = xopt(ind+1:end); 483 | end 484 | 485 | % function for optimal control 486 | function [Xc,Uc,Xopt,Uopt] = optimalControl(obj,Dynfcn,Costfcn,Confcn,conlb,conub,time,u,init,maxiter,displev) 487 | % optimal control use collocation 488 | % input: 489 | % Dynfcn: dynamics function handle for vectorized discretized states and control 490 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 491 | % Xc: state at nodes 492 | % Uc: control at nodes 493 | % D: differential matrix 494 | % nS: dim of states 495 | % scale: time scale 496 | % P: parameter 497 | % Costfcn: function handle cost function 498 | % format: Costfcn( Xc,Uc,tc,D,nS,nU,w,scale ) 499 | % Confcn: function handle of constraint function !!!: need modification and consideration... 500 | % format: Confcn( Xc,Uc,D,scale ) 501 | % conlb: lower bound of constraint function 502 | % conub: upper bound of constraint function 503 | % target: target of output, NxnO(row for time, col for var) 504 | % time: simulation time 505 | % u: input, corresponding to time(col for control, row for time) 506 | % init: initial condition for states 507 | % maxiter: maximum iteration for optimization 508 | % displev: disp level for optimization 509 | % output: 510 | % Pvid: discretized states(col for state, row for time) 511 | % Xsim: simulated states corresponding to time(col for state, row for time) 512 | type = 'Optimal Control'; 513 | import casadi.*; 514 | disp([type,': Initialize optimization problem ......']); 515 | init=init(:);% reshape to col vec 516 | time=time(:).';% reshape to row vec 517 | fullx=@(x)[init;x];% from xc to get full x with init cond 518 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 519 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 520 | obj.guess.tSpan=[time(1),time(end)];% get time span 521 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 522 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 523 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 524 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 525 | Uc=reshape(Uc.',numel(Uc),1); 526 | obj.guess.state=[obj.guess.state;Uc];% get initial state and control vec 527 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 528 | ind=obj.nS*(obj.npts-1);% index seperation of state and control 529 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 530 | x(ind+1:end),obj.orth.D,obj.nS,obj.nU,obj.scale,obj.P);% dynamics constraint 531 | 532 | cost = @(x)Costfcn( fullx(x(1:ind)),x(ind+1:end),obj.guess.time,... 533 | obj.orth.D,obj.nS,obj.nU,... 534 | obj.orth.ww,obj.scale );% cost for optimal control 535 | controlieq = @(x)Confcn(fullx(x(1:ind)),x(ind+1:end),... 536 | obj.orth.D,obj.scale);% constraintfcn bound 537 | 538 | obj.x=SX.sym('x',(obj.npts-1)*obj.nS+obj.npts*obj.nU,1);% casadi variable for state at node and parameter 539 | nlp=struct('x',obj.x,'f',cost(obj.x),...% objective = minimize cost fcn 540 | 'g',[controlieq(obj.x);controleqn(obj.x)]);% constraints = extconstr, dynamics 541 | 542 | opts.ipopt.max_iter=maxiter;% set maximum iteration number 543 | opts.ipopt.print_level=displev;% set display level 544 | obj.S=nlpsol('S','ipopt',nlp,opts);% optimization problem 545 | disp([type,': Initialize over, solving ......']); 546 | r=obj.S('x0',obj.guess.state,'lbg',[kron(ones(obj.npts,1),conlb);zeros(obj.npts*obj.nS,1)],... 547 | 'ubg',[kron(ones(obj.npts,1),conub);zeros(obj.npts*obj.nS,1)]);% solve optimization problem 548 | xopt=full(r.x);% get numerical result 549 | 550 | Xc = fullx(xopt(1:ind)); 551 | Xc = reshape(Xc,obj.nS,[]); 552 | Xc = Xc.'; 553 | Uc = xopt(ind+1:end); 554 | Uc = reshape(Uc,obj.nU,[]); 555 | Uc = Uc.'; 556 | 557 | Xopt=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 558 | Uopt=ChebTest.barycentricInterpolate(time(:),Uc,obj.guess.time(:),obj.orth.vv(:)); 559 | end 560 | 561 | % function for optimal control 562 | function [Xc,Uc,Xopt,Uopt] = optimalControlTerminal(obj,Dynfcn,Costfcn,Confcn,conlb,conub,time,u,init,terminal,maxiter,displev) 563 | % optimal control use collocation 564 | % input: 565 | % Dynfcn: dynamics function handle for vectorized discretized states and control 566 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 567 | % Xc: state at nodes 568 | % Uc: control at nodes 569 | % D: differential matrix 570 | % nS: dim of states 571 | % scale: time scale 572 | % P: parameter 573 | % Costfcn: function handle cost function 574 | % format: Costfcn( Xc,Uc,tc,D,nS,nU,w,scale ) 575 | % Confcn: function handle of constraint function !!!: need modification and consideration... 576 | % format: Confcn( Xc,Uc,D,scale ) 577 | % conlb: lower bound of constraint function 578 | % conub: upper bound of constraint function 579 | % target: target of output, NxnO(row for time, col for var) 580 | % time: simulation time 581 | % u: input, corresponding to time(col for control, row for time) 582 | % init: initial condition for states 583 | % maxiter: maximum iteration for optimization 584 | % displev: disp level for optimization 585 | % output: 586 | % Pvid: discretized states(col for state, row for time) 587 | % Xsim: simulated states corresponding to time(col for state, row for time) 588 | type = 'Optimal Control'; 589 | import casadi.*; 590 | disp([type,': Initialize optimization problem ......']); 591 | init=init(:);% reshape to col vec 592 | terminal=terminal(:); 593 | time=time(:).';% reshape to row vec 594 | %fullx=@(x)[init;x];% from xc to get full x with init cond 595 | fullx=@(x)[init;x;terminal];% from xc to get full x with init cond 596 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 597 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 598 | obj.guess.tSpan=[time(1),time(end)];% get time span 599 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 600 | %ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 601 | ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end-1).');% initial of states at nodes 602 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 603 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 604 | Uc=reshape(Uc.',numel(Uc),1); 605 | obj.guess.state=[obj.guess.state;Uc];% get initial state and control vec 606 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 607 | %ind=obj.nS*(obj.npts-1);% index seperation of state and control 608 | ind=obj.nS*(obj.npts-2);% index seperation of state and control 609 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 610 | x(ind+1:end),obj.orth.D,obj.nS,obj.nU,obj.scale,obj.P);% dynamics constraint 611 | 612 | cost = @(x)Costfcn( fullx(x(1:ind)),x(ind+1:end),obj.guess.time,... 613 | obj.orth.D,obj.nS,obj.nU,... 614 | obj.orth.ww,obj.scale );% cost for optimal control 615 | controlieq = @(x)Confcn(fullx(x(1:ind)),x(ind+1:end),... 616 | obj.orth.D,obj.scale);% constraintfcn bound 617 | 618 | %obj.x=SX.sym('x',(obj.npts-1)*obj.nS+obj.npts*obj.nU,1);% casadi variable for state at node and parameter 619 | obj.x=SX.sym('x',(obj.npts-2)*obj.nS+obj.npts*obj.nU,1);% casadi variable for state at node and parameter 620 | nlp=struct('x',obj.x,'f',cost(obj.x),...% objective = minimize cost fcn 621 | 'g',[controlieq(obj.x);controleqn(obj.x)]);% constraints = extconstr, dynamics 622 | 623 | opts.ipopt.max_iter=maxiter;% set maximum iteration number 624 | opts.ipopt.print_level=displev;% set display level 625 | obj.S=nlpsol('S','ipopt',nlp,opts);% optimization problem 626 | disp([type,': Initialize over, solving ......']); 627 | r=obj.S('x0',obj.guess.state,'lbg',[kron(ones(obj.npts,1),conlb);zeros(obj.npts*obj.nS,1)],... 628 | 'ubg',[kron(ones(obj.npts,1),conub);zeros(obj.npts*obj.nS,1)]);% solve optimization problem 629 | xopt=full(r.x);% get numerical result 630 | 631 | Xc = fullx(xopt(1:ind)); 632 | Xc = reshape(Xc,obj.nS,[]); 633 | Xc = Xc.'; 634 | Uc = xopt(ind+1:end); 635 | Uc = reshape(Uc,obj.nU,[]); 636 | Uc = Uc.'; 637 | 638 | Xopt=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 639 | Uopt=ChebTest.barycentricInterpolate(time(:),Uc,obj.guess.time(:),obj.orth.vv(:)); 640 | end 641 | 642 | % function for optimal control 643 | function [Xc,Uc,Xopt,Uopt] = optimalControlPre(obj,Dynfcn,Costfcn,Confcn,conlb,conub,time,u,init,maxiter,displev,sGuess) 644 | % optimal control use collocation 645 | % input: 646 | % Dynfcn: dynamics function handle for vectorized discretized states and control 647 | % format: Dynfcn( Xc,Uc,D,nS,scale,P ) 648 | % Xc: state at nodes 649 | % Uc: control at nodes 650 | % D: differential matrix 651 | % nS: dim of states 652 | % scale: time scale 653 | % P: parameter 654 | % Costfcn: function handle cost function 655 | % format: Costfcn( Xc,Uc,tc,D,nS,nU,w,scale ) 656 | % Confcn: function handle of constraint function !!!: need modification and consideration... 657 | % format: Confcn( Xc,Uc,D,scale ) 658 | % conlb: lower bound of constraint function 659 | % conub: upper bound of constraint function 660 | % target: target of output, NxnO(row for time, col for var) 661 | % time: simulation time 662 | % u: input, corresponding to time(col for control, row for time) 663 | % init: initial condition for states 664 | % maxiter: maximum iteration for optimization 665 | % displev: disp level for optimization 666 | % output: 667 | % Pvid: discretized states(col for state, row for time) 668 | % Xsim: simulated states corresponding to time(col for state, row for time) 669 | type = 'Optimal Control'; 670 | import casadi.*; 671 | disp([type,': Initialize optimization problem ......']); 672 | init=init(:);% reshape to col vec 673 | time=time(:).';% reshape to row vec 674 | fullx=@(x)[init;x];% from xc to get full x with init cond 675 | [obj.orth.xx,obj.orth.ww,obj.orth.vv]=chebpts(obj.npts);% cheb pnts and weights 676 | obj.orth.D=ChebTest.getDifferentiationMatrix(obj.orth.xx,obj.orth.vv);% differential matrix 677 | obj.guess.tSpan=[time(1),time(end)];% get time span 678 | obj.guess.time=chebpts(obj.npts,obj.guess.tSpan).';% get time nodes 679 | %ls = interp1(time(:),obj.sGuess,obj.guess.time(2:end).');% initial of states at nodes 680 | ls = interp1(time(:),sGuess,obj.guess.time(2:end).');% initial of states at nodes 681 | obj.guess.state=reshape(ls.',numel(ls),1);% get initial state vec 682 | Uc=interp1(time(:),u,obj.guess.time(:));% control at nodes 683 | Uc=reshape(Uc.',numel(Uc),1); 684 | obj.guess.state=[obj.guess.state;Uc];% get initial state and control vec 685 | obj.scale=(obj.guess.tSpan(2)-obj.guess.tSpan(1))/2;% time scale 686 | ind=obj.nS*(obj.npts-1);% index seperation of state and control 687 | controleqn = @(x)Dynfcn(fullx(x(1:ind)),... 688 | x(ind+1:end),obj.orth.D,obj.nS,obj.nU,obj.scale,obj.P);% dynamics constraint 689 | 690 | cost = @(x)Costfcn( fullx(x(1:ind)),x(ind+1:end),obj.guess.time,... 691 | obj.orth.D,obj.nS,obj.nU,... 692 | obj.orth.ww,obj.scale );% cost for optimal control 693 | controlieq = @(x)Confcn(fullx(x(1:ind)),x(ind+1:end),... 694 | obj.orth.D,obj.scale);% constraintfcn bound 695 | 696 | obj.x=SX.sym('x',(obj.npts-1)*obj.nS+obj.npts*obj.nU,1);% casadi variable for state at node and parameter 697 | %nlp=struct('x',obj.x,'f',cost(obj.x),...% objective = minimize cost fcn 698 | % 'g',[controlieq(obj.x);controleqn(obj.x)]);% constraints = extconstr, dynamics 699 | 700 | opts.ipopt.max_iter=maxiter;% set maximum iteration number 701 | opts.ipopt.print_level=displev;% set display level 702 | %obj.S=nlpsol('S','ipopt',nlp,opts);% optimization problem 703 | disp([type,': Initialize over, solving ......']); 704 | r=obj.S('x0',obj.guess.state,'lbg',[kron(ones(obj.npts,1),conlb);zeros(obj.npts*obj.nS,1)],... 705 | 'ubg',[kron(ones(obj.npts,1),conub);zeros(obj.npts*obj.nS,1)]);% solve optimization problem 706 | xopt=full(r.x);% get numerical result 707 | 708 | Xc = fullx(xopt(1:ind)); 709 | Xc = reshape(Xc,obj.nS,[]); 710 | Xc = Xc.'; 711 | Uc = xopt(ind+1:end); 712 | Uc = reshape(Uc,obj.nU,[]); 713 | Uc = Uc.'; 714 | 715 | Xopt=ChebTest.barycentricInterpolate(time(:),Xc,obj.guess.time(:),obj.orth.vv(:)); 716 | Uopt=ChebTest.barycentricInterpolate(time(:),Uc,obj.guess.time(:),obj.orth.vv(:)); 717 | end 718 | 719 | end 720 | 721 | end 722 | 723 | --------------------------------------------------------------------------------