diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFeeder.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFeeder.java
index 5775af956..807511eec 100644
--- a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFeeder.java
+++ b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFeeder.java
@@ -21,7 +21,7 @@ public class AmpFeeder extends SubsystemBase implements Glassy {
     public AmpFeeder(LoggerFactory parent) {
         LoggerFactory child = parent.child(this);
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 Feedforward100 ff = Feedforward100.makeNeo();
                 PIDConstants pid = new PIDConstants(0, 0, 0);
                 ampDrive = new NeoCANSparkMotor(child, 33, MotorPhase.FORWARD, 40, ff, pid);
diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java
index 1ab9f2c14..91c3c5fb4 100644
--- a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java
+++ b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java
@@ -67,7 +67,7 @@ public AmpPivot(LoggerFactory parent) {
         controller.setIntegratorRange(0, 0.1);
 
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 Feedforward100 ff = Feedforward100.makeArmPivot();
                 PIDConstants pid = new PIDConstants(kOutboardP, kOutboardI, kOutboardD);
                 CANSparkMotor motor = new NeoCANSparkMotor(
diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/intake/Intake.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/intake/Intake.java
index f116823ee..9ffc1a758 100644
--- a/comp/swerve100/src/main/java/org/team100/frc2024/motion/intake/Intake.java
+++ b/comp/swerve100/src/main/java/org/team100/frc2024/motion/intake/Intake.java
@@ -48,7 +48,7 @@ public Intake(LoggerFactory parent, SensorInterface sensors) {
         m_sensors = sensors;
 
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 m_intake = new PWMSparkMax(1);
                 m_centering = new PWMSparkMax(2);
                 superRollers = ServoFactory.limitedNeoVelocityServo(
diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/shooter/DrumShooter.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/shooter/DrumShooter.java
index bfad124ff..18af7d7b6 100644
--- a/comp/swerve100/src/main/java/org/team100/frc2024/motion/shooter/DrumShooter.java
+++ b/comp/swerve100/src/main/java/org/team100/frc2024/motion/shooter/DrumShooter.java
@@ -120,7 +120,7 @@ public DrumShooter(
         Feedforward100 rollerFF = Feedforward100.makeShooterFalcon6();
 
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 Falcon6Motor leftMotor = new Falcon6Motor(
                         leftLogger,
                         leftID,
diff --git a/lib/src/main/java/org/team100/lib/config/Identity.java b/lib/src/main/java/org/team100/lib/config/Identity.java
index 88dda28a3..4eefcaa3b 100644
--- a/lib/src/main/java/org/team100/lib/config/Identity.java
+++ b/lib/src/main/java/org/team100/lib/config/Identity.java
@@ -28,6 +28,7 @@ public enum Identity {
     SWERVE_TWO("0317f285"),
     COMP_BOT("03238232"), 
     SWERVE_ONE("032363AC"), 
+    DISABLED("disabled"), // for mechanisms which don't exist
     BLANK(""), // e.g. test default or simulation
     UNKNOWN(null);
 
diff --git a/lib/src/main/java/org/team100/lib/encoder/AnalogTurningEncoder.java b/lib/src/main/java/org/team100/lib/encoder/AnalogTurningEncoder.java
index e8b3cecaf..f57ff2da7 100644
--- a/lib/src/main/java/org/team100/lib/encoder/AnalogTurningEncoder.java
+++ b/lib/src/main/java/org/team100/lib/encoder/AnalogTurningEncoder.java
@@ -33,9 +33,9 @@ public AnalogTurningEncoder(
         m_input = new AnalogInput(channel);
         m_voltage = Memo.ofDouble(m_input::getVoltage);
         m_rail = Memo.ofDouble(RobotController::getVoltage5V);
-        child.intLogger(Level.TRACE, "channel").log(m_input::getChannel);
         m_log_voltage = child.doubleLogger(Level.TRACE, "voltage");
         m_log_ratio = child.doubleLogger(Level.TRACE, "ratio");
+        child.intLogger(Level.COMP, "channel").log(m_input::getChannel);
     }
 
     @Override
diff --git a/lib/src/main/java/org/team100/lib/encoder/DutyCycleRotaryPositionSensor.java b/lib/src/main/java/org/team100/lib/encoder/DutyCycleRotaryPositionSensor.java
index 94231b1c0..e891f97ef 100644
--- a/lib/src/main/java/org/team100/lib/encoder/DutyCycleRotaryPositionSensor.java
+++ b/lib/src/main/java/org/team100/lib/encoder/DutyCycleRotaryPositionSensor.java
@@ -42,7 +42,7 @@ protected DutyCycleRotaryPositionSensor(
         m_dutyCycle = new DutyCycle(m_digitalInput);
         m_duty = Memo.ofDouble(m_dutyCycle::getOutput);
         m_log_duty = child.doubleLogger(Level.TRACE, "duty cycle");
-        child.intLogger(Level.TRACE, "channel").log(() -> channel);
+        child.intLogger(Level.COMP, "channel").log(() -> channel);
     }
 
     @Override
diff --git a/lib/src/test/java/org/team100/lib/swerve/SwerveUtilTest.java b/lib/src/test/java/org/team100/lib/swerve/SwerveUtilTest.java
index 93faead32..4338fd0b9 100644
--- a/lib/src/test/java/org/team100/lib/swerve/SwerveUtilTest.java
+++ b/lib/src/test/java/org/team100/lib/swerve/SwerveUtilTest.java
@@ -207,4 +207,18 @@ void testMinAccel() {
         double accelLimit = SwerveUtil.minAccel(limits, 0.92);
         assertEquals(0.8, accelLimit, kDelta);
     }
+
+    @Test
+    void simMinAccel() {
+        // simulate full-throttle to see the exponential curve. 
+        // https://docs.google.com/spreadsheets/d/1k-g8_blQP3X1RNtjFQgk1CJXyzzvNLNuUbWhaNLduvw
+        SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest();
+        double v = 0;
+        final double dt = 0.02;
+        for (double t = 0; t < 3; t += dt) {
+            System.out.printf("%5.3f %5.3f\n", t, v);
+            double a = SwerveUtil.minAccel(limits, v);
+            v += dt * a;
+        }
+    }
 }
diff --git a/raspberry_pi/app/pose_estimator/calibrate.py b/raspberry_pi/app/pose_estimator/calibrate.py
index 5db54a3cf..6e34107ad 100644
--- a/raspberry_pi/app/pose_estimator/calibrate.py
+++ b/raspberry_pi/app/pose_estimator/calibrate.py
@@ -114,6 +114,12 @@
 # TODO: if you want to model *blur* then you need more noise here, and maybe more in x than y.
 PX_NOISE = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
 
+# the "batch" mode uses a python CustomFactor that takes multiple landmarks
+# and corresponding pixels; the python stuff is very slow.
+# the "not batch" mode uses a C++ factor, PlanarProjectionFactor,
+# which takes a single landmark at a time (because there's no particular
+# performance advantage to batching in C++)
+USE_BATCH = False
 
 class Calibrate:
     def __init__(self, lag_s: float) -> None:
@@ -262,12 +268,27 @@ def apriltag_for_calibration(
     def apriltag_for_calibration_batch(
         self, landmarks: list[np.ndarray], measured: np.ndarray, t0_us: int
     ) -> None:
-        noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
-        self._new_factors.push_back(
-            apriltag_calibrate_batch.factor(
-                landmarks, measured, noise, X(t0_us), C(0), K(0)
+        """With constant offset and calibration, either uses a python CustomFactor
+        to solve a batch at once, or, uses multiple C++ factors.
+        landmarks: list of 3d points
+        measured: concatenated px measurements
+        TODO: flatten landmarks"""
+        if USE_BATCH:
+            noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
+            self._new_factors.push_back(
+                apriltag_calibrate_batch.factor(
+                    landmarks, measured, noise, X(t0_us), C(0), K(0)
+                )
             )
-        )
+        else:
+            for i, landmark in enumerate(landmarks):
+                px = measured[i * 2 : (i + 1) * 2]
+                noise = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
+                self._new_factors.push_back(
+                    apriltag_calibrate.factor(
+                        landmark, px, noise, X(t0_us), C(0), K(0)
+                    )
+                )   
 
     def keep_calib_hot(self, t0_us: int) -> None:
         """Even if we don't see any targets, remember the
diff --git a/raspberry_pi/app/pose_estimator/estimate.py b/raspberry_pi/app/pose_estimator/estimate.py
index dd778513d..80f337361 100644
--- a/raspberry_pi/app/pose_estimator/estimate.py
+++ b/raspberry_pi/app/pose_estimator/estimate.py
@@ -14,6 +14,7 @@
 from wpimath.geometry import Pose2d, Rotation2d, Translation2d, Twist2d
 
 import app.pose_estimator.factors.apriltag_smooth_batch as apriltag_smooth_batch
+import app.pose_estimator.factors.apriltag_smooth as apriltag_smooth
 import app.pose_estimator.factors.gyro as gyro
 import app.pose_estimator.factors.odometry as odometry
 from app.util.drive_util import DriveUtil
@@ -41,13 +42,20 @@
 # TODO: if you want to model *blur* then you need more noise here, and maybe more in x than y.
 PX_NOISE = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
 
+# the "batch" mode uses a python CustomFactor that takes multiple landmarks
+# and corresponding pixels; the python stuff is very slow.
+# the "not batch" mode uses a C++ factor, PlanarProjectionFactor,
+# which takes a single landmark at a time (because there's no particular
+# performance advantage to batching in C++)
+USE_BATCH = False
+
 
 class Estimate:
-    def __init__(self) -> None:
+    def __init__(self, lag: float) -> None:
         """Initialize the model
         initial module positions are at their origins.
         TODO: some other initial positions?"""
-        self._isam: gtsam.BatchFixedLagSmoother = make_smoother(0.1)
+        self._isam: gtsam.BatchFixedLagSmoother = make_smoother(lag)
         self._result: gtsam.Values = gtsam.Values()
         # between updates we accumulate inputs here
         self._new_factors = gtsam.NonlinearFactorGraph()
@@ -186,16 +194,29 @@ def apriltag_for_smoothing_batch(
         camera_offset: gtsam.Pose3,
         calib: gtsam.Cal3DS2,
     ) -> None:
-        """With constant offset and calibration, solves a batch of landmarks at once.
+        """With constant offset and calibration, either uses a python CustomFactor
+         to solve a batch at once, or, uses multiple C++ factors.
         landmarks: list of 3d points
         measured: concatenated px measurements
         TODO: flatten landmarks"""
-        noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
-        self._new_factors.push_back(
-            apriltag_smooth_batch.factor(
-                landmarks, measured, camera_offset, calib, noise, X(t0_us)
+        if USE_BATCH:
+            noise = noiseModel.Diagonal.Sigmas(
+                np.concatenate([[1, 1] for _ in landmarks])
             )
-        )
+            self._new_factors.push_back(
+                apriltag_smooth_batch.factor(
+                    landmarks, measured, camera_offset, calib, noise, X(t0_us)
+                )
+            )
+        else:
+            for i, landmark in enumerate(landmarks):
+                px = measured[i * 2 : (i + 1) * 2]
+                noise = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
+                self._new_factors.push_back(
+                    apriltag_smooth.factor(
+                        landmark, px, camera_offset, calib, noise, X(t0_us)
+                    )
+                )
 
     def update(self) -> None:
         """Run the solver"""
diff --git a/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py b/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py
index f743fd8a7..6a103362d 100644
--- a/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py
+++ b/raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py
@@ -34,7 +34,7 @@ def h_fn(
     landmark: np.ndarray,
 ) -> Callable[[gtsam.Pose2, gtsam.Pose3, gtsam.Cal3DS2], np.ndarray]:
     """Returns a pixel estimation function for a constant landmark,
-    with variable pose, offset, and calibration. 
+    with variable pose, offset, and calibration.
     The landmark is the field position of a tag corner."""
 
     def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
@@ -43,6 +43,8 @@ def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
         offset_pose = gtsam.Pose3(p0).compose(offset)
         # this is z-forward y-down
         camera_pose = offset_pose.compose(CAM_COORD)
+        print("camera_pose", camera_pose)
+        print("landmark", landmark)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
         return camera.project(landmark)
 
@@ -67,7 +69,7 @@ def h_H(
     return result
 
 
-def factor(
+def factorCustom(
     landmark: np.ndarray,
     measured: np.ndarray,
     model: SharedNoiseModel,
@@ -95,3 +97,28 @@ def error_func(
         gtsam.KeyVector([p0_key, offset_key, calib_key]),  # type:ignore
         error_func,
     )
+
+
+def factorNative(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    model: SharedNoiseModel,
+    p0_key: int,
+    offset_key: int,
+    calib_key: int,
+) -> gtsam.NoiseModelFactor:
+    return gtsam.PlanarSFMFactor(
+        landmark, measured, model, p0_key, offset_key, calib_key
+    )
+
+
+def factor(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    model: SharedNoiseModel,
+    p0_key: int,
+    offset_key: int,
+    calib_key: int,
+) -> gtsam.NoiseModelFactor:
+    return factorNative(landmark, measured, model, p0_key, offset_key, calib_key)
+    # return factorCustom(landmark, measured, model, p0_key, offset_key, calib_key)
diff --git a/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py b/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py
index b6ff81686..4ec9bae2d 100644
--- a/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py
+++ b/raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py
@@ -69,7 +69,7 @@ def h_H(
     return result
 
 
-def factor(
+def factorCustom(
     landmark: np.ndarray,
     measured: np.ndarray,
     offset: gtsam.Pose3,
@@ -93,3 +93,28 @@ def error_func(
         gtsam.KeyVector([p0_key]),  # type:ignore
         error_func,
     )
+
+
+def factorNative(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    offset: gtsam.Pose3,
+    calib: gtsam.Cal3DS2,
+    model: SharedNoiseModel,
+    p0_key: int,
+) -> gtsam.NoiseModelFactor:
+    return gtsam.PlanarProjectionFactor(
+        landmark, measured, offset, calib, model, p0_key
+    )
+
+
+def factor(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    offset: gtsam.Pose3,
+    calib: gtsam.Cal3DS2,
+    model: SharedNoiseModel,
+    p0_key: int,
+) -> gtsam.NoiseModelFactor:
+    return factorNative(landmark, measured, offset, calib, model, p0_key)
+    # return factorCustom(landmark, measured, offset, calib, model, p0_key)
diff --git a/raspberry_pi/app/pose_estimator/nt_estimate.py b/raspberry_pi/app/pose_estimator/nt_estimate.py
index 3747f621a..9c66abf36 100644
--- a/raspberry_pi/app/pose_estimator/nt_estimate.py
+++ b/raspberry_pi/app/pose_estimator/nt_estimate.py
@@ -37,7 +37,7 @@ def __init__(self, field_map: FieldMap, cam: CameraConfig, net: Network) -> None
         self.gyro_receiver = net.get_gyro_receiver("gyro")
         self.pose_sender = net.get_pose_sender("pose")
 
-        self.est = Estimate()
+        self.est = Estimate(0.1)
         # current estimate, used for initial value for next time
         # TODO: remove gtsam types
         self.state = gtsam.Pose2()
@@ -98,7 +98,7 @@ def _receive_prior(self) -> None:
         # TODO: supply sigma on the wire?
         s = noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05]))
 
-        self.est = Estimate()
+        self.est = Estimate(0.1)
         self.state = p
         self.est.init()
         now = self.net.now()
diff --git a/raspberry_pi/tests/pose_estimator/calibrate_simulate_test.py b/raspberry_pi/tests/pose_estimator/calibrate_simulate_test.py
index 73ec54678..35c3a3d0f 100644
--- a/raspberry_pi/tests/pose_estimator/calibrate_simulate_test.py
+++ b/raspberry_pi/tests/pose_estimator/calibrate_simulate_test.py
@@ -147,6 +147,10 @@ def test_camera_batched(self) -> None:
         apart, and/or multiple cameras, or, as below, with odometry.
         It takes a long time to learn the fx and fy values, like >20s.
         but k1 and k2 seem pretty close within 10s.
+        For the 1000-iteration (20s) case with the python batched factor,
+        runtime is about 20s (i.e. 100% realtime, totally not ok).
+        For the c++ not-batched factor the runtime is about 5s (still
+        not great).
         """
         np.set_printoptions(suppress=True, precision=2)
 
@@ -163,18 +167,19 @@ def test_camera_batched(self) -> None:
 
         state = gtsam.Pose2()
 
-        print()
-        print(
-            "     t, "
-            "  GT X,   GT Y,   GT Θ, "
-            " Est X,  Est Y,  Est Θ, "
-            " sig X,  sig Y,  sig Θ, "
-            " Err X,  Err Y,  Err Θ, "
-            "   F_X,    F_Y,    P_X, "
-            "   P_Y,    K_0,    K_1, "
-            "   C_X,    C_Y,    C_Z, "
-            "   C_R,    C_P,    C_Y"
-        )
+        if ACTUALLY_PRINT:
+            print()
+            print(
+                "     t, "
+                "  GT X,   GT Y,   GT Θ, "
+                " Est X,  Est Y,  Est Θ, "
+                " sig X,  sig Y,  sig Θ, "
+                " Err X,  Err Y,  Err Θ, "
+                "   F_X,    F_Y,    P_X, "
+                "   P_Y,    K_0,    K_1, "
+                "   C_X,    C_Y,    C_Z, "
+                "   C_R,    C_P,    C_Y"
+            )
         for i in range(1, 1000):
             t0 = time.time_ns()
             t0_us = 20000 * i
@@ -228,17 +233,18 @@ def test_camera_batched(self) -> None:
 
             f = '6.2f'
 
-            print(
-                f"{t:{f}}, "
-                f"{gt_x:{f}}, {gt_y:{f}}, {gt_theta:{f}}, "
-                f"{est_x:{f}}, {est_y:{f}}, {est_theta:{f}}, "
-                f"{s0[0]:{f}}, {s0[1]:{f}}, {s0[2]:{f}}, "
-                f"{err_x:{f}}, {err_y:{f}}, {err_theta:{f}}, "
-                f"{k0.fx():{f}}, {k0.fy():{f}}, {k0.px():{f}}, "
-                f"{k0.py():{f}}, {k0.k1():{f}}, {k0.k2():{f}}, "
-                f"{c0.x():{f}}, {c0.y():{f}}, {c0.z():{f}}, "
-                f"{c0.rotation().roll():{f}}, {c0.rotation().pitch():{f}}, {c0.rotation().yaw():{f}}"
-            )
+            if ACTUALLY_PRINT:
+                print(
+                    f"{t:{f}}, "
+                    f"{gt_x:{f}}, {gt_y:{f}}, {gt_theta:{f}}, "
+                    f"{est_x:{f}}, {est_y:{f}}, {est_theta:{f}}, "
+                    f"{s0[0]:{f}}, {s0[1]:{f}}, {s0[2]:{f}}, "
+                    f"{err_x:{f}}, {err_y:{f}}, {err_theta:{f}}, "
+                    f"{k0.fx():{f}}, {k0.fy():{f}}, {k0.px():{f}}, "
+                    f"{k0.py():{f}}, {k0.k1():{f}}, {k0.k2():{f}}, "
+                    f"{c0.x():{f}}, {c0.y():{f}}, {c0.z():{f}}, "
+                    f"{c0.rotation().roll():{f}}, {c0.rotation().pitch():{f}}, {c0.rotation().yaw():{f}}"
+                )
 
     def test_batched_camera_and_odometry_and_gyro(self) -> None:
         """Camera and odometry and gyro.
@@ -248,6 +254,9 @@ def test_batched_camera_and_odometry_and_gyro(self) -> None:
         It takes a long time to learn fx and fy, as the model above also does.
         k1 and k2 are pretty close within 2 sec (!)
         The prediction errors are below 1cm and 0.5 deg all the time.
+        for the python batched factor for 20s, the runtime is about 15s
+        for the c++ not-batched factor for 20s, the runtime is about 1.3s
+        (much better)
         """
         sim = CircleSimulator(FieldMap())
         est = Calibrate(0.1)
@@ -262,18 +271,19 @@ def test_batched_camera_and_odometry_and_gyro(self) -> None:
 
         state = gtsam.Pose2()
 
-        print()
-        print(
-            "     t, "
-            "  GT X,   GT Y,   GT Θ, "
-            " Est X,  Est Y,  Est Θ, "
-            " sig X,  sig Y,  sig Θ, "
-            " Err X,  Err Y,  Err Θ, "
-            "   F_X,    F_Y,    P_X, "
-            "   P_Y,    K_0,    K_1, "
-            "   C_X,    C_Y,    C_Z, "
-            "   C_R,    C_P,    C_Y"
-        )
+        if ACTUALLY_PRINT:
+            print()
+            print(
+                "     t, "
+                "  GT X,   GT Y,   GT Θ, "
+                " Est X,  Est Y,  Est Θ, "
+                " sig X,  sig Y,  sig Θ, "
+                " Err X,  Err Y,  Err Θ, "
+                "   F_X,    F_Y,    P_X, "
+                "   P_Y,    K_0,    K_1, "
+                "   C_X,    C_Y,    C_Z, "
+                "   C_R,    C_P,    C_Y"
+            )
         for i in range(1, 1000):
             t0 = time.time_ns()
             t0_us = 20000 * i
@@ -322,14 +332,15 @@ def test_batched_camera_and_odometry_and_gyro(self) -> None:
 
             f = '6.2f'
 
-            print(
-                f"{t:{f}}, "
-                f"{gt_x:{f}}, {gt_y:{f}}, {gt_theta:{f}}, "
-                f"{est_x:{f}}, {est_y:{f}}, {est_theta:{f}}, "
-                f"{s0[0]:{f}}, {s0[1]:{f}}, {s0[2]:{f}}, "
-                f"{err_x:{f}}, {err_y:{f}}, {err_theta:{f}}, "
-                f"{k0.fx():{f}}, {k0.fy():{f}}, {k0.px():{f}}, "
-                f"{k0.py():{f}}, {k0.k1():{f}}, {k0.k2():{f}}, "
-                f"{c0.x():{f}}, {c0.y():{f}}, {c0.z():{f}}, "
-                f"{c0.rotation().roll():{f}}, {c0.rotation().pitch():{f}}, {c0.rotation().yaw():{f}}"
-            )
+            if ACTUALLY_PRINT:
+                print(
+                    f"{t:{f}}, "
+                    f"{gt_x:{f}}, {gt_y:{f}}, {gt_theta:{f}}, "
+                    f"{est_x:{f}}, {est_y:{f}}, {est_theta:{f}}, "
+                    f"{s0[0]:{f}}, {s0[1]:{f}}, {s0[2]:{f}}, "
+                    f"{err_x:{f}}, {err_y:{f}}, {err_theta:{f}}, "
+                    f"{k0.fx():{f}}, {k0.fy():{f}}, {k0.px():{f}}, "
+                    f"{k0.py():{f}}, {k0.k1():{f}}, {k0.k2():{f}}, "
+                    f"{c0.x():{f}}, {c0.y():{f}}, {c0.z():{f}}, "
+                    f"{c0.rotation().roll():{f}}, {c0.rotation().pitch():{f}}, {c0.rotation().yaw():{f}}"
+                )
diff --git a/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py b/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py
index 9763d4b22..cbde24fb6 100644
--- a/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py
+++ b/raspberry_pi/tests/pose_estimator/estimate_apriltag_batch_test.py
@@ -19,7 +19,7 @@ class EstimateAprilTagTest(unittest.TestCase):
 
     def test_apriltag_1(self) -> None:
         """Test the smoothing factor."""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
diff --git a/raspberry_pi/tests/pose_estimator/estimate_gyro_test.py b/raspberry_pi/tests/pose_estimator/estimate_gyro_test.py
index 5db8538b6..b14f414bb 100644
--- a/raspberry_pi/tests/pose_estimator/estimate_gyro_test.py
+++ b/raspberry_pi/tests/pose_estimator/estimate_gyro_test.py
@@ -17,7 +17,7 @@
 class EstimateGyroTest(unittest.TestCase):
     def test_gyro_0(self) -> None:
         """motionless"""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
@@ -63,7 +63,7 @@ def test_gyro_0(self) -> None:
 
     def test_gyro_1(self) -> None:
         """rotating"""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
diff --git a/raspberry_pi/tests/pose_estimator/estimate_odometry_test.py b/raspberry_pi/tests/pose_estimator/estimate_odometry_test.py
index 01510d3d9..70ba5174b 100644
--- a/raspberry_pi/tests/pose_estimator/estimate_odometry_test.py
+++ b/raspberry_pi/tests/pose_estimator/estimate_odometry_test.py
@@ -26,7 +26,7 @@
 class EstimateOdometryTest(unittest.TestCase):
     def test_odometry_0(self) -> None:
         # initial position at origin
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         # std dev of 1 cm and 0.5 deg
@@ -131,7 +131,7 @@ def test_odometry_0(self) -> None:
     def test_odometry_1(self) -> None:
         """combined rotation and translation, no wheel slip, should yield exact solution."""
         # initial position at origin, 1m wheelbase
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         odometry_noise = noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.01]))
@@ -246,7 +246,7 @@ def test_odometry_1(self) -> None:
     def test_odometry_2(self) -> None:
         """add some wheel slip to the above case."""
         # initial position at origin, 1m wheelbase
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
diff --git a/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py b/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py
index 70a7f1dac..05429a983 100644
--- a/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py
+++ b/raspberry_pi/tests/pose_estimator/estimate_simulate_test.py
@@ -24,7 +24,7 @@ def test_odo_only(self) -> None:
         This is very fast, 0.1s on my machine for a 1s window,
         0.03s for a 0.1s window"""
         sim = CircleSimulator(FieldMap())
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         # sim starts at (2,0)
@@ -80,7 +80,7 @@ def test_odo_only(self) -> None:
     def test_odo_and_gyro(self) -> None:
 
         sim = CircleSimulator(FieldMap())
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         # sim starts at (2,0)
@@ -136,9 +136,13 @@ def test_odo_and_gyro(self) -> None:
 
     def test_camera_batched(self) -> None:
         """Camera with batch factors.
-        this is much faster than multiple factors, 0.4s for a 0.1s window"""
+        using 0.1s window
+        using 2s of data
+        Python: 0.3s (15% of realtime)
+        C++: 0.04s (2% of realtime)
+        """
         sim = CircleSimulator(FieldMap())
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
@@ -147,10 +151,11 @@ def test_camera_batched(self) -> None:
 
         state = gtsam.Pose2()
 
-        print()
-        print(
-            "      t,    GT X,    GT Y,  GT ROT,   EST X,   EST Y, EST ROT,   ERR X,   ERR Y, ERR ROT"
-        )
+        if ACTUALLY_PRINT:
+            print()
+            print(
+                "      t,    GT X,    GT Y,  GT ROT,   EST X,   EST Y, EST ROT,   ERR X,   ERR Y, ERR ROT"
+            )
         for i in range(1, 100):
             t0 = time.time_ns()
             t0_us = 20000 * i
@@ -189,18 +194,21 @@ def test_camera_batched(self) -> None:
             err_x = est_x - gt_x
             err_y = est_y - gt_y
             err_theta = est_theta - gt_theta
+            if ACTUALLY_PRINT:
 
-            print(
-                f"{t:7.4f}, {gt_x:7.4f}, {gt_y:7.4f}, {gt_theta:7.4f}, {est_x:7.4f}, {est_y:7.4f}, {est_theta:7.4f}, {err_x:7.4f}, {err_y:7.4f}, {err_theta:7.4f}"
-            )
+                print(
+                    f"{t:7.4f}, {gt_x:7.4f}, {gt_y:7.4f}, {gt_theta:7.4f}, {est_x:7.4f}, {est_y:7.4f}, {est_theta:7.4f}, {err_x:7.4f}, {err_y:7.4f}, {err_theta:7.4f}"
+                )
 
     def test_batched_camera_and_odometry_and_gyro(self) -> None:
         """Camera and odometry and gyro.
-        Batching vision speeds things up significantly,
-        0.6s for 2s of data in a 0.1s window
+        using 0.1s window
+        using 2s of data
+        Batching python: 0.33s (i.e. 16% of realtime)
+        Non-batching c++: 0.07s (i.e. 4% of realtime)
         """
         sim = CircleSimulator(FieldMap())
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
@@ -213,10 +221,11 @@ def test_batched_camera_and_odometry_and_gyro(self) -> None:
 
         state = gtsam.Pose2()
 
-        print()
-        print(
-            "      t,    GT X,    GT Y,  GT ROT,   EST X,   EST Y, EST ROT,   ERR X,   ERR Y, ERR ROT"
-        )
+        if ACTUALLY_PRINT:
+            print()
+            print(
+                "      t,    GT X,    GT Y,  GT ROT,   EST X,   EST Y, EST ROT,   ERR X,   ERR Y, ERR ROT"
+            )
         # gt_theta_0 = sim.gt_theta
         for i in range(1, 100):
             t0 = time.time_ns()
@@ -260,6 +269,7 @@ def test_batched_camera_and_odometry_and_gyro(self) -> None:
             err_y = est_y - gt_y
             err_theta = est_theta - gt_theta
 
-            print(
-                f"{t:7.4f}, {gt_x:7.4f}, {gt_y:7.4f}, {gt_theta:7.4f}, {est_x:7.4f}, {est_y:7.4f}, {est_theta:7.4f}, {err_x:7.4f}, {err_y:7.4f}, {err_theta:7.4f}"
-            )
+            if ACTUALLY_PRINT:
+                print(
+                    f"{t:7.4f}, {gt_x:7.4f}, {gt_y:7.4f}, {gt_theta:7.4f}, {est_x:7.4f}, {est_y:7.4f}, {est_theta:7.4f}, {err_x:7.4f}, {err_y:7.4f}, {err_theta:7.4f}"
+                )
diff --git a/raspberry_pi/tests/pose_estimator/estimate_test.py b/raspberry_pi/tests/pose_estimator/estimate_test.py
index 2ebc2c7fd..89a225bdd 100644
--- a/raspberry_pi/tests/pose_estimator/estimate_test.py
+++ b/raspberry_pi/tests/pose_estimator/estimate_test.py
@@ -24,7 +24,7 @@
 class EstimateAccelerometerTest(unittest.TestCase):
     def test_no_factors(self) -> None:
         """Nothing bad happens if you don't have any factors."""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
@@ -76,7 +76,7 @@ def test_no_factors(self) -> None:
 
     def test_result(self) -> None:
         """Just one state and one prior factor."""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         est.add_state(0, gtsam.Pose2(0, 0, 0))
@@ -118,7 +118,7 @@ def test_result(self) -> None:
 
     def test_multiple_priors(self) -> None:
         """One state but two different priors: the tighter prior "wins"."""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         est.add_state(0, gtsam.Pose2(0, 0, 0))
@@ -169,7 +169,7 @@ def test_multiple_priors(self) -> None:
         )
 
     def test_marginals(self) -> None:
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_mean = gtsam.Pose2(0, 0, 0)
@@ -198,7 +198,7 @@ def test_marginals(self) -> None:
 
     def test_joint_marginals(self) -> None:
         """OK we're not actually going to use this."""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_noise = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1]))
@@ -240,7 +240,7 @@ def test_joint_marginals(self) -> None:
 
     def test_extrapolate(self) -> None:
         """extrapolate the most-recent odometry."""
-        est = Estimate()
+        est = Estimate(0.1)
         est.init()
 
         prior_noise = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1]))
diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py
index 198eb4c1a..e1f8c3e3e 100644
--- a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py
+++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_batch_test.py
@@ -158,9 +158,18 @@ def test_H_upper_left(self) -> None:
         )
 
     def test_factor(self) -> None:
-        # same as above
-        measured = np.array([200, 200, 0, 0])
-        landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
+        landmarks = [
+            np.array([1, 0, 0]),  # on bore, 1m away (in x)
+            np.array([1, 1, 1]),  # upper left corner
+        ]
+        measured = np.array(
+            [
+                200,  # matches (cx,cy)
+                200,
+                0,  # this happens to end up in the corner of the frame :-)
+                0,
+            ]
+        )
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
         f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
@@ -176,7 +185,6 @@ def test_factor(self) -> None:
         v.insert(C(0), offset)
         v.insert(K(0), KCAL)
         err_px = f.unwhitenedError(v)
-        # same case as above
         print("err: ", err_px)
         self.assertTrue(
             np.allclose(
@@ -191,3 +199,130 @@ def test_factor(self) -> None:
                 err_px,
             )
         )
+
+    def test_factor2(self) -> None:
+        landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
+        # camera offset up, so measured points
+        # should be down
+        measured = np.array(
+            [
+                200,  # center
+                380,  # lower
+                20,  # distorted
+                200,  # center
+            ]
+        )
+        p0 = gtsam.Pose2()
+        offset = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 1]))
+        f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
+            landmarks,
+            measured,
+            NOISE2,
+            X(0),
+            C(0),
+            K(0),
+        )
+        v = gtsam.Values()
+        v.insert(X(0), p0)
+        v.insert(C(0), offset)
+        v.insert(K(0), KCAL)
+        err_px = f.unwhitenedError(v)
+        print("err: ", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+            )
+        )
+
+    def test_factor3(self) -> None:
+        landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
+        # camera tilt up, so measured points
+        # should be down
+        measured = np.array(
+            [
+                200,  # center
+                220,  # bore is slightly above target
+                31,  # distorted
+                49,
+            ]
+        )
+        p0 = gtsam.Pose2()
+        offset = gtsam.Pose3(gtsam.Rot3.Pitch(-0.1), np.array([0, 0, 0]))
+        f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
+            landmarks,
+            measured,
+            NOISE2,
+            X(0),
+            C(0),
+            K(0),
+        )
+        v = gtsam.Values()
+        v.insert(X(0), p0)
+        v.insert(C(0), offset)
+        v.insert(K(0), KCAL)
+        err_px = f.unwhitenedError(v)
+        print("err: ", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+                atol=1,
+            )
+        )
+
+    def test_factor4(self) -> None:
+        landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
+        # closer, so points are closer to the edges
+        measured = np.array(
+            [
+                200,  # center
+                390,  # closer to the edge
+                10,  # closer to the edge
+                200,  # center
+            ]
+        )
+        p0 = gtsam.Pose2(0.05, 0, 0)
+        offset = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 1]))
+        f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
+            landmarks,
+            measured,
+            NOISE2,
+            X(0),
+            C(0),
+            K(0),
+        )
+        v = gtsam.Values()
+        v.insert(X(0), p0)
+        v.insert(C(0), offset)
+        v.insert(K(0), KCAL)
+        err_px = f.unwhitenedError(v)
+        print("err: ", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+                atol=1,
+            )
+        )
diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py
index 2138f44c8..51b43ae1f 100644
--- a/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py
+++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_calibrate_test.py
@@ -8,11 +8,10 @@
 from gtsam import noiseModel  # type:ignore
 from gtsam.symbol_shorthand import C, K, X  # type:ignore
 
-import app.pose_estimator.factors.apriltag_calibrate as apriltag_calibrate_batch
+import app.pose_estimator.factors.apriltag_calibrate as apriltag_calibrate
 
 # example calibration, focal length 200, pixel center 200, a little bit
 # of distortion.
-KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
 NOISE2 = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
 
 
@@ -21,7 +20,10 @@ def test_h_center_1(self) -> None:
         landmark = np.array([1, 0, 0])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
-        estimate_px: np.ndarray = apriltag_calibrate_batch.h_fn(landmark)(p0, offset, KCAL)
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
+        estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)(
+            p0, offset, KCAL
+        )
         # landmark on the camera bore, so it's at (cx, cy)
         self.assertTrue(
             np.allclose(
@@ -40,7 +42,10 @@ def test_h_side_0(self) -> None:
         landmark = np.array([1, 0, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
-        estimate_px: np.ndarray = apriltag_calibrate_batch.h_fn(landmark)(p0, offset, KCAL)
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
+        estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)(
+            p0, offset, KCAL
+        )
         # landmark above the camera bore, so the 'y' value is less
         self.assertTrue(
             np.allclose(
@@ -59,7 +64,10 @@ def test_h_upper_left(self) -> None:
         landmark = np.array([1, 1, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
-        estimate_px: np.ndarray = apriltag_calibrate_batch.h_fn(landmark)(p0, offset, KCAL)
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
+        estimate_px: np.ndarray = apriltag_calibrate.h_fn(landmark)(
+            p0, offset, KCAL
+        )
         # above and to the left, so both x and y are less.
         # (coincidentally right on the edge)
         print("estimate: ", estimate_px)
@@ -81,8 +89,9 @@ def test_H_upper_left(self) -> None:
         landmark = np.array([1, 1, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
         H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))]
-        err_px: np.ndarray = apriltag_calibrate_batch.h_H(
+        err_px: np.ndarray = apriltag_calibrate.h_H(
             landmark, measured, p0, offset, KCAL, H
         )
         # same case as above
@@ -137,13 +146,144 @@ def test_H_upper_left(self) -> None:
             )
         )
 
+    def test_H_upper_left2(self) -> None:
+        # as above but with jacobians
+        measured = np.array([0, 0])
+        landmark = np.array([1, 1, 1])
+        p0 = gtsam.Pose2()
+        offset = gtsam.Pose3()
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, 0, 0)
+        H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))]
+        err_px: np.ndarray = apriltag_calibrate.h_H(
+            landmark, measured, p0, offset, KCAL, H
+        )
+        # same case as above
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+            )
+        )
+        print("H[0]:\n", H[0])
+        print("H[1]:\n", H[1])
+        print("H[2]:\n", H[2])
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [-200, 200, 400],
+                        [-200, 0, 200],
+                    ]
+                ),
+                H[0],
+                atol=0.001,
+            )
+        )
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [-200, -200, 400, -200, 200, 0],
+                        [200, -400, 200, -200, 0, 200],
+                    ]
+                ),
+                H[1],
+                atol=0.001,
+            )
+        )
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [-1, 0, -1, 1, 0, -400, -800, 400, 800],
+                        [0, -1, 0, 0, 1, -400, -800, 800, 400],
+                    ]
+                ),
+                H[2],
+                atol=0.001,
+            )
+        )
+
+    def test_H_upper_left3(self) -> None:
+        # offset rotates the center near the landmark
+        measured = np.array([238, 278])
+        landmark = np.array([1, 1, 1])
+        p0 = gtsam.Pose2()
+        # Remember Ypr is intrinsic
+        offset = gtsam.Pose3(gtsam.Rot3().Ypr(1, -1, 0), np.array([0, 0, 0]))
+        print("OFFSET", offset)
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, 0, 0)
+        H = [np.zeros((3, 2)), np.zeros((6, 2)), np.zeros((9, 2))]
+        err_px: np.ndarray = apriltag_calibrate.h_H(
+            landmark, measured, p0, offset, KCAL, H
+        )
+        # same case as above
+        print("err_px", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+                atol=1
+            )
+        )
+        print("H[0]:\n", H[0])
+        print("H[1]:\n", H[1])
+        print("H[2]:\n", H[2])
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [-99, 79, 178],
+                        [-43, -67, -24],
+                    ]
+                ),
+                H[0],
+                atol=1,
+            )
+        )
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [78, -14, 207, 24, 126, 0],
+                        [-38, -230, 15, 49, 0, 126],
+                    ]
+                ),
+                H[1],
+                atol=1,
+            )
+        )
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [0.190, 0, 0.392, 1, 0, 7.191, 1.363, 29.732, 52.303],
+                        [0, 0.392, 0, 0, 1, 14.861, 2.817, 99.364, 29.732],
+                    ]
+                ),
+                H[2],
+                atol=0.001,
+            )
+        )
+
     def test_factor(self) -> None:
         # same as above
         measured = np.array([0, 0])
         landmark = np.array([1, 1, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
-        f: gtsam.NoiseModelFactor = apriltag_calibrate_batch.factor(
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
+        f: gtsam.NoiseModelFactor = apriltag_calibrate.factor(
             landmark,
             measured,
             NOISE2,
diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py
index 45d2ec309..705f86d61 100644
--- a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py
+++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_batch_test.py
@@ -129,8 +129,10 @@ def test_H_upper_left(self) -> None:
         )
 
     def test_factor(self) -> None:
-        # same case as above
-        landmarks = [np.array([1, 0, 0]), np.array([1, 1, 1])]
+        landmarks = [
+            np.array([1, 0, 0]),  # on bore, 1m away (in x)
+            np.array([1, 1, 1]),  # upper left corner
+        ]
         measured = np.array([200, 200, 0, 0])
         offset = gtsam.Pose3()
         f: gtsam.NoiseModelFactor = apriltag_smooth_batch.factor(
@@ -145,7 +147,6 @@ def test_factor(self) -> None:
         p0 = gtsam.Pose2()
         v.insert(X(0), p0)
         err_px = f.unwhitenedError(v)
-        # same case as above
         print("err: ", err_px)
         self.assertTrue(
             np.allclose(
@@ -160,3 +161,133 @@ def test_factor(self) -> None:
                 err_px,
             )
         )
+
+    def test_factor2(self) -> None:
+        landmarks = [
+            np.array([1, 0, 0]),  # on bore, 1m away (in x)
+            np.array([1, 1, 1]),  # upper left corner
+        ]
+        # camera offset up, so measured points
+        # should be down
+        measured = np.array(
+            [
+                200,  # center
+                380,  # lower
+                20,  # distorted
+                200,  # center
+            ]
+        )
+        offset = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 1]))
+        f: gtsam.NoiseModelFactor = apriltag_smooth_batch.factor(
+            landmarks,
+            measured,
+            offset,
+            KCAL,
+            NOISE2,
+            X(0),
+        )
+        v = gtsam.Values()
+        p0 = gtsam.Pose2()
+        v.insert(X(0), p0)
+        err_px = f.unwhitenedError(v)
+        print("err: ", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+            )
+        )
+
+    def test_factor3(self) -> None:
+        landmarks = [
+            np.array([1, 0, 0]),  # on bore, 1m away (in x)
+            np.array([1, 1, 1]),  # upper left corner
+        ]
+        # camera tilt up, so measured points
+        # should be down
+        measured = np.array(
+            [
+                200,  # center
+                220,  # bore is slightly above target
+                31,  # distorted
+                49,
+            ]
+        )
+        offset = gtsam.Pose3(gtsam.Rot3.Pitch(-0.1), np.array([0, 0, 0]))
+        f: gtsam.NoiseModelFactor = apriltag_smooth_batch.factor(
+            landmarks,
+            measured,
+            offset,
+            KCAL,
+            NOISE2,
+            X(0),
+        )
+        v = gtsam.Values()
+        p0 = gtsam.Pose2()
+        v.insert(X(0), p0)
+        err_px = f.unwhitenedError(v)
+        print("err: ", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+                atol=1,
+            )
+        )
+
+    def test_factor4(self) -> None:
+        landmarks = [
+            np.array([1, 0, 0]),  # on bore, 1m away (in x)
+            np.array([1, 1, 1]),  # upper left corner
+        ]
+        # closer, so points are closer to the edges
+        measured = np.array(
+            [
+                200,  # center
+                390,  # closer to the edge
+                10,  # closer to the edge
+                200,  # center
+            ]
+        )
+        offset = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 1]))
+        f: gtsam.NoiseModelFactor = apriltag_smooth_batch.factor(
+            landmarks,
+            measured,
+            offset,
+            KCAL,
+            NOISE2,
+            X(0),
+        )
+        v = gtsam.Values()
+        p0 = gtsam.Pose2(0.05, 0, 0)
+        v.insert(X(0), p0)
+        err_px = f.unwhitenedError(v)
+        print("err: ", err_px)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+                atol=1,
+            )
+        )
diff --git a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py
index c872cdfcc..0fb003484 100644
--- a/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py
+++ b/raspberry_pi/tests/pose_estimator/factors/apriltag_smooth_test.py
@@ -10,15 +10,12 @@
 
 import app.pose_estimator.factors.apriltag_smooth as apriltag_smooth
 
-KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
-NOISE2 = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
-
-
 class AprilTagSmoothTest(unittest.TestCase):
     def test_h_center_1(self) -> None:
         landmark = np.array([1, 0, 0])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
         estimate_px: np.ndarray = apriltag_smooth.h_fn(landmark, offset, KCAL)(p0)
         # landmark on the camera bore, so it's at (cx, cy)
         self.assertTrue(
@@ -38,6 +35,7 @@ def test_h_side_0(self) -> None:
         landmark = np.array([1, 0, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
         estimate_px: np.ndarray = apriltag_smooth.h_fn(landmark, offset, KCAL)(p0)
         # landmark above the camera bore, so the 'y' value is less
         self.assertTrue(
@@ -57,6 +55,7 @@ def test_h_upper_left(self) -> None:
         landmark = np.array([1, 1, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
         estimate_px: np.ndarray = apriltag_smooth.h_fn(landmark, offset, KCAL)(p0)
         # above and to the left, so both x and y are less.
         # (coincidentally right on the edge)
@@ -74,11 +73,14 @@ def test_h_upper_left(self) -> None:
         )
 
     def test_H_center(self) -> None:
+        # see gtsam/slam/tests/testPlanarProjectionFactor.h
         # as above but with jacobians
-        measured = np.array([200, 200])
         landmark = np.array([1, 0, 0])
-        p0 = gtsam.Pose2()
+        measured = np.array([200, 200])
         offset = gtsam.Pose3()
+        p0 = gtsam.Pose2()
+        # target on bore, distortion has no impact.
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
         H = [np.zeros((3, 2))]
         err_px: np.ndarray = apriltag_smooth.h_H(
             landmark, measured, p0, offset, KCAL, H
@@ -97,6 +99,10 @@ def test_H_center(self) -> None:
             )
         )
         print("H[0]:\n", H[0])
+        # du/dx = 0 (target on bore)
+        # du/dy = f (truck, note inverse coord)
+        # du/dtheta = f (pan, note inverse coord)
+        # dv/dx = dv/dy = dv/dtheta = 0
         self.assertTrue(
             np.allclose(
                 np.array(
@@ -111,11 +117,15 @@ def test_H_center(self) -> None:
         )
 
     def test_H_upper_left(self) -> None:
+        # see gtsam/slam/tests/testPlanarProjectionFactor.h
         # as above but with jacobians
-        measured = np.array([0, 0])
         landmark = np.array([1, 1, 1])
-        p0 = gtsam.Pose2()
+        measured = np.array([0, 0])
         offset = gtsam.Pose3()
+        p0 = gtsam.Pose2()
+        # distortion
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
+
         H = [np.zeros((3, 2))]
         err_px: np.ndarray = apriltag_smooth.h_H(
             landmark, measured, p0, offset, KCAL, H
@@ -133,6 +143,12 @@ def test_H_upper_left(self) -> None:
             )
         )
         print("H[0]:\n", H[0])
+        # du/dx = - (dolly fwd => px goes left)
+        # du/dy = + (as above)
+        # du/dtheta = + (as above)
+        # dv/dx = - (dolly fwd => px goes up)
+        # dv/dy = +
+        # dv/dtheta = +
         self.assertTrue(
             np.allclose(
                 np.array(
@@ -146,18 +162,66 @@ def test_H_upper_left(self) -> None:
             )
         )
 
+    def test_H_upper_left2(self) -> None:
+        # see gtsam/slam/tests/testPlanarProjectionFactor.h
+        # as above but with jacobians
+        landmark = np.array([1, 1, 1])
+        measured = np.array([0, 0])
+        offset = gtsam.Pose3()
+        p0 = gtsam.Pose2()
+        # no distortion
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, 0, 0)
+
+        H = [np.zeros((3, 2))]
+        err_px: np.ndarray = apriltag_smooth.h_H(
+            landmark, measured, p0, offset, KCAL, H
+        )
+        # same case as above
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        0,
+                        0,
+                    ]
+                ),
+                err_px,
+            )
+        )
+        print("H[0]:\n", H[0])
+        # du/dx = - (dolly fwd => px goes left)
+        # du/dy = + (as above)
+        # du/dtheta = + (as above)
+        # dv/dx = - (dolly fwd => px goes up)
+        # dv/dy = 0 (just truck)
+        # dv/dtheta = + 
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [-200, 200, 400],
+                        [-200, 0, 200],
+                    ]
+                ),
+                H[0],
+                atol=0.001,
+            )
+        )
+
     def test_factor(self) -> None:
         # higher than the camera
         measured = np.array([0, 0])
         landmark = np.array([1, 1, 1])
         p0 = gtsam.Pose2()
         offset = gtsam.Pose3()
+        KCAL = gtsam.Cal3DS2(200.0, 200.0, 0.0, 200.0, 200.0, -0.2, 0.1)
+        noise = noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
         f: gtsam.NoiseModelFactor = apriltag_smooth.factor(
             landmark,
             measured,
             offset,
             KCAL,
-            NOISE2,
+            noise,
             X(0),
         )
         v = gtsam.Values()
diff --git a/raspberry_pi/tests/pose_estimator/util_test.py b/raspberry_pi/tests/pose_estimator/util_test.py
index 279086c73..3b0243a5e 100644
--- a/raspberry_pi/tests/pose_estimator/util_test.py
+++ b/raspberry_pi/tests/pose_estimator/util_test.py
@@ -1,4 +1,4 @@
-# pylint: disable=E1101
+# pylint: disable=C0103,E1101
 
 
 import math
@@ -96,3 +96,340 @@ def test_cal(self) -> None:
         self.assertAlmostEqual(2, wc.k2)
         # self.assertAlmostEqual(3, wc.p1)
         # self.assertAlmostEqual(4, wc.p2)
+
+    def test_compose_h(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose two identities
+        p0 = gtsam.Pose3()
+        p1 = gtsam.Pose3()
+        H1 = np.zeros((6, 6), order="F")
+        H2 = np.zeros((6, 6), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0, 0, 0, 0],
+                        [0, 1, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0, 1, 0, 0],
+                        [0, 0, 0, 0, 1, 0],
+                        [0, 0, 0, 0, 0, 1],
+                    ]
+                ),
+                H1,
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0, 0, 0, 0],
+                        [0, 1, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0, 1, 0, 0],
+                        [0, 0, 0, 0, 1, 0],
+                        [0, 0, 0, 0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+    def test_compose_h2(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose identity with pure translation
+        p0 = gtsam.Pose3()
+        p1 = gtsam.Pose3(gtsam.Rot3(), np.array([1, 0, 0]))
+        H1 = np.zeros((6, 6), order="F")
+        H2 = np.zeros((6, 6), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0, 0, 0, 0],
+                        [0, 1, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0, 1, 0, 0],
+                        [0, 0, 1, 0, 1, 0],
+                        [0, -1, 0, 0, 0, 1],
+                    ]
+                ),
+                H1,
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0, 0, 0, 0],
+                        [0, 1, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0, 1, 0, 0],
+                        [0, 0, 0, 0, 1, 0],
+                        [0, 0, 0, 0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+    def test_compose_h3(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose identity with pure rotation
+        p0 = gtsam.Pose3()
+        p1 = gtsam.Pose3(gtsam.Rot3().Yaw(1), np.array([0, 0, 0]))
+        H1 = np.zeros((6, 6), order="F")
+        H2 = np.zeros((6, 6), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [0.540, 0.841, 0.000, 0.000, 0.000, 0.000],
+                        [-0.841, 0.540, 0.000, 0.000, 0.000, 0.000],
+                        [0.000, 0.000, 1.000, 0.000, 0.000, 0.000],
+                        [0.000, 0.000, 0.000, 0.540, 0.841, 0.000],
+                        [0.000, 0.000, 0.000, -0.841, 0.540, 0.000],
+                        [0.000, 0.000, 0.000, 0.000, 0.000, 1.000],
+                    ]
+                ),
+                H1,
+                atol=0.001,
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0, 0, 0, 0],
+                        [0, 1, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0, 1, 0, 0],
+                        [0, 0, 0, 0, 1, 0],
+                        [0, 0, 0, 0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+    def test_compose_h4(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose identity with translation + rotation
+        p0 = gtsam.Pose3()
+        p1 = gtsam.Pose3(gtsam.Rot3().Yaw(1), np.array([1, 0, 0]))
+        H1 = np.zeros((6, 6), order="F")
+        H2 = np.zeros((6, 6), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [0.540, 0.841, 0, 0, 0, 0],
+                        [-0.841, 0.540, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0.841, 0.540, 0.841, 0],
+                        [0, 0, 0.540, -0.841, 0.540, 0],
+                        [0, -1, 0, 0, 0, 1],
+                    ]
+                ),
+                H1,
+                atol=0.001
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0, 0, 0, 0],
+                        [0, 1, 0, 0, 0, 0],
+                        [0, 0, 1, 0, 0, 0],
+                        [0, 0, 0, 1, 0, 0],
+                        [0, 0, 0, 0, 1, 0],
+                        [0, 0, 0, 0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+
+    def test_compose_2h(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose two identities
+        p0 = gtsam.Pose2()
+        p1 = gtsam.Pose2()
+        H1 = np.zeros((3, 3), order="F")
+        H2 = np.zeros((3, 3), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0],
+                        [0, 1, 0],
+                        [0, 0, 1],
+
+                    ]
+                ),
+                H1,
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0],
+                        [0, 1, 0],
+                        [0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+    def test_compose_2h2(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose identity with pure translation
+        p0 = gtsam.Pose2()
+        p1 = gtsam.Pose2(1, 0, 0)
+        H1 = np.zeros((3, 3), order="F")
+        H2 = np.zeros((3, 3), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        # since we moved +x, dtheta produces dy.
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0],
+                        [0, 1, 1],
+                        [0, 0, 1],
+                    ]
+                ),
+                H1,
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0],
+                        [0, 1, 0],
+                        [0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+    def test_compose_2h3(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose identity with pure rotation
+        p0 = gtsam.Pose2()
+        p1 = gtsam.Pose2(0, 0, 1)
+        H1 = np.zeros((3, 3), order="F")
+        H2 = np.zeros((3, 3), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        # pure rotation
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [0.540, 0.841, 0],
+                        [-0.841, 0.540, 0],
+                        [0, 0, 1],
+                    ]
+                ),
+                H1,
+                atol=0.001,
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0],
+                        [0, 1, 0],
+                        [0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
+
+    def test_compose_2h4(self) -> None:
+        """Figure out what the composition Jacobians do"""
+        # compose identity with translation + rotation
+        p0 = gtsam.Pose2()
+        p1 = gtsam.Pose2(1, 0, 1)
+        H1 = np.zeros((3, 3), order="F")
+        H2 = np.zeros((3, 3), order="F")
+        p0.compose(p1, H1, H2)
+
+        # adjoint of the inverse
+        # same theta-y coupling but since we rotated a bit, it's not just y.
+        print("H1", H1)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [0.540, 0.841, 0.841],
+                        [-0.841, 0.540, 0.540],
+                        [0, 0, 1],
+                    ]
+                ),
+                H1,
+                atol=0.001
+            )
+        )
+        # always identity
+        print("H2", H2)
+        self.assertTrue(
+            np.allclose(
+                np.array(
+                    [
+                        [1, 0, 0],
+                        [0, 1, 0],
+                        [0, 0, 1],
+                    ]
+                ),
+                H2,
+            )
+        )
\ No newline at end of file