ground/gcs/src/experimental/SerialLogger/ParseEKFLog.m

   1 function ParseEKFLog(fn)
   2
   3 fid = fopen(fn);
   4 dat = uint8(fread(fid,inf,'uchar'));
   5 fclose(fid);
   6
   7 mag_data_size = 1 + 4*3;
   8 accel_sensor_size = 4*3;
   9 gyro_sensor_size = 4*3;
  10 raw_framing = 15:-1:0;
  11
  12 starts = strfind(char(dat'),char(raw_framing));
  13 starts(end) = [];
  14
  15 counts = typecast(reshape(dat(bsxfun(@plus,starts,(16:19)')),1,[]),'int32');
  16 accel = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(20:31)')),1,[]),'single'),3,[])';
  17 gyro = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(32:43)')),1,[]),'single'),3,[])';
  18 mag_updated = dat(starts+44);
  19 mag = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(45:56)')),1,[]),'single'),3,[])';
  20
  21 %gps = 28 bytes 57:84
  22 NED = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(57:68)')),1,[]),'single'),3,[])';
  23 heading = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(69:72)')),1,[]),'single'),1,[])';
  24 groundspeed = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(73:76)')),1,[]),'single'),1,[])';
  25 quality = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(77:80)')),1,[]),'single'),1,[])';
  26 updated = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(81:84)')),1,[]),'int32'),1,[])';
  27
  28 X = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(85:85+13*4-1)')),1,[]),'single'),13,[])';
  29 P = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(137:137+13*4-1)')),1,[]),'single'),13,[])';
  30
  31 Baro = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(189:192)')),1,[]),'single'),1,[])';
  32 BaroOffset = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(193:196)')),1,[]),'single'),1,[])';
  33
  34 N=length(accel);
  35 rpy = zeros(3,N);
  36 Vne = zeros(2,N);
  37 dt = 0.01;
  38 t=0:dt:(N-1)*dt;
  39 for i = 1:N
  40     rpy(:,i) = q2rpy(X(i,7:10))*180/pi;
  41     groundTrack = heading(i)*pi/180;
  42     Vne(:,i) = groundspeed(i)*[cos(groundTrack); sin(groundTrack)];
  43 end
  44
  45 figure(1); plot(t,rpy(1,:),t,rpy(2,:),t,rpy(3,:));
  46 title('roll-pitch-yaw');
  47 figure(2); plot(t,NED(:,1),t,NED(:,2),t,NED(:,3),t,X(:,1),t,X(:,2),t,X(:,3),t,BaroOffset-Baro);
  48 title('gpsNED');ylim([-500 500]);
  49 figure(3); plot(t,Vne(1,:),t,Vne(2,:),t,X(:,4),t,X(:,5));
  50 title('gpsVne');ylim([-3 3]);
  51 figure(4); plot(t,accel(:,1),t,accel(:,2),t,accel(:,3));
  52 title('accels');ylim([-10 10]);
  53 figure(5); plot(t,gyro(:,1),t,gyro(:,2),t,gyro(:,3));
  54 title('gyros'); ylim([-pi pi]);
  55 figure(6); plot(t,mag(:,1),t,mag(:,2),t,mag(:,3));
  56 title('mags'); ylim([-1200 1200]);
  57 figure(7); plot(t,Baro,t,BaroOffset,t,BaroOffset-NED(:,3));
  58 title('BaroAlt'); ylim([300 410]);
  59 figure(8); plot(t,X(:,11),t,X(:,12),t,X(:,13));
  60 title('GyroBias'); ylim([-1 1]);
  61
  62 Be = [20595  1363  49068]';
  63 ScaledBe = [Be(1)  Be(2) Be(3)]'/Be(3);
  64 ScaledBe = [ScaledBe ScaledBe];
  65 ScaledMag = zeros(3,N);
  66 MagMag = zeros(1,N);
  67 for i = 1:N
  68     ScaledMag(:,i) = mag(i,:)'/mag(i,3);
  69     MagMag(i) = norm(mag(i,:));
  70 end
  71 t2 = [t(1) t(end)];
  72 figure(13); plot(t,ScaledMag(1,:),t,ScaledMag(2,:),t,ScaledMag(3,:),t2,ScaledBe(1,:),t2,ScaledBe(2,:),t2,ScaledBe(3,:));
  73 title('scaledMags'); ylim([-1.01 1.01]);
  74 figure(14); plot(t,MagMag);
  75 title('Mag Magnitude'); ylim([500 1500]);
  76
  77
  78 keyboard