Added a quick code for the main algorithms under test
Some changes in the covariance matrix definition. No big changes
Fixed a bug in the plotting code. Implicit is plotted correctly now.