How to find controllability matrix