Rewrote program to use std::vector instead of armadillo vectors/matrices