Make marginalization use same math as in paper

[cntaylor]

There are a couple of different ways to do marginalization, specifically how to express the "prior" factor from the marginalization. What is in the paper is, I believe, fairly straightforward, but I think I am using an older (and harder to understand) method in the code. It should really be doing the same things as in the paper.

[cntaylor]

Marginalization is working better now, I think, but a couple of odd things:

1. Sliding window now has better real-time performance than the full factor graph. So, there is a small difference between the full factor graph (which did not have a prior in it at all) and the EKF and factor graphs which do all have a prior
2. For a very small sliding window, things are getting strange. You start to see it at 100, but things appear to be unstable at the beginning. I need to track this down before I close this issue.