Bugfix: incorrect loop bound

The upper limit of the loop was incorrect, which led the segfaults from out-of-bounds memory access
Bugfix: matrix Ahat was not initialised correctly

By default, set the entries of the dense matrix Ahat to 0, but set it to the corresponding value in the banded matrix A if the index pair (i,j) is in the correct range. This check for the range was missing before and the code gave incorrect results when the number of vertical layers was increased, it now passes the test