diff --git a/Source/BSP/Inc/imu.h b/Source/BSP/Inc/imu.h
index 3182ae7..71ac265 100644
--- a/Source/BSP/Inc/imu.h
+++ b/Source/BSP/Inc/imu.h
@@ -24,8 +24,8 @@
 
 typedef struct
 {
-	int16_t ax, ay, az; // lsb
-	int16_t gx, gy, gz; // lsb
+	float ax, ay, az; // lsb
+	float gx, gy, gz; // lsb
 } imu_raw_t;
 
 void imu_pow_init();
diff --git a/Source/BSP/Inc/imu_processing.h b/Source/BSP/Inc/imu_processing.h
index bd32da9..a92a0f7 100644
--- a/Source/BSP/Inc/imu_processing.h
+++ b/Source/BSP/Inc/imu_processing.h
@@ -17,8 +17,12 @@ typedef struct
 
 void imu_processing_init();
 
-void imu_read_scaled(imu_scaled_t* out);
+void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib);
+void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax);
 
 void imu_calibrate();
 
+float normalize(float value, float scale, float min, float max);
+long absl(long value);
+
 #endif
\ No newline at end of file
diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c
index 876673c..e7ba4ae 100644
--- a/Source/BSP/Src/imu_processing.c
+++ b/Source/BSP/Src/imu_processing.c
@@ -2,6 +2,8 @@
 #include "biquad_filter.h"
 #include "math.h"
 
+
+
 static float gyro_bias_x = 0.0f;
 static float gyro_bias_y = 0.0f;
 static float gyro_bias_z = 0.0f;
@@ -18,6 +20,15 @@ static biquad_t gyro_x_lpf;
 static biquad_t gyro_y_lpf;
 static biquad_t gyro_z_lpf;
 
+const float accel_min = -8192.0f;
+const float accel_max = 8192.0f;
+
+static int16_t x_calib;
+static int16_t y_calib;
+static int16_t z_calib;
+static long gyro_shift[3] = {0, 0, 0};
+static float acc;
+
 void imu_processing_init()
 {
 	biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
@@ -29,13 +40,42 @@ void imu_processing_init()
 	biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
 }
 
-void imu_read_scaled(imu_scaled_t* out)
+void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
 {
 	static imu_raw_t raw;
 	
 	imu_read_raw(&raw);
 	
-	out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
+	raw.ax = biquad_apply(&accel_x_lpf, raw.ax);
+	raw.ay = biquad_apply(&accel_y_lpf, raw.ay);
+	raw.az = biquad_apply(&accel_z_lpf, raw.az);
+	
+	raw.gx = biquad_apply(&gyro_x_lpf, raw.gx);
+	raw.gy = biquad_apply(&gyro_y_lpf, raw.gy);
+	raw.gz = biquad_apply(&gyro_z_lpf, raw.gz);
+	
+	if (allowed_calib) imu_calib(&raw, 50, 8192, 1000);
+	
+	/*float alpha = 0.01;
+	
+	out->ax = out->ax * (1.0f - alpha) + raw.ax * alpha;
+	out->ay = out->ay * (1.0f - alpha) + raw.ay * alpha;
+	out->az = out->az * (1.0f - alpha) + raw.az * alpha;
+	
+	out->gx = out->gx * (1.0f - alpha) + raw.gx * alpha;
+	out->gy = out->gy * (1.0f - alpha) + raw.gy * alpha;
+	out->gz = out->gz * (1.0f - alpha) + raw.gz * alpha;*/
+	
+	out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max);
+	out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max);
+	out->az = normalize(raw.az, 1.0f, accel_min, accel_max);
+	
+	out->gx = (raw.gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR;
+	out->gy = (raw.gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR;
+	out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
+	
+	////////////////////////////////////////////////////////////
+	/*out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
 	out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
 	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
 	
@@ -49,7 +89,33 @@ void imu_read_scaled(imu_scaled_t* out)
 	
 	out->gx = biquad_apply(&gyro_x_lpf, out->gx);
 	out->gy = biquad_apply(&gyro_y_lpf, out->gy);
-	out->gz = biquad_apply(&gyro_z_lpf, out->gz);
+	out->gz = biquad_apply(&gyro_z_lpf, out->gz);*/
+}
+
+void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
+{
+	long len = imu->gx*imu->gx + imu->gy*imu->gy + imu->gz*imu->gz;
+	if (len > gyrLim*gyrLim) return;
+	
+	x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
+	const float alpha = 0.001f;
+	
+	x_calib = x_calib * (1.0f - alpha) + imu->gx * alpha;
+	y_calib = y_calib * (1.0f - alpha) + imu->gy * alpha;
+	z_calib = z_calib * (1.0f - alpha) + imu->gz * alpha;
+	
+	gyro_shift[0] = x_calib;
+	gyro_shift[1] = y_calib;
+	gyro_shift[2] = z_calib;
+	
+	len = imu->ax*imu->ax + imu->ay*imu->ay + imu->az*imu->az;
+	if (absl(len - accZero*accZero) > accMax*accMax) return;
+	
+	len = sqrtf(len);
+	
+	acc = len;
+	
+	acc = acc * (1.0f - alpha) + len * alpha;
 }
 
 void imu_calibrate()
@@ -89,3 +155,16 @@ void imu_calibrate()
 	gyro_bias_y = sum_gy / samples;
 	gyro_bias_z = sum_gz / samples;
 }
+
+float normalize(float value, float scale, float min, float max)
+{
+	const float len = (max - min) / 2.0f;
+	const float shift = (max + min) / 2.0f;
+	return (value - shift) * scale / len;
+}
+
+long absl(long value)
+{
+	if (value < 0) return -value;
+	return value;
+}
diff --git a/Source/main.c b/Source/main.c
index 3d044b4..8b77777 100644
--- a/Source/main.c
+++ b/Source/main.c
@@ -12,6 +12,7 @@ void TIM6_DAC_IRQHandler();
 
 static uint8_t irs_update_flag = 0;
 static uint8_t control_update_flag = 0;
+static uint8_t allowed_calib = 0;
 
 imu_scaled_t imu;
 IRS irs;
@@ -60,7 +61,7 @@ int main(void)
 		{
 		    irs_update_flag = 0;
 			
-			imu_read_scaled(&imu);
+			imu_read_scaled(&imu, &allowed_calib);
 			
 			irs.gyro.x = imu.gx;
 			irs.gyro.y = imu.gy;
@@ -89,6 +90,8 @@ int main(void)
 		
 		if (rx_chs_normalized.rc_armed)
 	    {
+			allowed_calib = 1;
+			
 	        motors_set_throttle_mix(
 	            rx_chs_normalized.rc_throttle,
 	            &ctrl_chs,
@@ -98,6 +101,7 @@ int main(void)
 	    else
 		{
 			motors_turn_off();
+			allowed_calib = 0;
 		}
 	}
 }