Add output of RS and RV to outputStateMatlab

Add changes to the cpp file. PR #184 included changes in the hpp file only.