8000 MATLAB: migrate MHE example to new interface by FreyJo · Pull Request #1249 · acados/acados · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

MATLAB: migrate MHE example to new interface #1249

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Sep 13, 2024
Merged

MATLAB: migrate MHE example to new interface #1249

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
abe6003
migrate MHE example to new interface
FreyJo Sep 12, 2024
48b3000
remove lorentz test
FreyJo Sep 12, 2024
e05cb90
test lorentz example in octave
FreyJo Sep 12, 2024
f94a2ea
remove lorentz test in MATLAB
FreyJo Sep 12, 2024
0d00733
fix path for cmake test
FreyJo Sep 12, 2024
aed2f03
Matlab use single parenthesis
FreyJo Sep 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/linux/export_paths.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,5 +34,5 @@ echo "ACADOS_INSTALL_DIR=$1/acados" >> $GITHUB_ENV
echo "LD_LIBRARY_PATH=$1/acados/lib" >> $GITHUB_ENV
echo "MATLABPATH=$MATLABPATH:$1/acados/interfaces/acados_matlab_octave:$1/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-matlab" >> $GITHUB_ENV
echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/acados/interfaces/acados_matlab_octave:${1}/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-octave" >> $GITHUB_ENV
echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code" >> $GITHUB_ENV
echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code" >> $GITHUB_ENV
echo "ENV_RUN=true" >> $GITHUB_ENV
42 changes: 16 additions & 26 deletions examples/acados_matlab_octave/lorentz/example_mhe.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,30 +30,22 @@
%
% author: Katrin Baumgaertner




% NOTE: `acados` currently supports both an old MATLAB/Octave interface (< v0.4.0)
% as well as a new interface (>= v0.4.0).

% THIS EXAMPLE still uses the OLD interface. If you are new to `acados` please start
% with the examples that have been ported to the new interface already.
% see https://github.com/acados/acados/issues/1196#issuecomment-2311822122)

clear all

h = 0.05; % time step
N_mhe = 15; % MHE horizon

%% model
model = lorentz_model();

nx = model.nx;
nu = model.nu;
ny = model.ny;
sim_solver = setup_integrator(model, h);
estimator = setup_estimator(model, h, N_mhe);

sim_solver = setup_integrator(model);
estimator = setup_estimator(model);
nx = estimator.ocp.dims.nx;
nu = estimator.ocp.dims.nu;
ny = estimator.ocp.dims.ny - estimator.ocp.dims.nu; % cost ny = dimension of measurement y + noise w

%% Simulation

N_sim = 120;

iter_step = 50;
Expand Down Expand Up @@ -90,45 +82,43 @@
end

%% Estimation

x_est = zeros(nx, N_sim-model.N);
x_est = zeros(nx, N_sim-N_mhe);

yref_0 = zeros(ny + nu + nx, 1);
yref = zeros(ny + nu, 1);

for n=1:N_sim-model.N
for n=1:N_sim-N_mhe

% set measurements
yref_0(1:ny) = y_sim(:, n);
yref_0(ny+nu+1:end) = x0_bar;

estimator.set('cost_y_ref', yref_0, 0);

for i=1:model.N-1
for i=1:N_mhe-1
yref(1:ny) = y_sim(:, n+i);
estimator.set('cost_y_ref', yref, i);
end

%estimator.set('init_x', x_sim(:, n:n+model.N))

%estimator.set('init_x', x_sim(:, n:n+N_mhe))
% solve
estimator.solve()

x_est(:, n) = estimator.get('x', model.N);
x_est(:, n) = estimator.get('x', N_mhe);

% update arrival cost (TODO: update P0 as well)
x0_bar = estimator.get('x', 1);
end

%% Plot
ts = model.h*(0:N_sim);
ts = h*(0:N_sim);

figure;
States = {'x_1', 'x_2', 'x_3', 'p'};
for i=1:length(States)
subplot(length(States), 1, i); hold on;
plot(ts, x_sim(i,:));
plot(ts(model.N+1:end-1), x_est(i,:));
plot(ts(N_mhe+1:end-1), x_est(i,:));

if i == 1
plot(ts(1:end-1), y_sim, 'x');
Expand All @@ -144,7 +134,7 @@
for i=1:length(States)
subplot(length(States), 1, i); hold on; grid on;

plot(ts(model.N+1:end-1), abs(x_est(i,:) - x_sim(i, model.N+1:end-1)));
plot(ts(N_mhe+1:end-1), abs(x_est(i,:) - x_sim(i, N_mhe+1:end-1)));

ylabel(States{i});
xlabel('t [s]');
Expand Down
25 changes: 6 additions & 19 deletions examples/acados_matlab_octave/lorentz/lorentz_model.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,35 +52,22 @@
nw = 1; % state noise on parameter
ny = 1;

% weighting matrices
Q = 1*eye(nw);
R = 1;
P0 = 0.1*eye(nx);
P0(nx, nx) = 0.001;

% dynamics
x_expr = SX.sym('x', nx, 1);
w_expr = SX.sym('w', nw, 1);

x_dot_expr = [10*(x_expr(2) - x_expr(1)); ...
f_expl_expr = [10*(x_expr(2) - x_expr(1)); ...
x_expr(4)*x_expr(1)-x_expr(2)-x_expr(1)*x_expr(3); ...
-(8/3)*x_expr(3)+x_expr(1)*x_expr(2); ...
w_expr];

% store eveything in model struct
model = struct();
model.nx = nx;
model.nu = nw;
model.ny = ny;
model = AcadosModel();

model.sym_x = x_expr;
model.sym_u = w_expr;
model.f_expl_expr = x_dot_expr;

model.W_0 = blkdiag(R, Q, P0);
model.W = blkdiag(R, Q);

model.h = 0.05;
< EDB7 /td> model.N = 15; % MHE horizon
model.x = x_expr;
model.u = w_expr;
model.f_expl_expr = f_expl_expr;

model.name = 'lorentz_model_estimator';
end
130 changes: 54 additions & 76 deletions examples/acados_matlab_octave/lorentz/setup_estimator.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,101 +28,79 @@
% POSSIBILITY OF SUCH DAMAGE.;


function [estimator] = setup_estimator(model, h, N)

% NOTE: `acados` currently supports both an old MATLAB/Octave interface (< v0.4.0)
% as well as a new interface (>= v0.4.0).
T = N * h;

% THIS EXAMPLE still uses the OLD interface. If you are new to `acados` please start
% with the examples that have been ported to the new interface already.
% see https://github.com/acados/acados/issues/1196#issuecomment-2311822122)
%% model dynamics
nx = length(model.x);
nu = length(model.u);
ny = 1;
nw = nu; % state noise on parameter

ocp = AcadosOcp();

function [estimator] = setup_estimator(model)
ocp.model = model;
ocp.solver_options.tf = T;

N = model.N;
T = N * model.h;
%% cost
% weighting matrices
Q = 1*eye(nw);
R = 1;
P0 = 0.1*eye(nx);
P0(nx, nx) = 0.001;

nlp_solver = 'sqp'; % sqp, sqp_rti
qp_solver = 'partial_condensing_hpipm';
% full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases
W_0 = blkdiag(R, Q, P0);
W = blkdiag(R, Q);

% integrator type
sim_method = 'erk'; % erk, irk, irk_gnsf
ocp.cost.cost_type = 'LINEAR_LS';
ocp.cost.cost_type_0 = 'LINEAR_LS';
ocp.cost.cost_type_e = 'LINEAR_LS';

%% model dynamics
nx = model.nx;
nu = model.nu;
ny = model.ny;
nout = ny + nu;
nout_0 = ny + nu + nx;

ocp_model = acados_ocp_model();
model_name = 'lorentz_model_estimator';
Vx = zeros(nout, nx);
Vx(1, 1) = 1;

%% acados ocp model
ocp_model.set('name', model_name);
ocp_model.set('T', T);
Vu = zeros(nout, nu);
Vu(ny+1:ny+nu, :) = eye(nu);

% symbolics
ocp_model.set('sym_x', model.sym_x);
ocp_model.set('sym_u', model.sym_u);
ocp_model.set('dyn_expr_f', model.f_expl_expr);
Vx_0 = zeros(nout_0, nx);
Vx_0(1:ny, :) = eye(ny, nx);
Vx_0(ny+nu+1:end, :) = eye(nx);

% cost
ocp_model.set('cost_type', 'linear_ls');
ocp_model.set('cost_type_0', 'linear_ls');
ocp_model.set('cost_type_e','linear_ls');
Vu_0 = zeros(nout_0, nu);
Vu_0(ny+1:ny+nu, :) = eye(nu);

nout = ny + nu;
nout_0 = ny + nu + nx;
yref = zeros(nout, 1);
yref_0 = zeros(nout_0, 1);

Vx = zeros(nout, nx);
Vx(1, 1) = 1;
ocp.cost.Vx = Vx;
ocp.cost.Vu = Vu;

Vu = zeros(nout, nu);
Vu(ny+1:ny+nu, :) = eye(nu);
ocp.cost.Vx_0 = Vx_0;
ocp.cost.Vu_0 = Vu_0;

Vx_0 = zeros(nout_0, nx);
Vx_0(1:ny, :) = eye(ny, nx);
Vx_0(ny+nu+1:end, :) = eye(nx);
ocp.cost.yref = yref;
ocp.cost.yref_0 = yref_0;

Vu_0 = zeros(nout_0, nu);
Vu_0(ny+1:ny+nu, :) = eye(nu);
ocp.cost.W = W;
ocp.cost.W_0 = W_0;

yref = zeros(nout, 1);
yref_0 = zeros(nout_0, 1);
%% options
ocp.solver_options.N_horizon = N;
ocp.solver_options.nlp_solver_type = 'SQP';
ocp.solver_options.integrator_type = 'ERK';

ocp_model.set('cost_Vx', Vx);
ocp_model.set('cost_Vu', Vu);
ocp.solver_options.sim_method_num_stages = 2;
ocp.solver_options.sim_method_num_steps = 5;

ocp_model.set('cost_Vx_0', Vx_0);
ocp_model.set('cost_Vu_0', Vu_0);

ocp_model.set('cost_y_ref', yref);
ocp_model.set('cost_y_ref_0', yref_0);

ocp_model.set('cost_W', model.W);
ocp_model.set('cost_W_0', model.W_0);

% dynamics
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.f_expl_expr);


%% acados ocp set opts
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('sim_method', sim_method);

ocp_opts.set('sim_method_num_stages', 2);
ocp_opts.set('sim_method_num_steps', 5);

ocp_opts.set('qp_solver', qp_solver);
ocp_opts.set('qp_solver_cond_N', N);
ocp_opts.set('print_level', 0);
ocp_opts.set('ext_fun_compile_flags', '');

%% create ocp solver
estimator = acados_ocp(ocp_model, ocp_opts);
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM';
ocp.solver_options.qp_solver_cond_N = N;
ocp.solver_options.print_level = 0;
ocp.solver_options.ext_fun_compile_flags = '';

%% create ocp solver
estimator = AcadosOcpSolver(ocp);
end

50 changes: 14 additions & 36 deletions examples/acados_matlab_octave/lorentz/setup_integrator.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,40 +27,18 @@
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;




% NOTE: `acados` currently supports both an old MATLAB/Octave interface (< v0.4.0)
% as well as a new interface (>= v0.4.0).

% THIS EXAMPLE still uses the OLD interface. If you are new to `acados` please start
% with the examples that have been ported to the new interface already.
% see https://github.com/acados/acados/issues/1196#issuecomment-2311822122)


function [sim_solver] = setup_integrator(model)

model_name = 'lorentz_model_integrator';
sim_model = acados_sim_model();
sim_model.set('name', model_name);
sim_model.set('T', model.h);

sim_model.set('sym_x', model.sym_x);
sim_model.set('sym_u', model.sym_u);

% explit integrator (erk)
sim_model.set('dyn_type', 'explicit');
sim_model.set('dyn_expr_f', model.f_expl_expr);

% options
sim_opts = acados_sim_opts();

sim_opts.set('num_stages', 2);
sim_opts.set('num_steps', 5);
sim_opts.set('method', 'erk');
sim_opts.set('sens_forw', 'true'); % generate forward sensitivities

% create acados integrator
sim_solver = acados_sim(sim_model, sim_opts);
function [sim_solver] = setup_integrator(model, h)
sim = AcadosSim();
sim.model = model;
sim.model.name = 'lorentz_model_integrator';
sim.solver_options.Tsim = h;

% options
sim.solver_options.num_stages = 2;
sim.solver_options.num_steps = 5;
sim.solver_options.integrator_type = 'ERK';
sim.solver_options.sens_forw = true; % generate forward sensitivities

% create acados integrator
sim_solver = AcadosSimSolver(sim);
end

1 change: 0 additions & 1 deletion examples/acados_matlab_octave/test/run_matlab_tests.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,6 @@
"run_test_sim_dae",
% "run_test_sim_forw",
"run_test_sim_hess",
"run_test_mhe_lorentz",
"param_test",
];

Expand Down
Loading
Loading
0