diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 33184b759..095d92b7b 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -43,7 +43,7 @@
 #define INSGPS_GPS_TIMEOUT 2   /* 2 seconds triggers reinit of position */
 #define INSGPS_GPS_MINSAT  6   /* 2 seconds triggers reinit of position */
 #define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates    */
-#define INSGPS_MAGLEN       1000
+#define INSGPS_MAGLEN      1000
 #define INSGPS_MAGTOL      0.5 /* error in magnetic vector length to use  */
 
 // For debugging the raw sensors
@@ -104,6 +104,9 @@ struct gps_sensor gps_data;
 //! The oversampling rate, ekf is 2k / this
 static uint8_t adc_oversampling = 20;
 
+//! Offset correction of barometric alt, to match gps data
+static float baro_offset = 0;
+
 /**
  * @}
  */
@@ -117,7 +120,6 @@ void ins_outdoor_update()
 {
 	float gyro[3], accel[3], vel[3];
 	static uint32_t last_gps_time = 0;
-	static float baro_offset = 0;
 	uint16_t sensors;
 		
 	// format data for INS algo
@@ -174,7 +176,7 @@ void ins_outdoor_update()
 			baro_offset = gps_data.NED[2] + altitude_data.altitude;
 		} else {
 			/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
-			baro_offset = baro_offset * 0.99 + (gps_data.NED[2] + altitude_data.altitude) * 0.01;
+			baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
 		}
 		gps_data.updated = false;
 	} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
@@ -376,6 +378,7 @@ void print_ekf_binary()
 		PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4);                                           // diag(P) (138:189)
 	
 	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4);                                 // BaroAlt (190:193)	
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4);                                            // baro_offset (194:198)
 }
 #else
 void print_ekf_binary() {}
diff --git a/ground/src/experimental/SerialLogger/ParseEKFLog.m b/ground/src/experimental/SerialLogger/ParseEKFLog.m
new file mode 100644
index 000000000..cf09a42b1
--- /dev/null
+++ b/ground/src/experimental/SerialLogger/ParseEKFLog.m
@@ -0,0 +1,78 @@
+function ParseEKFLog(fn)
+
+fid = fopen(fn);
+dat = uint8(fread(fid,inf,'uchar'));
+fclose(fid);
+
+mag_data_size = 1 + 4*3;
+accel_sensor_size = 4*3;
+gyro_sensor_size = 4*3;
+raw_framing = 15:-1:0;
+
+starts = strfind(char(dat'),char(raw_framing));
+starts(end) = [];
+
+counts = typecast(reshape(dat(bsxfun(@plus,starts,(16:19)')),1,[]),'int32');
+accel = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(20:31)')),1,[]),'single'),3,[])';
+gyro = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(32:43)')),1,[]),'single'),3,[])';
+mag_updated = dat(starts+44);
+mag = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(45:56)')),1,[]),'single'),3,[])';
+
+%gps = 28 bytes 57:84
+NED = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(57:68)')),1,[]),'single'),3,[])';
+heading = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(69:72)')),1,[]),'single'),1,[])';
+groundspeed = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(73:76)')),1,[]),'single'),1,[])';
+quality = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(77:80)')),1,[]),'single'),1,[])';
+updated = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(81:84)')),1,[]),'int32'),1,[])';
+
+X = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(85:85+13*4-1)')),1,[]),'single'),13,[])';
+P = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(137:137+13*4-1)')),1,[]),'single'),13,[])';
+
+Baro = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(189:192)')),1,[]),'single'),1,[])';
+BaroOffset = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(193:196)')),1,[]),'single'),1,[])';
+
+N=length(accel);
+rpy = zeros(3,N);
+Vne = zeros(2,N);
+dt = 0.01;
+t=0:dt:(N-1)*dt;
+for i = 1:N
+    rpy(:,i) = q2rpy(X(i,7:10))*180/pi;
+    groundTrack = heading(i)*pi/180;
+    Vne(:,i) = groundspeed(i)*[cos(groundTrack); sin(groundTrack)];
+end
+
+figure(1); plot(t,rpy(1,:),t,rpy(2,:),t,rpy(3,:));
+title('roll-pitch-yaw');
+figure(2); plot(t,NED(:,1),t,NED(:,2),t,NED(:,3),t,X(:,1),t,X(:,2),t,X(:,3),t,BaroOffset-Baro);
+title('gpsNED');ylim([-500 500]);
+figure(3); plot(t,Vne(1,:),t,Vne(2,:),t,X(:,4),t,X(:,5));
+title('gpsVne');ylim([-3 3]);
+figure(4); plot(t,accel(:,1),t,accel(:,2),t,accel(:,3));
+title('accels');ylim([-10 10]);
+figure(5); plot(t,gyro(:,1),t,gyro(:,2),t,gyro(:,3));
+title('gyros'); ylim([-pi pi]);
+figure(6); plot(t,mag(:,1),t,mag(:,2),t,mag(:,3));
+title('mags'); ylim([-1200 1200]);
+figure(7); plot(t,Baro,t,BaroOffset,t,BaroOffset-NED(:,3));
+title('BaroAlt'); ylim([300 410]);
+figure(8); plot(t,X(:,11),t,X(:,12),t,X(:,13));
+title('GyroBias'); ylim([-1 1]);
+
+Be = [20595  1363  49068]';
+ScaledBe = [Be(1)  Be(2) Be(3)]'/Be(3);
+ScaledBe = [ScaledBe ScaledBe];
+ScaledMag = zeros(3,N);
+MagMag = zeros(1,N);
+for i = 1:N
+    ScaledMag(:,i) = mag(i,:)'/mag(i,3);
+    MagMag(i) = norm(mag(i,:));
+end
+t2 = [t(1) t(end)];
+figure(13); plot(t,ScaledMag(1,:),t,ScaledMag(2,:),t,ScaledMag(3,:),t2,ScaledBe(1,:),t2,ScaledBe(2,:),t2,ScaledBe(3,:));
+title('scaledMags'); ylim([-1.01 1.01]);
+figure(14); plot(t,MagMag);
+title('Mag Magnitude'); ylim([500 1500]);
+
+
+keyboard