Classical Kalman Filter to smooth and iterate

The Estimate struct already has everything it needs to support this, so it really just needs to be coded up.