Fix plot_estimates, plot_forward_speed in PlotSample.

Fix displayed fields and legend for plot_estimates. Fix threshold
speeds.
Merge remote-tracking branch 'oliverlee/fullorderobs' into full_order_observer
Fix bug in observer A matrix.

Previous commit had observer A matrix equal to plant A matrix, instead of
A-K_obs*C. This results in a observer that has unstable dynamics since the
plant dynamics are unstable.