summaryrefslogtreecommitdiffstats
path: root/templates
diff options
context:
space:
mode:
authorNao Pross <np@0hm.ch>2023-05-08 14:41:38 +0200
committerNao Pross <np@0hm.ch>2023-05-08 14:41:38 +0200
commitf51f9608bc84023150c89481bd03f4343d84761f (patch)
treeeedff30c5c7ca4636a926dbc47bcc681c9a5cd59 /templates
parentFIX: pass lqr_tuning (diff)
downloadmpc_pe-f51f9608bc84023150c89481bd03f4343d84761f.tar.gz
mpc_pe-f51f9608bc84023150c89481bd03f4343d84761f.zip
ADD: lqr_tuning_script m and mat file, pass task 11
Diffstat (limited to '')
-rw-r--r--templates/lqr_tuning_script.m58
-rw-r--r--templates/lqr_tuning_script.matbin0 -> 2792 bytes
2 files changed, 58 insertions, 0 deletions
diff --git a/templates/lqr_tuning_script.m b/templates/lqr_tuning_script.m
new file mode 100644
index 0000000..4a02fc6
--- /dev/null
+++ b/templates/lqr_tuning_script.m
@@ -0,0 +1,58 @@
+function [tuning_struct] = lqr_tuning_script(params)
+ % From the hint: look in the vicinity of vector from the next section
+ % also from the hint: qx, qy, qvx, qvy are decoupled from qz, qvz
+ q_start = [94, .1579, 300, .01, .1, .1];
+
+ % parameters for sampling
+ v = 0.4; % search around q +- v * q
+ nsamples = 3;
+
+ q = q_start;
+ best_J_u = inf;
+ while best_J_u > 7.6
+ % Create sampling grid
+ range = linspace(1-v, 1+v, nsamples)';
+
+ % Sample in qx qy qvx qvy
+ G = kron(q([1,2,4,5]), range);
+ [Sx, Sy, Svx, Svy] = ndgrid(G(:,1), G(:,2), G(:,3), G(:,4));
+ Q = [Sx(:), Sy(:), q(3) * ones(size(Sx(:))), ...
+ Svx(:), Svy(:), q(6) * ones(size(Sx(:)))]';
+
+ % Find best q
+ [ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params);
+
+ % Save new best solution
+ if ~ isnan(i_opt)
+ tuning_struct = ts(i_opt);
+ best_J_u = ts(i_opt).InputCost;
+ q = ts(i_opt).Qdiag';
+ end
+
+ % Sample in qz qvz
+ G = kron(q([3,6]), range);
+ [Sz, Svz] = ndgrid(G(:,1), G(:,2));
+ qs = kron(q, ones(size(Sz(:))));
+ Q = [qs(:,1:2), Sz(:), qs(:,3:4), Svz(:)]';
+
+ % Find best item
+ [ts, i_opt] = lqr_tuning(params.model.InitialConditionA, Q, params);
+
+ % Save new best solution
+ if ~ isnan(i_opt)
+ tuning_struct = ts(i_opt);
+ best_J_u = ts(i_opt).InputCost;
+ q = ts(i_opt).Qdiag';
+ end
+
+ % Search in a narrower region, increase resolution
+ v = v / 2;
+
+ % Log
+ fprintf("Lowest J_u so far: %d, searching with v=%f\n", best_J_u,v);
+ end
+
+ % Save for test
+ matfile = struct("tuning_struct", tuning_struct, "q", tuning_struct.Qdiag);
+ save("lqr_tuning_script.mat", "-struct", "matfile");
+end
diff --git a/templates/lqr_tuning_script.mat b/templates/lqr_tuning_script.mat
new file mode 100644
index 0000000..af9681c
--- /dev/null
+++ b/templates/lqr_tuning_script.mat
Binary files differ