Add least squares Batch filter

1. Grab mathematics from Born, Tapley, Schultz.
2. Needed for better estimation of a bad initial state
3. Support Gauss Markov stuff like in the book