Fixed bug with non-scaler measurements in the constrained smoothing.
Comment about weird smoothing results.
Bug for non scalar z variable
Fixes for the param estimation.