>
Q = zeros(D1); Q([1 D0+1],[1 D0+1]) = [1 ell]'*[1 ell]; Q(D0+2,D0+2) = ell^2;

<span class="comment">% 3. Trigonometric augmentation</span>
<span class="keyword">if</span> D1-D0 &gt; 0
  <span class="comment">% augment target</span>
  target = [cost.target(:); gTrig(cost.target(:), 0*s, cost.angle)];

  <span class="comment">% augment state</span>
  i = 1:D0; k = D0+1:D1;
  [M(k) S(k,k) C mdm sdm Cdm mds sds Cds] = gTrig(M(i),S(i,i),cost.angle);

  <span class="comment">% compute derivatives (for augmenta