The 'home made' tridiagonal matrix solver didn't work, used the built in armadillo routine in stead. Looks promising