MADE:
--> added functions to Matrix to get size.
Merge branch 'master' into EKF
TODO:
--> Virtual members updateJf and updateJh