Update tracking LQR

Author: janbruedigam

src/control/lqr_tracking.jl

@@ -85,13 +85,14 @@ function dlqr(mechanism::Mechanism{T,Nn,Nb,Ne}, xd, vd, qd, ωd, Fτd, eqcids,Q,
     k = 0
     for outer k=N-1:-1:1
         A, Bu, Bλ, G = linearsystem(mechanism, xd[k], vd[k], qd[k], ωd[k], Fτd[k], bodyids, eqcids)
-        M11 = R + Bu'*Pk*Bu
-        M12 = Bu'*Pk*Bλ
+        D = Bu - Bλ/(G*Bλ)*G*Bu
+        M11 = R + D'*Pk*Bu
+        M12 = D'*Pk*Bλ
         M21 = G*Bu
         M22 = G*Bλ
 
         M = [M11 M12;M21 M22]
-        b = [Bu'*Pk;G]*A
+        b = [D'*Pk;G]*A
 
         Kk = M\b