Use observer state w in estimator.

Fix state calculations in state_estimate function. Add new observer
state w to Sample and use in state_estimate.
Merge remote-tracking branch 'oliverlee/gainschd' into gainschd
Set estimate.w and estimate.has_w.