diff --git a/lib/include/srslte/phy/ch_estimation/csi_rs.h b/lib/include/srslte/phy/ch_estimation/csi_rs.h
index cde4f1194..5fe5f9e58 100644
--- a/lib/include/srslte/phy/ch_estimation/csi_rs.h
+++ b/lib/include/srslte/phy/ch_estimation/csi_rs.h
@@ -88,8 +88,8 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   srslte_csi_rs_resource_mapping_t resource_mapping;
 
-  int8_t power_control_offset;    // -8..15 dB
-  int8_t power_control_offset_ss; // -3, 0, 3, 6 dB
+  float power_control_offset;    // -8..15 dB
+  float power_control_offset_ss; // -3, 0, 3, 6 dB
 
   uint32_t scrambling_id; // 0..1023
 
@@ -107,6 +107,10 @@ typedef struct SRSLTE_API {
   float rsrp_dB;
   float epre;
   float epre_dB;
+  float    n0;
+  float    n0_dB;
+  float    snr_dB;
+  uint32_t nof_re;
 } srslte_csi_rs_measure_t;
 
 SRSLTE_API int srslte_csi_rs_nzp_measure(const srslte_carrier_nr_t*          carrier,
@@ -115,4 +119,6 @@ SRSLTE_API int srslte_csi_rs_nzp_measure(const srslte_carrier_nr_t*          car
                                          const cf_t*                         grid,
                                          srslte_csi_rs_measure_t*            measure);
 
+SRSLTE_API uint32_t srslte_csi_rs_measure_info(const srslte_csi_rs_measure_t* measure, char* str, uint32_t str_len);
+
 #endif // SRSLTE_CSI_RS_H_
diff --git a/lib/src/phy/ch_estimation/csi_rs.c b/lib/src/phy/ch_estimation/csi_rs.c
index e52215e9e..17b6cbd70 100644
--- a/lib/src/phy/ch_estimation/csi_rs.c
+++ b/lib/src/phy/ch_estimation/csi_rs.c
@@ -75,7 +75,7 @@ static int csi_rs_location_get_k_list(const srslte_csi_rs_resource_mapping_t* re
       resource->density == srslte_csi_rs_resource_mapping_density_three && resource->cdm == srslte_csi_rs_cdm_nocdm) {
     k_list[0] = k0;
     k_list[1] = k0 + 4;
-    k_list[3] = k0 + 8;
+    k_list[2] = k0 + 8;
     return 3;
   }
 
@@ -151,6 +151,37 @@ uint32_t csi_rs_count(srslte_csi_rs_density_t density, uint32_t nprb)
   return 0;
 }
 
+uint32_t csi_rs_rb_begin(const srslte_carrier_nr_t* carrier, const srslte_csi_rs_resource_mapping_t* m)
+{
+  uint32_t ret = SRSLTE_MAX(carrier->start, m->freq_band.start_rb);
+
+  if ((m->density == srslte_csi_rs_resource_mapping_density_dot5_even && ret % 2 == 1) ||
+      (m->density == srslte_csi_rs_resource_mapping_density_dot5_odd && ret % 2 == 0)) {
+    ret++;
+  }
+
+  return ret;
+}
+
+uint32_t csi_rs_rb_end(const srslte_carrier_nr_t* carrier, const srslte_csi_rs_resource_mapping_t* m)
+{
+  return SRSLTE_MIN(carrier->start + carrier->nof_prb, m->freq_band.start_rb + m->freq_band.nof_rb);
+}
+
+uint32_t csi_rs_rb_stride(const srslte_csi_rs_resource_mapping_t* m)
+{
+  uint32_t ret = 1;
+
+  // Special .5 density cases
+  if (m->density == srslte_csi_rs_resource_mapping_density_dot5_even ||
+      m->density == srslte_csi_rs_resource_mapping_density_dot5_odd) {
+    // Skip one RB
+    ret = 2;
+  }
+
+  return ret;
+}
+
 int srslte_csi_rs_nzp_put(const srslte_carrier_nr_t*          carrier,
                           const srslte_dl_slot_cfg_t*         slot_cfg,
                           const srslte_csi_rs_nzp_resource_t* resource,
@@ -173,9 +204,9 @@ int srslte_csi_rs_nzp_put(const srslte_carrier_nr_t*          carrier,
   }
 
   // Calculate Resource Block boundaries
-  uint32_t rb_begin  = resource->resource_mapping.freq_band.start_rb;
-  uint32_t rb_end    = resource->resource_mapping.freq_band.start_rb + resource->resource_mapping.freq_band.nof_rb;
-  uint32_t rb_stride = 1;
+  uint32_t rb_begin  = csi_rs_rb_begin(carrier, &resource->resource_mapping);
+  uint32_t rb_end    = csi_rs_rb_end(carrier, &resource->resource_mapping);
+  uint32_t rb_stride = csi_rs_rb_stride(&resource->resource_mapping);
 
   // Calculate power allocation
   float beta = srslte_convert_dB_to_amplitude((float)resource->power_control_offset);
@@ -183,21 +214,6 @@ int srslte_csi_rs_nzp_put(const srslte_carrier_nr_t*          carrier,
     beta = 1.0f;
   }
 
-  // Special .5 density cases
-  if (resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_even ||
-      resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_odd) {
-    // Increase the start by one if:
-    // - Even and starts with odd
-    // - Odd and starts with even
-    if ((resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_even && rb_begin % 2 == 1) ||
-        (resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_odd && rb_begin % 2 == 0)) {
-      rb_begin++;
-    }
-
-    // Skip one RB
-    rb_stride = 2;
-  }
-
   for (int l_idx = 0; l_idx < nof_l; l_idx++) {
     // Get symbol index
     uint32_t l = l_list[l_idx];
@@ -259,30 +275,9 @@ int srslte_csi_rs_nzp_measure(const srslte_carrier_nr_t*          carrier,
   }
 
   // Calculate Resource Block boundaries
-  uint32_t rb_begin  = resource->resource_mapping.freq_band.start_rb;
-  uint32_t rb_end    = resource->resource_mapping.freq_band.start_rb + resource->resource_mapping.freq_band.nof_rb;
-  uint32_t rb_stride = 1;
-
-  // Calculate power allocation
-  float beta = srslte_convert_dB_to_amplitude((float)resource->power_control_offset);
-  if (!isnormal(beta)) {
-    beta = 1.0f;
-  }
-
-  // Special .5 density cases
-  if (resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_even ||
-      resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_odd) {
-    // Increase the start by one if:
-    // - Even and starts with odd
-    // - Odd and starts with even
-    if ((resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_even && rb_begin % 2 == 1) ||
-        (resource->resource_mapping.density == srslte_csi_rs_resource_mapping_density_dot5_odd && rb_begin % 2 == 0)) {
-      rb_begin++;
-    }
-
-    // Skip one RB
-    rb_stride = 2;
-  }
+  uint32_t rb_begin  = csi_rs_rb_begin(carrier, &resource->resource_mapping);
+  uint32_t rb_end    = csi_rs_rb_end(carrier, &resource->resource_mapping);
+  uint32_t rb_stride = csi_rs_rb_stride(&resource->resource_mapping);
 
   // Accumulators
   float    epre_acc = 0.0f;
@@ -314,14 +309,14 @@ int srslte_csi_rs_nzp_measure(const srslte_carrier_nr_t*          carrier,
         // Do we need more r?
         if (r_idx >= 64) {
           // ... Generate a bunch of it!
-          srslte_sequence_state_gen_f(&sequence_state, M_SQRT1_2 / beta, (float*)r, 64 * 2);
+          srslte_sequence_state_gen_f(&sequence_state, M_SQRT1_2, (float*)r, 64 * 2);
           r_idx = 0;
         }
 
         // Take CSI-RS from grid and measure
         cf_t tmp = grid[l * SRSLTE_NRE * carrier->nof_prb + k] * conjf(r[r_idx++]);
         rsrp_acc += tmp;
-        epre_acc = __real__ tmp * __real__ tmp + __imag__ tmp * __imag__ tmp;
+        epre_acc += __real__ tmp * __real__ tmp + __imag__ tmp * __imag__ tmp;
         count++;
       }
     }
@@ -329,8 +324,33 @@ int srslte_csi_rs_nzp_measure(const srslte_carrier_nr_t*          carrier,
 
   if (count) {
     measure->epre = epre_acc / (float)count;
-    measure->rsrp = cabsf(rsrp_acc) / (float)count;
+    rsrp_acc /= (float)count;
+    measure->rsrp = (__real__ rsrp_acc * __real__ rsrp_acc + __imag__ rsrp_acc * __imag__ rsrp_acc);
+    if (measure->epre > measure->rsrp) {
+      measure->n0 = measure->epre - measure->rsrp;
+    } else {
+      measure->n0 = 0.0f;
+    }
   }
 
+  measure->rsrp_dB = srslte_convert_power_to_dB(measure->rsrp);
+  measure->epre_dB = srslte_convert_power_to_dB(measure->epre);
+  measure->n0_dB   = srslte_convert_power_to_dB(measure->n0);
+  measure->snr_dB  = measure->rsrp_dB - measure->n0_dB;
+  measure->nof_re  = count;
+
   return SRSLTE_SUCCESS;
+}
+
+uint32_t srslte_csi_rs_measure_info(const srslte_csi_rs_measure_t* measure, char* str, uint32_t str_len)
+{
+  return srslte_print_check(str,
+                            str_len,
+                            0,
+                            "rsrp=%+.1f, epre=%+.1f, n0=%+.1f, snr=%+.1f, nof_re=%d",
+                            measure->rsrp_dB,
+                            measure->epre_dB,
+                            measure->n0_dB,
+                            measure->snr_dB,
+                            measure->nof_re);
 }
\ No newline at end of file
diff --git a/lib/src/phy/ch_estimation/test/CMakeLists.txt b/lib/src/phy/ch_estimation/test/CMakeLists.txt
index fce186bf8..94c3cf030 100644
--- a/lib/src/phy/ch_estimation/test/CMakeLists.txt
+++ b/lib/src/phy/ch_estimation/test/CMakeLists.txt
@@ -88,3 +88,13 @@ target_link_libraries(dmrs_pdcch_test srslte_phy)
 
 add_test(dmrs_pdcch_test dmrs_pdcch_test)
 
+
+########################################################################
+# NR CSI RS Measurement test
+########################################################################
+
+add_executable(csi_rs_test csi_rs_test.c)
+target_link_libraries(csi_rs_test srslte_phy)
+
+add_test(csi_rs_test csi_rs_test -o 3 -S 0 -L 150 -f 3 -p 15)
+
diff --git a/lib/src/phy/ch_estimation/test/csi_rs_test.c b/lib/src/phy/ch_estimation/test/csi_rs_test.c
new file mode 100644
index 000000000..abacf339a
--- /dev/null
+++ b/lib/src/phy/ch_estimation/test/csi_rs_test.c
@@ -0,0 +1,210 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2020 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#include "srslte/common/test_common.h"
+#include "srslte/phy/ch_estimation/csi_rs.h"
+#include "srslte/phy/utils/vector.h"
+#include <getopt.h>
+#include <srslte/srslte.h>
+#include <stdlib.h>
+
+static srslte_carrier_nr_t carrier = {
+    0,  // cell_id
+    0,  // numerology
+    50, // nof_prb
+    0,  // start
+    1   // max_mimo_layers
+};
+
+static float    snr_dB               = 20.0;
+static float    power_control_offset = NAN;
+static uint32_t start_rb             = UINT32_MAX;
+static uint32_t nof_rb               = UINT32_MAX;
+static uint32_t first_symbol         = UINT32_MAX;
+
+static int test(const srslte_dl_slot_cfg_t*         slot_cfg,
+                const srslte_csi_rs_nzp_resource_t* resource,
+                srslte_channel_awgn_t*              awgn,
+                cf_t*                               grid)
+{
+  srslte_csi_rs_measure_t measure = {};
+
+  // Put NZP-CSI-RS
+  TESTASSERT(srslte_csi_rs_nzp_put(&carrier, slot_cfg, resource, grid) == SRSLTE_SUCCESS);
+
+  // Configure N0 and add Noise
+  TESTASSERT(srslte_channel_awgn_set_n0(awgn, (float)resource->power_control_offset - snr_dB) == SRSLTE_SUCCESS);
+  srslte_channel_awgn_run_c(awgn, grid, grid, SRSLTE_SLOT_LEN_RE_NR(carrier.nof_prb));
+
+  TESTASSERT(srslte_csi_rs_nzp_measure(&carrier, slot_cfg, resource, grid, &measure) == SRSLTE_SUCCESS);
+
+  const float rsrp_dB_gold = (float)resource->power_control_offset;
+  const float epre_dB_gold =
+      srslte_convert_power_to_dB(srslte_convert_dB_to_power(rsrp_dB_gold) + awgn->std_dev * awgn->std_dev);
+  const float n0_dB_gold = srslte_convert_amplitude_to_dB(awgn->std_dev);
+
+  if (srslte_verbose >= SRSLTE_VERBOSE_INFO) {
+    char str[128] = {};
+    srslte_csi_rs_measure_info(&measure, str, sizeof(str));
+    INFO("Measure: %s\n", str);
+  }
+
+  TESTASSERT(fabsf(measure.rsrp_dB - rsrp_dB_gold) < 1.0f);
+  TESTASSERT(fabsf(measure.epre_dB - epre_dB_gold) < 1.0f);
+  TESTASSERT(fabsf(measure.n0_dB - n0_dB_gold) < 2.0f);
+  TESTASSERT(fabsf(measure.snr_dB - snr_dB) < 2.0f);
+
+  return SRSLTE_SUCCESS;
+}
+
+static void usage(char* prog)
+{
+  printf("Usage: %s [recov]\n", prog);
+  printf("\t-p nof_prb [Default %d]\n", carrier.nof_prb);
+  printf("\t-c cell_id [Default %d]\n", carrier.id);
+  printf("\t-s SNR in dB [Default %.2f]\n", snr_dB);
+  printf("\t-S Start RB index [Default %d]\n", start_rb);
+  printf("\t-L Number of RB [Default %d]\n", nof_rb);
+  printf("\t-f First symbol index [Default %d]\n", first_symbol);
+  printf("\t-o Power control offset [Default %.2f]\n", power_control_offset);
+  printf("\t-v increase verbosity\n");
+}
+
+static void parse_args(int argc, char** argv)
+{
+  int opt;
+  while ((opt = getopt(argc, argv, "pcosSLfv")) != -1) {
+    switch (opt) {
+      case 'p':
+        carrier.nof_prb = (uint32_t)strtol(argv[optind], NULL, 10);
+        break;
+      case 'c':
+        carrier.id = (uint32_t)strtol(argv[optind], NULL, 10);
+        break;
+      case 'o':
+        power_control_offset = strtof(argv[optind], NULL);
+        break;
+      case 's':
+        snr_dB = strtof(argv[optind], NULL);
+        break;
+      case 'S':
+        start_rb = (uint32_t)strtol(argv[optind], NULL, 10);
+        break;
+      case 'L':
+        nof_rb = (uint32_t)strtol(argv[optind], NULL, 10);
+        break;
+      case 'f':
+        first_symbol = (uint32_t)strtol(argv[optind], NULL, 10);
+        break;
+      case 'v':
+        srslte_verbose++;
+        break;
+      default:
+        usage(argv[0]);
+        exit(-1);
+    }
+  }
+}
+
+int main(int argc, char** argv)
+{
+  int                          ret      = SRSLTE_ERROR;
+  srslte_dl_slot_cfg_t         slot_cfg = {};
+  srslte_csi_rs_nzp_resource_t resource = {};
+  srslte_channel_awgn_t        awgn     = {};
+
+  parse_args(argc, argv);
+
+  cf_t* grid = srslte_vec_cf_malloc(SRSLTE_SLOT_LEN_RE_NR(carrier.nof_prb));
+  if (grid == NULL) {
+    ERROR("Alloc\n");
+    goto clean_exit;
+  }
+
+  if (srslte_channel_awgn_init(&awgn, 1234) < SRSLTE_SUCCESS) {
+    ERROR("AWGN Init\n");
+    goto clean_exit;
+  }
+
+  // Fixed parameters, other params are not implemented
+  resource.resource_mapping.cdm     = srslte_csi_rs_cdm_nocdm;
+  resource.resource_mapping.density = srslte_csi_rs_resource_mapping_density_three;
+  resource.resource_mapping.row     = srslte_csi_rs_resource_mapping_row_1;
+  resource.resource_mapping.ports   = 1;
+
+  // Row 1 supported only!
+  uint32_t nof_freq_dom_alloc = SRSLTE_CSI_RS_NOF_FREQ_DOMAIN_ALLOC_ROW1;
+
+  uint32_t first_symbol_begin = (first_symbol != UINT32_MAX) ? first_symbol : 0;
+  uint32_t first_symbol_end   = (first_symbol != UINT32_MAX) ? first_symbol : 13;
+  for (resource.resource_mapping.first_symbol_idx = first_symbol_begin;
+       resource.resource_mapping.first_symbol_idx <= first_symbol_end;
+       resource.resource_mapping.first_symbol_idx++) {
+
+    // Iterate over possible power control offset
+    float power_control_offset_begin = isnormal(power_control_offset) ? power_control_offset : -8.0f;
+    float power_control_offset_end   = isnormal(power_control_offset) ? power_control_offset : 15.0f;
+    for (resource.power_control_offset = power_control_offset_begin;
+         resource.power_control_offset <= power_control_offset_end;
+         resource.power_control_offset += 1.0f) {
+
+      // Iterate over all possible starting number of PRB
+      uint32_t start_rb_begin = (start_rb != UINT32_MAX) ? start_rb : 0;
+      uint32_t start_rb_end   = (start_rb != UINT32_MAX) ? start_rb : carrier.nof_prb - 24;
+      for (resource.resource_mapping.freq_band.start_rb = start_rb_begin;
+           resource.resource_mapping.freq_band.start_rb <= start_rb_end;
+           resource.resource_mapping.freq_band.start_rb += 4) {
+
+        // Iterate over all possible number of PRB
+        uint32_t nof_rb_begin = (nof_rb != UINT32_MAX) ? nof_rb : 24;
+        uint32_t nof_rb_end =
+            (nof_rb != UINT32_MAX) ? nof_rb : (carrier.nof_prb - resource.resource_mapping.freq_band.start_rb);
+        for (resource.resource_mapping.freq_band.nof_rb = nof_rb_begin;
+             resource.resource_mapping.freq_band.nof_rb <= nof_rb_end;
+             resource.resource_mapping.freq_band.nof_rb += 4) {
+
+          // Iterate for all slot numbers
+          for (slot_cfg.idx = 0; slot_cfg.idx < SRSLTE_NSLOTS_PER_FRAME_NR(carrier.numerology); slot_cfg.idx++) {
+            // Steer Frequency allocation
+            for (uint32_t freq_dom_alloc = 0; freq_dom_alloc < nof_freq_dom_alloc; freq_dom_alloc++) {
+              for (uint32_t i = 0; i < nof_freq_dom_alloc; i++) {
+                resource.resource_mapping.frequency_domain_alloc[i] = i == freq_dom_alloc;
+              }
+
+              // Call actual test
+              if (test(&slot_cfg, &resource, &awgn, grid) < SRSLTE_SUCCESS) {
+                goto clean_exit;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  ret = SRSLTE_SUCCESS;
+
+clean_exit:
+  if (grid) {
+    free(grid);
+  }
+
+  srslte_channel_awgn_free(&awgn);
+
+  if (ret == SRSLTE_SUCCESS) {
+    printf("Passed!\n");
+  } else {
+    printf("Failed!\n");
+  }
+
+  return ret;
+}
\ No newline at end of file
diff --git a/lib/src/phy/ch_estimation/test/dmrs_pdsch_test.c b/lib/src/phy/ch_estimation/test/dmrs_pdsch_test.c
index c035838f2..1e2de636f 100644
--- a/lib/src/phy/ch_estimation/test/dmrs_pdsch_test.c
+++ b/lib/src/phy/ch_estimation/test/dmrs_pdsch_test.c
@@ -124,7 +124,7 @@ static const golden_t gold[] = {{.mapping_type   = srslte_pdsch_mapping_type_A,
                                  .sc_idx         = {0, 2, 4, 6, 8, 10}},
                                 {}};
 
-void usage(char* prog)
+static void usage(char* prog)
 {
   printf("Usage: %s [recov]\n", prog);
 
@@ -135,7 +135,7 @@ void usage(char* prog)
   printf("\t-v increase verbosity\n");
 }
 
-void parse_args(int argc, char** argv)
+static void parse_args(int argc, char** argv)
 {
   int opt;
   while ((opt = getopt(argc, argv, "rcov")) != -1) {