diff --git a/src/costs/CostUtils.jl b/src/costs/CostUtils.jl
index ca7e39da..633da322 100644
--- a/src/costs/CostUtils.jl
+++ b/src/costs/CostUtils.jl
@@ -15,15 +15,20 @@ abstract type NonQuadraticCost <: Cost end
 # If we need to perform a spectral shift to enforce PD-ness, we can set ρ accordingly.
 function homogenize_cost_matrix(M::AbstractMatrix{Float64}, m=zeros(size(M, 1))::AbstractVector{Float64}, cm=0.0::Float64, ρ=nothing)
     # If we're gonna have problems with singularity, then spectral shift the matrix.
-    if all(iszero.(m)) && cm == 0.0 && ρ == nothing
-        ρ = 1e-32
-    elseif (ρ == nothing)
-        ρ = 0.0
+    # if all(iszero.(m)) && cm == 0.0 && ρ == nothing
+    #     ρ = vcat(hcat(zeros(size(M)), zeros(size(m))),
+    #              hcat(zeros(size(m')),   [1e-32]))
+    # else
+    if (ρ == nothing)
+        ρ = zeros(size(M, 1)+1, size(M, 2)+1)
+    else
+        ρ = ρ * Matrix(I, size(M, 1)+1, size(M, 2)+1)
     end
 
     M_dim = size(M, 1)
-    return vcat(hcat(M ,  m),
-                hcat(m', cm)) + ρ * I
+    out = vcat(hcat(M ,  m),
+               hcat(m', cm)) + ρ
+    return out
 end
 
 # Homogenize state - by default, this adds a 1 to the bottom. If a custom one is needed, define it elsewhere.
diff --git a/src/costs/QuadraticCost.jl b/src/costs/QuadraticCost.jl
index f0225f57..199f7cc8 100644
--- a/src/costs/QuadraticCost.jl
+++ b/src/costs/QuadraticCost.jl
@@ -51,7 +51,7 @@ function get_homogenized_state_cost_matrix(c::QuadraticCost)
 end
 
 function get_homogenized_control_cost_matrix(c::QuadraticCost, player_idx::Int)
-    return homogenize_cost_matrix(c.Rs[player_idx], c.rs[player_idx], c.crs[player_idx])
+    return homogenize_cost_matrix(c.Rs[player_idx], c.rs[player_idx], c.crs[player_idx], 1e-32)
 end
 
 export get_homogenized_state_cost_matrix, get_homogenized_control_cost_matrix
diff --git a/test/TestLQSolvers.jl b/test/TestLQSolvers.jl
index afae3483..77445d5f 100644
--- a/test/TestLQSolvers.jl
+++ b/test/TestLQSolvers.jl
@@ -52,7 +52,7 @@ seed!(0)
 
         # Perturb each strategy a little bit and confirm that cost only
         # increases for that player.
-        ϵ = 1e-1
+        ϵ = 1e-2
         for ii in 1:2
             for tt in 1:horizon
                 ctrl_strat_P̃s = deepcopy(ctrl_strat_Ps)
@@ -105,12 +105,16 @@ seed!(0)
         optimal_stackelberg_costs = [evaluate(c, xs, us) for c in costs]
 
         # Define some useful constants.
-        ϵ = 1e-1
+        ϵ = 1e-2
         leader_idx = stackelberg_leader_idx
         follower_idx = 3 - stackelberg_leader_idx
         num_players = num_agents(dyn)
 
+
+        println("\n\n\n=================\n\n\n")
+
         for tt in horizon-1:-1:1
+            println(tt)
             time_range = (tt, tt+1)
 
             # Copy the things we will alter.
@@ -123,6 +127,8 @@ seed!(0)
             ũs[leader_idx][:, tt] += ϵ * randn(udim(dyn, leader_idx))
             ũhs = homogenize_ctrls(dyn, ũs)
 
+            println(get_homogenized_state_cost_matrix(future_costs[follower_idx][tt+1])[5, 5])
+
             # Re-solve for the optimal follower input given the perturbed leader trajectory.
             A = get_homogenized_state_dynamics_matrix(dyn)
             B₂ = get_homogenized_control_dynamics_matrix(dyn, follower_idx)
@@ -159,7 +165,6 @@ seed!(0)
 
             x̃s = unroll_raw_controls(dyn, times, ũs, x₁)
             new_stack_costs = [evaluate(c, x̃s, ũs) for c in costs]
-            optimal_stackelberg_costs = [evaluate(c, xs, us) for c in costs]
 
             # This test evaluates the cost for the entire perturbed trajectory against the optimal cost.
             @test new_stack_costs[leader_idx] ≥ optimal_stackelberg_costs[leader_idx]
@@ -175,7 +180,7 @@ seed!(0)
         optimal_stackelberg_costs = [evaluate(c, xs, us) for c in costs]
 
         # Define some useful constants.
-        ϵ = 1e-1
+        ϵ = 1e-2
         leader_idx = stackelberg_leader_idx
         follower_idx = 3 - stackelberg_leader_idx
         num_players = follower_idx