matlab/ins/test.m

   1 function test
   2
   3 insgps_ml('INSGPSInit');
   4 insgps_ml('INSSetMagNorth',[24000 1700 43000]);
   5 insgps_ml('INSSetMagVar',[5 5 5]/1000/1000);
   6 insgps_ml('INSSetAccelVar',1.5e-4 * ones(1,3));
   7 insgps_ml('INSSetGyroVar',2e-5*ones(1,3));
   8
   9 accel = [.15 -.15 -9.9];
  10 gyro = [0.01 0.007 0.012];
  11 mag = [-364 -64 603];
  12 %mag = [-1700 24000 43000];
  13
  14 N = 20000;
  15 a = zeros(N,13);
  16 c = zeros(N,13);
  17 dt = 0.01;
  18 time = (1:N)*dt;
  19 for i = 1:20000
  20         [a(i,:) b K] = insgps_ml('INSStatePrediction',gyro, accel,0.01);
  21         if(mod(i,10) == 0)
  22                 [c(i,:) d K] = insgps_ml('INSMagVelBaroCorrection',mag,[0 0 0], 0);
  23         else
  24                 [c(i,:) d K] = insgps_ml('INSVelBaroCorrection',[0 0 0], 0);
  25         end
  26         if(mod(i,50) == 0)
  27                 subplot(221);
  28                 imagesc(b); %//,[-10 0])
  29                 subplot(222);
  30                 imagesc(K)
  31                 subplot(212);
  32                 imagesc(a)
  33                 drawnow
  34         end
  35 end
  36
  37 keyboard
  38