Add minimum diagonal value

If no noise is added to certain observations because they are only just positive definite that values later in the decomposition tend towards infinity. I added a minimum allowed value to account for this.