Full diagonal kl method (the first KL method)

@karlnapf 
take a look at this.
Currently I use the log_det in MatrixOperations.cpp.
 I will use some @lambday's work once other things in the implementation are fine.
the comments here should be *inside* the function rather than outside