matlab/ins/analyzeEKF.m

   1 function analyzeEKF(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 %gps = 28 bytes 57:84
  21 X = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(85:85+13*4-1)')),1,[]),'single'),13,[])';
  22 P = reshape(typecast(reshape(dat(bsxfun(@plus,starts,(137:137+13*4-1)')),1,[]),'single'),13,[])';
  23
  24 insgps_ml('INSGPSInit');
  25 insgps_ml('INSSetMagNorth',[24000 1700 43000]);
  26 insgps_ml('INSSetMagVar',[50 50 50]/1000/1000);
  27 insgps_ml('INSSetAccelVar',1.5e-4 * ones(1,3));
  28 insgps_ml('INSSetGyroVar',2e-5*ones(1,3));
  29
  30 for i = 1:size(accel,1)
  31         [a(:,i) b K] = insgps_ml('INSStatePrediction',gyro(i,:),accel(i,:), 0.01);
  32         if(mag_updated(i))
  33                 [c(:,i) d] = insgps_ml('INSMagVelBaroCorrection',mag(i,:),[0 0 0], 0);
  34         else
  35                 [c(:,i) d] = insgps_ml('INSVelBaroCorrection',[0 0 0], 0);
  36         end
  37         if(mod(i,10) == 0)
  38                 subplot(211);
  39                 imagesc(b < 0); %//,[-10 0])
  40                 subplot(212);
  41                 imagesc(a)
  42                 drawnow
  43         end
  44 end
  45
  46 imagesc(a)
  47 keyboard