Add larger local map matching

[jakemclaughlin6]

Currently, matching against the current sliding window of features doesn't result in many matches.

This could be because we are using the SSC detector or because we aren't detecting enough features. The fix to this would be:

detect and track more features This could also just be because the sliding window isn't big enough to match against. The solution to this would be:

store marginalized landmarks in a seperate map and match against these (same as existing logic and we would store the fuse landmark variables directly) would need to add constraints for fixed landmark values every once in a while, the map would need to be culled so that it isn't ever growing (i.e. remove landmarks that are older than X)