【dr.can】最优控制第三节matlab代码

初始代码（符合第二节视频）

%matlab动态规划的代码实现

clear;
clc;

%%%%%%%%%%%%%%%%%%设立系统%%%%%%%%%%%%%%
%define IC定义系统初始状态
h_init=0;%h:height
v_init=0;%v:velocity
%define final state终值状态
h_final=10;
v_final=0;
%boundary condition设定边界条件
h_min=0;
h_max=10;N_h=5;%离散为5段
v_min=0;
v_max=3;N_v=3;%速度分三段
%create state array创建边界数组（xy轴）
Hd=h_min:(h_max-h_min)/N_h:h_max;
Vd=v_min:(v_max-v_min)/N_v:v_max;
%input constraint,input is the system acceleration输入约束
u_min=-3;%u=input=a;
u_max=2;
%define cost to go matrix定义cost to go矩阵
J_costtogo=zeros(N_h + 1,N_v+ 1);
%define input acceleration matrix
Input_acc=zeros(N_h + 1,N_v + 1);

%%%%%%%%%%%%%%%from 10m to 8m%%%%%%%%%%%%%%%%%%%%%%%
%calculate average speed计算每种选择的平均速度
v_avg=0.5*(v_final+Vd) ; %0 0.5 1 1.5
%calculate travel time ,this is the cost
T_delta = (h_max - h_min)./(N_h * v_avg);%2/v_avg
%calculate acceleration
acc=(0-Vd)./T_delta;
%assigh delta T to cost to go
J_temp=T_delta;%临时代价（未考虑系统约束）
%find which acc is over the limit
[acc_x,acc_y]=find(acc<u_min|acc>u_max);
    %[acc_x,acc_y]:(超过的行，超过的列）
%find linear index查找对应超过位置在acc矩阵里的线性索引
Ind_lin_acc=sub2ind(size(acc),acc_x,acc_y);
%let certain elements to infitiy在J_temp里把对应位置也变成Inf
J_temp(Ind_lin_acc)=Inf;
%save to cost to go matrix
J_costtogo(2,:)=J_temp;
%save to acceleration matrix
Input_acc(2,:)=acc;



%%%%%%%%%%%%%%%from 8m to 2m%%%%%%%%%%%%%%%%%%%%%%%
%终端位置有4种可能 每一个起始位置都要考虑对应的4个终端位置 一共16种选择
%还要加上上一步的cost to go再储存


for k=3:1:N_h

%prepare the matrix
[Vd_x,Vd_y]=meshgrid(Vd,Vd);
%calculate average speed计算每种选择的平均速度
v_avg=0.5*(Vd_x+Vd_y) ; 
%calculate travel time ,this is the cost
T_delta = (h_max - h_min)./(N_h * v_avg);%2/v_avg
%calculate acceleration
acc=(Vd_y-Vd_x)./T_delta;
%assigh delta T to cost to go
J_temp=T_delta;%临时代价（未考虑系统约束）
%find which acc is over the limit
[acc_x,acc_y]=find(acc<u_min|acc>u_max);
    %[acc_x,acc_y]:(超过的行，超过的列）
%find linear index查找对应超过位置在acc矩阵里的线性索引
Ind_lin_acc=sub2ind(size(acc),acc_x,acc_y);
%let certain elements to infitiy在J_temp里把对应位置也变成Inf
J_temp(Ind_lin_acc)=Inf;
%add last cost to go把之前的cost togo也加进来 
J_temp=J_temp+meshgrid(J_costtogo(k-1,:))';
    %把之前一行的costtogo变成列加到现在的每一列上
%save to cost to go matrix把jtemp里面的最小值存进去
[J_costtogo(k,:),l]=min(J_temp)%这个l什么意思？
%find linear index找到最小值对应的线性序列
Ind_lin_acc=sub2ind(size(J_temp),l,1:length(l));
%save to acceleration matrix把最小j对应的a放进去 
Input_acc(k,:)=acc(Ind_lin_acc)

end

%%%%%%%%%%%%%%%%%%%%%%%%%from 2m to 0m%%%%%%%%%%%%%%%%%%
%calculate average speed计算每种选择的平均速度
v_avg=0.5*(v_init+Vd) ; %0 0.5 1 1.5
%calculate travel time ,this is the cost
T_delta = (h_max - h_min)./(N_h * v_avg);%2/v_avg
%calculate acceleration
acc=(Vd-v_init)./T_delta;
%assigh delta T to cost to go
J_temp=T_delta;%临时代价（未考虑系统约束）
%find which acc is over the limit
[acc_x,acc_y]=find(acc<u_min|acc>u_max);
    %[acc_x,acc_y]:(超过的行，超过的列）
%find linear index查找对应超过位置在acc矩阵里的线性索引
Ind_lin_acc=sub2ind(size(acc),acc_x,acc_y);
%let certain elements to infitiy在J_temp里把对应位置也变成Inf
J_temp(Ind_lin_acc)=Inf;
%add last cost to go把之前的cost togo也加进来 
J_temp=J_temp+J_costtogo(N_h,:);
    %把之前一行的costtogo变成列加到现在的每一列上
%save to cost to go matrix把jtemp里面的最小值存进去
[J_costtogo(N_h+1,1),l]=min(J_temp)%这个l什么意思？
%find linear index找到最小值对应的线性序列
Ind_lin_acc=sub2ind(size(J_temp),l);
%save to acceleration matrix把最小j对应的a放进去 
Input_acc(N_h+1,1)=acc(Ind_lin_acc)

%%%%%%%%%%%%%%%%%%%plot画图%%%%%%%%%%%%%%
h_plot_init=0;%initial height
v_plot_init=0;%initial velocity

acc_plot=zeros(length(Hd),1);%define acc plot array
v_plot=zeros(length(Hd),1);%define velocity plot array
h_plot=zeros(length(Hd),1);%define height plot array

h_plot(1)=h_plot_init;%first value
v_plot(1)=v_plot_init;%first value

for k=1:1:N_h
    [min_h,h_plot_index]=min(abs(h_plot(k)-Hd));%Table look up
    [min_v,v_plot_index]=min(abs(v_plot(k)-Vd));%Table look up
    %find control input/acceleration
    acc_index=sub2ind(size(Input_acc),N_h+2-h_plot_index,v_plot_index);
    %save aceleration to the matrix
    acc_plot(k)=Input_acc(acc_index);
    %calculate speed and height
    v_plot(k+1)=sqrt((2*(h_max-h_min)/N_h*acc_plot(k))+v_plot(k)^2);
    h_plot(k+1)=h_plot(k)+(h_max-h_min)/N_h;
end

%plot
subplot(2,1,1);
plot(v_plot,h_plot,'--^'),grid on;
ylabel('h(m)');
xlabel('v(m/s)');

subplot(2,1,2);
plot(acc_plot,h_plot,'^'),grid on;
xlabel('a(m/s^2)');
ylabel('h(m)');