calf.cpp: calf and ankle joint, no control, swings down below ground.
calf-manual.cpp: above with manual controller, balances, underdamped.
calf-id.cpp: indentify linear dynamics model. x_next = Ax + Bu
calf-lqr.cpp: use LQR gains
leg.cpp: thigh plus calf, no control, swings down below ground.
unipod.cpp: torso, thigh, calf, no control, swings down below ground.

Identifying calf model with different deltas. Wide range of deltas seem
to work.

delta = 1e-7
A = [    1.011319279670715    0.009999999776483 
   1.131923079490662    1.000000000000000 
]
B = [    0.009230767376721
   0.923076808452606
]

delta = 1e-6
A = [    1.011319279670715    0.009999999776483 
   1.131922960281372    1.000000000000000 
]
B = [    0.009230768308043
   0.923076868057251
]

delta = 1e-5
A = [    1.011319160461426    0.009999999776483 
   1.131922960281372    1.000000000000000 
]
B = [    0.009230768308043
   0.923076808452606
]

delta = 1e-4
A = [    1.011319160461426    0.009999999776483 
   1.131923079490662    1.000000000000000 
]
B = [    0.009230768308043
   0.923076868057251
]

delta = 1e-3
A = [    1.011319279670715    0.009999999776483 
   1.131922841072083    1.000000000000000 
]
B = [    0.009230768308043
   0.923076868057251
]

delta = 1e-2
A = [    1.011319041252136    0.009999999776483 
   1.131904006004333    1.000000000000000 
]
B = [    0.009230768308043
   0.923076808452606
]

delta = 1e-1
A = [    1.011300325393677    0.009999999776483 
   1.130041599273682    1.000000119209290 
]
B = [    0.009230767376721
   0.923076868057251
]

delta = 1e-0
A = [    1.009524822235107    0.009999916888773 
   0.952478706836700    0.999999940395355 
]
B = [    0.009230702184141
   0.923076808452606
]

/************** LQR design in Matlab for calf *****************/

>> A = [    1.011319160461426    0.009999999776483
   1.131923079490662    1.000000000000000
]

A =

    1.0113    0.0100
    1.1319    1.0000

>> B = [    0.009230768308043
   0.923076868057251
]

B =

    0.0092
    0.9231

>> Q = [ 1 0 
0 1 ]

Q =

     1     0
     0     1

>> R = [ 1 ]

R =

     1

>> [k,s,e] = dlqr(A,B,Q,R)

k =

    2.2253    0.6515


s =

  163.2561    2.4107
    2.4107    1.7057


e =

    0.4049
    0.9845