Added a file to solve for the K matrix for balance control. Not yet Complete.