Add observations based on latent states

Also output result of matlab implementation, for comparison purposes