diff --git a/hdl/matrix_mul.vhd b/hdl/matrix_mul.vhd
index be3293e65d7fe8fb3d40eee443f234765b461816..07a7d79e222962e1eda03502a8fb5ae2060a9544 100644
--- a/hdl/matrix_mul.vhd
+++ b/hdl/matrix_mul.vhd
@@ -241,8 +241,10 @@ begin
                 matmult(I)              <= (others => '0');
                 matmult(I+C_N_MM_PSC/2) <= (others => '0');
             elsif rising_edge(clk) then
-                matmult(I)              <= f_resize_sat(f_resize_lsb(accu_x, C_W_MM_ACCU-C_N_MM_RND), C_W_MM);
-                matmult(I+C_N_MM_PSC/2) <= f_resize_sat(f_resize_lsb(accu_y, C_W_MM_ACCU-C_N_MM_RND), C_W_MM);
+                matmult(I)              <= f_resize_sat(f_resize_lsb(accu_x, C_W_MM_ACCU-C_N_MM_RND), C_W_MM) when accu_x(C_N_MM_RND-1) = '0' else
+                                           f_resize_sat(f_sum_sat(f_resize_lsb(accu_x, C_W_MM_ACCU-C_N_MM_RND), to_signed(1, C_W_MM_ACCU-C_N_MM_RND)), C_W_MM);
+                matmult(I+C_N_MM_PSC/2) <= f_resize_sat(f_resize_lsb(accu_y, C_W_MM_ACCU-C_N_MM_RND), C_W_MM) when accu_y(C_N_MM_RND-1) = '0' else
+                                           f_resize_sat(f_sum_sat(f_resize_lsb(accu_y, C_W_MM_ACCU-C_N_MM_RND), to_signed(1, C_W_MM_ACCU-C_N_MM_RND)), C_W_MM);
             end if;
         end process;
 
diff --git a/hdl/pkg_corrmatrix.vhd b/hdl/pkg_corrmatrix.vhd
index 63473155cdfe8f2d61d85a80ba488aeacbc1dbc8..bd5305b5a6f755a2fb0c83149d687ee752b06131 100644
--- a/hdl/pkg_corrmatrix.vhd
+++ b/hdl/pkg_corrmatrix.vhd
@@ -26,15 +26,15 @@ package pkg_corr_matrix is
     constant C_N_MM_BPM         : natural := 122; -- number of bpm (matrix columns)
     constant C_N_MM_PSC         : natural := 100; -- number of psc (matrix lines)
 
-    constant C_W_MM_COEF        : natural := 24;
+    constant C_W_MM_COEF        : natural := 26;
 
-    constant C_W_MM_MULT        : natural := 48; -- C_W_OE+C_W_MM_COEF;
+    constant C_W_MM_MULT        : natural := 50; -- C_W_OE+C_W_MM_COEF;
     constant C_W_MM_IDCNT       : natural := 8;  -- > natural(ceil(log2(real(C_N_MM_BPM))));
-    constant C_W_MM_ACCU        : natural := 56; -- C_W_MM_MULT+C_W_MM_IDCNT;
+    constant C_W_MM_ACCU        : natural := 58; -- C_W_MM_MULT+C_W_MM_IDCNT;
 
-    constant C_N_MM_SAT         : natural := 0;
-    constant C_N_MM_RND         : natural := 23;
-    constant C_W_MM             : natural := 32; --C_W_MM_ACCU-C_N_MM_SAT-C_N_MM_RND
+    constant C_N_MM_SAT         : natural := 7;
+    constant C_N_MM_RND         : natural := 15;
+    constant C_W_MM             : natural := 36; --C_W_MM_ACCU-C_N_MM_SAT-C_N_MM_RND
 
     -- PI corrector
     constant C_W_COR_COEF       : natural := 17;