diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 4b089fed..ceefa39d 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -4467,12 +4467,12 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
 			// Get coords of a measuring point.
 			uint8_t ix = mesh_point % nMeasPoints; // from 0 to MESH_NUM_X_POINTS - 1
 			uint8_t iy = mesh_point / nMeasPoints;
-			if (!mbl_point_measurement_valid(ix, iy, nMeasPoints)) {
+			/*if (!mbl_point_measurement_valid(ix, iy, nMeasPoints, true)) {
 				printf_P(PSTR("Skipping point [%d;%d] \n"), ix, iy);
 				custom_message_state--;
 				mesh_point++;
 				continue; //skip
-			}
+			}*/
 			if (iy & 1) ix = (nMeasPoints - 1) - ix; // Zig zag
 			float z0 = 0.f;
 			if (has_z && (mesh_point > 0)) {
@@ -4694,6 +4694,9 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
 		if (nMeasPoints == 3) {
 			mbl.upsample_3x3(); //interpolation from 3x3 to 7x7 points using largrangian polynomials while using the same array z_values[iy][ix] for storing (just coppying measured data to new destination and interpolating between them)
 		}
+		if (nMeasPoints == 7) {
+			mbl_interpolation(nMeasPoints);
+		}
 //		SERIAL_ECHOLNPGM("Upsample finished");
 		mbl.active = 1; //activate mesh bed leveling
 //		SERIAL_ECHOLNPGM("Mesh bed leveling activated");
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 85ed0554..c5fc5829 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -3067,11 +3067,13 @@ void mbl_mode_init() {
 	else e_mbl_type = mbl_type;
 }
 
-bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points) {
+bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points, bool zigzag) {
 	    //"human readable" heatbed plan
 		//magnet proximity influence Z coordinate measurements significantly (40 - 100 um)
 		//0 - measurement point is above magnet and Z coordinate can be influenced negatively
 		//1 - we should be in safe distance from magnets, measurement should be accurate
+		if ((ix >= meas_points) || (iy >= meas_points)) return false;
+
 		uint8_t valid_points_mask[7] = {
 					//[X_MAX,Y_MAX]
 			0b1111101,
@@ -3089,4 +3091,24 @@ bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points) {
 		}
 		if((iy%2) == 0)	return (valid_points_mask[6 - iy] & (1 << (6 - ix)));
 		else return (valid_points_mask[6 - iy] & (1 << ix));
+}
+
+void mbl_single_point_interpolation(uint8_t x, uint8_t y, uint8_t meas_points) {
+		uint8_t count = 0;
+		float z = 0;
+		if(mbl_point_measurement_valid(x, y+1, meas_points, false)) { z += mbl.z_values[x][y+1]; count++; }
+		if(mbl_point_measurement_valid(x, y-1, meas_points, false)) { z += mbl.z_values[x][y-1]; count++; }
+		if(mbl_point_measurement_valid(x+1, y, meas_points, false)) { z += mbl.z_values[x+1][y]; count++; }
+		if(mbl_point_measurement_valid(x-1, y, meas_points, false)) { z += mbl.z_values[x+1][y]; count++; }
+		if(count != 0) mbl.z_values[x][y] = z / count; //if we have at least one valid point in surrounding area use average value, otherwise use inaccurately measured Z-coordinate
+}
+
+void mbl_interpolation(uint8_t meas_points) {
+	for (uint8_t x = 0; x < meas_points; x++) {
+		for (uint8_t y = 0; y < meas_points; y++) {
+			if (!mbl_point_measurement_valid(x, y, meas_points, false)) {
+				mbl_single_point_interpolation(x, y, meas_points);
+			}
+		}
+	}
 }
\ No newline at end of file
diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h
index 0d577271..7fd8011c 100644
--- a/Firmware/mesh_bed_calibration.h
+++ b/Firmware/mesh_bed_calibration.h
@@ -210,6 +210,6 @@ extern e_MBL_TYPE e_mbl_type;
 extern void mbl_mode_set();
 extern void mbl_mode_init();
 
-extern bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points);
-
+extern bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points, bool zigzag);
+extern void mbl_interpolation(uint8_t meas_points);
 #endif /* MESH_BED_CALIBRATION_H */