%Control for Crane Double Pendulum
clear all;
close all;
global K

A=[ 0    1.0000         0         0         0         0
    0  -58.1558  -13.3099         0         0         0
    0         0         0    1.0000         0         0
    0  -73.7445 -112.7311         0   79.6748         0
    0         0         0         0         0    1.0000
    0   73.7445  112.7311         0 -247.1962         0 ]
B=[0; 9.48888; 0; 46.275; 0; -46.275]
C=[1 0 0 0 0 0 ;
   0 1 0 0 0 0 ;
   0 0 1 0 0 0 ;
   0 0 0 1 0 0 ;
   0 0 0 0 1 0 ;
   0 0 0 0 0 1]
D=[0 0 0 0 0 0 ]'

Q=[1000 0 0   0 0   0;
   0    1 0   0 0   0;
   0    0 100 0 0   0;
   0    0 0   1 0   0;
   0    0 0   0 100 0;
   0    0 0   0 0   1];
R=0.1;
K=lqr(A,B,Q,R);
chap7_11sim;