Fixed an error in how the initial diagonal entries are calculated in the derviative matrix.