Continuacion de adaptacion de mat. a c++

Continuacion de la adaptacion de los calculos del matlab a comandos C++.
Especial atención a los calculos de matrices y a la señal de error que
resulta (consigna) para metersela a los pids.