Commit 7209d52d authored by Christian Hespe's avatar Christian Hespe
Browse files

Gitlab Runner Update from atc-aux-files/master:Merge branch 'mpc-exercises' into 'master'

parent 4165bf91
...@@ -24,30 +24,22 @@ N = 40; ...@@ -24,30 +24,22 @@ N = 40;
Q = diag([200, 1000, 0.1, 10]); Q = diag([200, 1000, 0.1, 10]);
R = 2000; R = 2000;
Q_hat = kron(eye(N), Q); Q_bar = kron(eye(N), Q);
R_hat = kron(eye(N), R); R_bar = kron(eye(N), R);
Phi = zeros(N*nx, nx); Phi = zeros(N*nx, nx);
Gamma = zeros(N*nx, N*nu); Gamma = zeros(N*nx, N*nu);
for k = 1:N for k = 1:N
x_range = (k-1)*nx+1:k*nx; x_range = (k-1)*nx+1:k*nx;
u_range = (k-1)*nu+1:k*nu;
if k == 1 Phi(x_range, :) = A^k;
Phi(x_range, :) = A; for j = 1:k
else Gamma(x_range, j) = A^(k-j)*B;
Phi(x_range, :) = A*Phi(x_range-nx, :);
end
Gamma(x_range, u_range) = B;
for j = 1:k-1
z_range = (j-1)*nu+1:j*nu;
Gamma(x_range, z_range) = A*Gamma(x_range-nx, z_range);
end end
end end
H = Gamma' * Q_hat * Gamma + R_hat; H = Gamma' * Q_bar * Gamma + R_bar;
g_hat = Gamma' * Q_hat * Phi; g_hat = Gamma' * Q_bar * Phi;
Acons = kron(eye(N), [eye(nu); -eye(nu)]); Acons = kron(eye(N), [eye(nu); -eye(nu)]);
bcons = kron(ones(2*N,1), u_bar); bcons = kron(ones(2*N,1), u_bar);
...@@ -66,7 +58,7 @@ for k = 1:Nsim ...@@ -66,7 +58,7 @@ for k = 1:Nsim
tic tic
g = g_hat * x_mpc(:,k); g = g_hat * x_mpc(:,k);
u = quadprog(H, g, Acons, bcons); u = quadprog(H, g, Acons, bcons);
t_mpc(k) = toc; t_mpc(k) = toc;
if isempty(u) if isempty(u)
error('Quadprog failed to find a solution') error('Quadprog failed to find a solution')
......
...@@ -24,14 +24,13 @@ N = % FILL OUT ...@@ -24,14 +24,13 @@ N = % FILL OUT
Q = % FILL OUT Q = % FILL OUT
R = % FILL OUT R = % FILL OUT
Q_hat = kron(eye(N), Q); Q_bar = kron(eye(N), Q);
R_hat = kron(eye(N), R); R_bar = kron(eye(N), R);
Phi = zeros(N*nx, nx); Phi = zeros(N*nx, nx);
Gamma = zeros(N*nx, N*nu); Gamma = zeros(N*nx, N*nu);
for k = 1:N for k = 1:N
x_range = (k-1)*nx+1:k*nx; x_range = (k-1)*nx+1:k*nx;
u_range = (k-1)*nu+1:k*nu;
% FILL OUT % FILL OUT
end end
...@@ -56,7 +55,7 @@ for k = 1:Nsim ...@@ -56,7 +55,7 @@ for k = 1:Nsim
tic tic
g = g_hat * x_mpc(:,k); g = g_hat * x_mpc(:,k);
u = quadprog(H, g, Acons, bcons); u = quadprog(H, g, Acons, bcons);
t_mpc(k) = toc; t_mpc(k) = toc;
if isempty(u) if isempty(u)
error('Quadprog failed to find a solution') error('Quadprog failed to find a solution')
......
...@@ -43,20 +43,10 @@ Phi = zeros(N*nx, nx); ...@@ -43,20 +43,10 @@ Phi = zeros(N*nx, nx);
Gamma = zeros(N*nx, N*nu); Gamma = zeros(N*nx, N*nu);
for k = 1:N for k = 1:N
x_range = (k-1)*nx+1:k*nx; x_range = (k-1)*nx+1:k*nx;
u_range = (k-1)*nu+1:k*nu;
if k == 1 Phi(x_range, :) = A^k;
Phi(x_range, :) = A; for j = 1:k
else Gamma(x_range, j) = A^(k-j)*B;
Phi(x_range, :) = A*Phi(x_range-nx, :);
end
Gamma(x_range, u_range) = B;
for j = 1:k-1
z_range = (j-1)*nu+1:j*nu;
Gamma(x_range, z_range) = A*Gamma(x_range-nx, z_range);
end end
end end
......
...@@ -42,18 +42,10 @@ Phi = zeros(N*nx, nx); ...@@ -42,18 +42,10 @@ Phi = zeros(N*nx, nx);
Gamma = zeros(N*nx, N*nu); Gamma = zeros(N*nx, N*nu);
for k = 1:N for k = 1:N
x_range = (k-1)*nx+1:k*nx; x_range = (k-1)*nx+1:k*nx;
u_range = (k-1)*nu+1:k*nu;
if k == 1 Phi(x_range, :) = A^k;
Phi(x_range, :) = A; for j = 1:k
else Gamma(x_range, j) = A^(k-j)*B;
Phi(x_range, :) = A*Phi(x_range-nx, :);
end
Gamma(x_range, u_range) = B;
for j = 1:k-1
z_range = (j-1)*nu+1:j*nu;
Gamma(x_range, z_range) = A*Gamma(x_range-nx, z_range);
end end
end end
......
...@@ -46,18 +46,10 @@ Phi = zeros(N*nx, nx); ...@@ -46,18 +46,10 @@ Phi = zeros(N*nx, nx);
Gamma = zeros(N*nx, N*nu); Gamma = zeros(N*nx, N*nu);
for k = 1:N for k = 1:N
x_range = (k-1)*nx+1:k*nx; x_range = (k-1)*nx+1:k*nx;
u_range = (k-1)*nu+1:k*nu;
if k == 1 Phi(x_range, :) = A^k;
Phi(x_range, :) = A; for j = 1:k
else Gamma(x_range, j) = A^(k-j)*B;
Phi(x_range, :) = A*Phi(x_range-nx, :);
end
Gamma(x_range, u_range) = B;
for j = 1:k-1
z_range = (j-1)*nu+1:j*nu;
Gamma(x_range, z_range) = A*Gamma(x_range-nx, z_range);
end end
end end
......
% Authors: Christian Hespe
% Institute of Control Systems, Hamburg University of Technology
% Date: 2022-01-17
%
% This file implements a moving horizon estimator for a simple second order
% damped oscillator.
addpath(genpath('../../common'))
clear
% Seed the pseudo random number generator. This is required if we want
% reproducible simulation, e. g. for profiling the code.
% rng(0);
%%
x0 = [ 0; 1 ];
T = 0.1;
A = [ 0.985 0.1 ;
-0.1 0.985 ];
C = [ 1 0 ];
nx = size(A,1);
ny = size(C,1);
x1min = -Inf;
% x1min = -0.3;
%%
N = 5;
Q = 1e-5*eye(2);
R = 1e-3;
% Q and R are noise variances. The noise samples can be generated from zero
% mean unit variance Gaussian random variables by multiplying with the
% matrix square roots, i.e. Q^{1/2} and R^{1/2}.
sqrtQ = sqrtm(Q);
sqrtR = sqrtm(R);
% Synthesize discrete-time Kalman filter
[L, P] = dlqr(A', C', Q, R);
L = -L'; % Fix sign convention and go to the dual problem
%% Construct MHE
% Construct a moving horizon estimator using Yalmip. We have the process
% noise w and the measurement noise v
x = sdpvar(nx,1);
x_hat = sdpvar(nx,1);
W = sdpvar(repmat(nx,1,N),ones(1,N));
Y = sdpvar(repmat(ny,1,N),ones(1,N));
cost = quad(x - x_hat, inv(P));
cnst = x(1) >= x1min;
for i = 1:N
v = Y{i} - C*x;
x = A*x + W{i};
cost = cost + quad(W{i}, inv(Q)) + quad(v, inv(R));
cnst = [ cnst, x(1) >= x1min ];
end
mhe_yalmip = optimizer(cnst, cost, [], {x_hat, [Y{:}]}, x);
%% Run simulation with MHE
Tf = 30;
Nsim = ceil(Tf / T);
% Allocate simulation memory
x_plant = zeros(nx, Nsim+1);
y_plant = zeros(ny, Nsim);
% Allocate estimator memory
x_kf = zeros(nx, Nsim);
x_mhe = zeros(nx, Nsim);
opts = sdpsettings('verbose', 0);
% Initialize simulation
x_plant(:,1) = x0;
% x_kf(:,1) = x0;
% x_mhe(:,1) = x0;
% Initialize full information estimation
x_fie = sdpvar(nx,1);
cost = quad(x_fie - x_mhe(:,1), inv(P));
cnst = x_fie(1) >= x1min;
for k = 1:Nsim
% Measure the output
vk = sqrtR * randn(ny,1);
y_plant(:,k) = C*x_plant(:,k) + vk;
% Update the Kalman filter
x_kf(:,k+1) = (A+L*C)*x_kf(:,k) - L*y_plant(:,k);
% Update the MHE
if k < N % Full information estimation for the first N-1 steps
w = sdpvar(nx,1);
v = y_plant(:,k) - C*x_fie;
x_fie = A*x_fie + w;
cost = cost + quad(w, inv(Q)) + quad(v, inv(R));
cnst = [ cnst, x_fie(1) >= x1min ];
optimize(cnst, cost, opts);
x_mhe(:,k+1) = value(x_fie);
else % MHE after that
x_mhe(:,k+1) = mhe_yalmip(x_mhe(:,k-N+1), y_plant(:,k-N+1:k));
end
% Apply state update
wk = sqrtQ * randn(nx,1);
x_plant(:,k+1) = A*x_plant(:,k) + wk;
% Artifial state constraint
x_plant(1,k+1) = max(x1min, x_plant(1,k+1));
end
%%
Tsim = T * (0:Nsim);
figure(1)
stairs(Tsim(1:end-1), y_plant)
hold on
stairs(Tsim(1:end-1), C*x_kf(:,1:end-1))
stairs(Tsim(1:end-1), C*x_mhe(:,1:end-1))
hold off
title('Output Matching')
xlabel('Time t in s')
legend('True', 'Kalman', 'MHE')
figure(2)
ax1 = subplot(211);
stairs(Tsim, x_plant(1,:))
hold on
stairs(Tsim, x_kf(1,:))
stairs(Tsim, x_mhe(1,:))
hold off
xlabel('Time t in s')
ylabel('State x_1(t)')
legend('True', 'Kalman', 'MHE')
ax2 = subplot(212);
stairs(Tsim, x_plant(2,:))
hold on
stairs(Tsim, x_kf(2,:))
stairs(Tsim, x_mhe(2,:))
hold off
xlabel('Time t in s')
ylabel('State x_2(t)')
legend('True', 'Kalman', 'MHE')
linkaxes([ax1, ax2])
%%
function a = quad(z, M)
a = z'*M*z;
end
% Authors: Christian Hespe
% Institute of Control Systems, Hamburg University of Technology
% Date: 2022-01-18
%
% This file implements a tracking MPC for the linearized ADIP model. A
% setpoint generator is used to demonstrate the trouble with proportional
% feedback.
addpath(genpath('../../common'))
clear
%% Model definition
T = 0.01;
[A, B, C] = adip_lti(T);
nx = size(A, 1);
nu = size(B, 2);
ny = size(C, 1);
x0 = zeros(4,1);
y_ref = [deg2rad(15); 0];
u_bar = 1.6;
%% Controller Setup
N = 40;
Q = diag([200, 1000, 0.1, 10]);
R = 2000;
% Calculate setpoint
sp_mat = [ A - eye(nx) B ;
C zeros(ny,nu) ];
sp_vec = [ zeros(nx,1); y_ref ];
sp_A = [ zeros(nu,nx) eye(nu) ;
zeros(nu,nx) -eye(nu) ];
sp_b = [ u_bar; u_bar ];
z = lsqlin(sp_mat, sp_vec, sp_A, sp_b);
x_star = z(1:nx);
% u_star = z(nx+1:end);
u_star = zeros(nu,1);
% Generate MPC scheme
U = sdpvar(repmat(nu,1,N),ones(1,N));
x_hat = sdpvar(nx,1);
x_ref = sdpvar(nx,1);
u_ref = sdpvar(nu,1);
cnts = [];
cost = 0;
% Objective and constraint functions generation:
x = x_hat;
for k = 1:N
x = A*x + B*U{k};
u = U{k} - u_ref;
e = x_ref - x;
cost = cost + e'*Q*e + u*R*u;
cnts = [cnts, -u_bar <= U{k} <= u_bar];
end
% Generate prepared solver for this MPC problem
controller = optimizer(cnts, cost, [], {x_hat, x_ref, u_ref}, U{1});
%% Run simulation with MPC
Tf = 3;
Nsim = ceil(Tf / T);
x_mpc = zeros(nx, Nsim+1);
u_mpc = zeros(nu, Nsim);
t_mpc = zeros(Nsim,1);
x_mpc(:,1) = x0;
cost_mpc = 0;
for k = 1:Nsim
tic
[uk, err] = controller{x_mpc(:,k), x_star, u_star};
t_mpc(k) = toc;
if err ~= 0
error('YALMIP can not find a solution')
end
u_mpc(:,k) = clamp(uk, u_bar);
odefun = @(~, x) adip_ode(x,uk);
[~, x] = ode45(odefun, [(k-1)*T, k*T], x_mpc(:,k));
x_mpc(:,k+1) = x(end,:);
cost_mpc = cost_mpc + x_mpc(:,k)'*Q*x_mpc(:,k) + u_mpc(:,k)'*R*u_mpc(:,k);
end
fprintf('Total cost using MPC: %g\n', cost_mpc);
fprintf('Mean solver time : %g [ms]\n', mean(t_mpc)*1e3);
fprintf('Median solver time : %g [ms]\n', median(t_mpc)*1e3);
fprintf('Max solver time : %g [ms]\n', max(t_mpc)*1e3);
%%
figure(1)
stairs(0:T:Tf-T, u_mpc, 'r')
yline(u_star)
yline( u_bar, 'k--')
yline(-u_bar, 'k--')
ylim padded
title('Control Trajectory')
xlabel('Time t')
ylabel('Torque \tau')
figure(2)
pause(0.1)
for k = 1:Nsim+1
draw_adip(x_star, 'k--')
hold on
draw_adip(x_mpc(:,k), 'b')
hold off
grid on
xlim([-0.1, 0.25])
ylim([-0.1, 0.25])
title('Movement of the Pendulum')
xlabel('x Coordinate')
ylabel('y Coordinate')
drawnow limitrate
end
function [A, B] = adip_lti(T) function [A, B, C] = adip_lti(T)
%ADIP_LTI Linearized ADIP model %ADIP_LTI Linearized ADIP model
% This function provides system matrices for the ADIP that are obtained % This function provides system matrices for the ADIP that are obtained
% by Jacobi linearization around the origin of the state-space. % by Jacobi linearization around the origin of the state-space.
...@@ -30,6 +30,7 @@ A = [ zeros(2) eye(2) ; ...@@ -30,6 +30,7 @@ A = [ zeros(2) eye(2) ;
-J*K -J*L ]; -J*K -J*L ];
B = [ zeros(2,1) ; B = [ zeros(2,1) ;
J * [1; 0] ]; J * [1; 0] ];
C = [ eye(2) zeros(2) ];
% Discretize if sampling time is given % Discretize if sampling time is given
if nargin >= 1 if nargin >= 1
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment