diff --git a/src/survive_kalman_lighthouses.c b/src/survive_kalman_lighthouses.c
index e99b479..f18daa4 100644
--- a/src/survive_kalman_lighthouses.c
+++ b/src/survive_kalman_lighthouses.c
@@ -92,7 +92,7 @@ void survive_kalman_lighthouse_update_position(SurviveKalmanLighthouse *tracker,
 			cnCopy(&tracker->model.P, &tracker->push_cov, 0);
 
 			SurvivePose min_error = {.Pos = {1e-5, 1e-5, 1e-5}, .Rot = {1e-6, 1e-6, 1e-6, 1e-6}};
-			CnMat minError = cnVec(7, min_error.Pos);
+			CnMat minError = cnVec(6, min_error.Pos);
 			cn_add_diag(&tracker->push_cov, &minError, 5);
 
 			tracker->state.Lighthouse = ctx->bsd[tracker->lh].Pose;
diff --git a/src/test_cases/test_replays.c b/src/test_cases/test_replays.c
index 52cb243..b4b838b 100644
--- a/src/test_cases/test_replays.c
+++ b/src/test_cases/test_replays.c
@@ -246,9 +246,13 @@ static int test_path(const char *filename, int main_argc, char **main_argv) {
 		FLT err[2] = {0};
 		ApplyPoseToPose(&pose, &original2current, &pose);
 
-		if (!quatiszero(pose.Rot))
+		if (!quatiszero(pose.Rot) && !quatiszero(ctx->bsd[i].Pose.Rot))
 			diff(err, &pose, &ctx->bsd[i].Pose);
 
+		if(!quatiszero(pose.Rot) && ctx->bsd[i].PositionSet == 0) {
+		    err[0] = INFINITY;
+		}
+
 		fprintf(stderr, "                  " SurvivePose_format "\terr: %f %f\n", pose.Pos[0], pose.Pos[1], pose.Pos[2],
 				pose.Rot[0], pose.Rot[1], pose.Rot[2], pose.Rot[3], err[0], err[1]);