KZ Filter Smoothing Algorithm added