again, please use SGMatrix/SGVector here, otherwise we will run into problems in the modular interfaces.
Please also add a method documentation with @param and @return