pan>
D0 = size(s,2);                           <span class="comment">% state dimension</span>
D1 = D0 + 2*length(cost.angle);           <span class="comment">% state dimension (with sin/cos)</span>

M = zeros(D1,1); M(1:D0) = m; S = zeros(D1); S(1:D0,1:D0) = s;
Mdm = [eye(D0); zeros(D1-D0,D0)]; Sdm = zeros(D1*D1,D0);
Mds = zeros(D1,D0*D0); Sds = kron(Mdm,Mdm);

<span class="comment">% 2. Define static penalty as distance from target setpoint</span>
ell = cost.p; <span class="comment">% pendulum length</span