Nonlinear Dynamic Inversion Matlab