diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index 33a282da9a..243abc5c08 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -33,7 +33,7 @@ class VehicleParamsLearner:
     self.x_initial[States.STEER_RATIO] = steer_ratio
     self.x_initial[States.STIFFNESS] = stiffness_factor
     self.x_initial[States.ANGLE_OFFSET] = angle_offset
-    self.P_initial = P_initial or CarKalman.P_initial
+    self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
 
     self.kf.set_globals(
       mass=CP.mass,
@@ -240,7 +240,7 @@ def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, re
 
         initial_filter_std = np.array(lp.debugFilterState.std)
         if debug and len(initial_filter_std) != 0:
-          p_initial = initial_filter_std
+          p_initial = np.diag(initial_filter_std)
 
         steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
         retrieve_success = True
diff --git a/selfdrive/locationd/test/test_paramsd.py b/selfdrive/locationd/test/test_paramsd.py
new file mode 100644
index 0000000000..3c2bf9249a
--- /dev/null
+++ b/selfdrive/locationd/test/test_paramsd.py
@@ -0,0 +1,57 @@
+import random
+import numpy as np
+import json
+
+from cereal import messaging
+from openpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed
+from openpilot.selfdrive.locationd.models.car_kf import CarKalman
+from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
+from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
+from openpilot.common.params import Params
+from openpilot.tools.lib.logreader import LogReader
+
+
+def get_random_live_parameters(CP):
+  msg = messaging.new_message("liveParameters")
+  msg.liveParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
+  msg.liveParameters.stiffnessFactor = random.random()
+  msg.liveParameters.angleOffsetAverageDeg = random.random()
+  msg.liveParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
+  return msg
+
+
+class TestParamsd:
+  def test_read_saved_params(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = get_random_live_parameters(CP)
+    params.put("LiveParameters", msg.to_bytes())
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+
+    migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
+    sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
+    np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
+    np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
+    np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)
+    np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
+    np.testing.assert_allclose(np.diagonal(p_init), msg.liveParameters.debugFilterState.std)
+
+  # TODO Remove this test after the support for old format is removed
+  def test_read_saved_params_old_format(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = get_random_live_parameters(CP)
+    params.put("LiveParameters", json.dumps(msg.liveParameters.to_dict()))
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+
+    migrate_cached_vehicle_params_if_needed(params)
+    sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
+    np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
+    np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
+    np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)