     XCTAssertEqual(graph.storage.count, 3)
     
     // check error at initial estimate, allow slack to account for discrete transition
-    XCTAssertEqual(graph.error(at: variables), 467.0, accuracy:0.3)
+    XCTAssertEqual(graph.error(at: variables), 234, accuracy:0.3)
     
     // optimize
     var opt = LM()

    return (graph, variables)
  }
  
  func printPoses(_ variables : VariableAssignments) {
    print(variables[TypedID<Pose2>(0)])
    print(variables[TypedID<Pose2>(1)])
    print(variables[TypedID<Pose2>(2)])
  }
  
  /// Tracking example from Figure 2.a
  func testTrackingExample() {
    // create a factor graph
    var (graph, variables) = createTrackingFactorGraph()
    _ = graph as FactorGraph
    
    // check number of factor types
    XCTAssertEqual(graph.storage.count, 2)

    /// Proposal to change one label, and re-optimize
    let flipAndOptimize = {(x:VariableAssignments) -> VariableAssignments in
      let labelVars = x.storage[ObjectIdentifier(Int.self)]
      //  let positionVars = x.storage[ObjectIdentifier(Pose2.self)]
      
      // Randomly change one label.
      let i = Int.random(in: 0..<labelVars!.count)
