matlab/ins/sim.m

   1 t0 = 60000;
   2
   3 bad = find(Gyros.timestamp > max(medfilt1(Gyros.timestamp,5)) | Gyros.timestamp < t0);
   4 Gyros.timestamp(bad) = [];
   5 Gyros.x(bad) = [];
   6 Gyros.y(bad) = [];
   7 Gyros.z(bad) = [];
   8 Gyros.timestamp = medfilt1(Gyros.timestamp,5);
   9
  10 bad = find(Accels.timestamp > max(medfilt1(Accels.timestamp,5)));
  11 Accels.timestamp(bad) = [];
  12 Accels.x(bad) = [];
  13 Accels.y(bad) = [];
  14 Accels.z(bad) = [];
  15 Accels.timestamp = medfilt1(Accels.timestamp,5);
  16
  17 bad = find(Magnetometer.timestamp > max(medfilt1(Magnetometer.timestamp,5)));
  18 Magnetometer.timestamp(bad) = [];
  19 Magnetometer.x(bad) = [];
  20 Magnetometer.y(bad) = [];
  21 Magnetometer.z(bad) = [];
  22 Magnetometer.timestamp = medfilt1(Magnetometer.timestamp,5);
  23
  24 bad = find(BaroAltitude.timestamp > max(medfilt1(BaroAltitude.timestamp,5)));
  25 BaroAltitude.timestamp(bad) = [];
  26 BaroAltitude.Altitude(bad) = [];
  27
  28
  29 bad = find(GPSPosition.timestamp > max(medfilt1(GPSPosition.timestamp,5)));
  30 GPSPosition.timestamp(bad) = [];
  31 GPSPosition.Latitude(bad) = [];
  32 GPSPosition.Longitude(bad) = [];
  33 GPSPosition.Altitude(bad) = [];
  34 GPSPosition.Heading(bad) = [];
  35 GPSPosition.Satellites(bad) = [];
  36 GPSPosition.Groundspeed(bad) = [];
  37 GPSPosition.timestamp = medfilt1(GPSPosition.timestamp,5);
  38
  39 outdoor = any(GPSPosition.Satellites > 7);
  40
  41 north = [24000 1700 43000];
  42 baro_offset = 0;
  43 if outdoor
  44     idx = find(GPSPosition.Satellites > 7, 1, 'first');
  45     home = [GPSPosition.Latitude(idx) GPSPosition.Longitude(idx) GPSPosition.Altitude(idx)];
  46     home(1:2) = home(1:2) / 10e6;
  47     baro_offset = -125;
  48 end
  49
  50 insgps_ml('INSGPSInit')
  51 insgps_ml('INSSetMagNorth',north);
  52 insgps_ml('INSSetMagVar',[5 5 5]/1000);
  53 insgps_ml('INSSetAccelVar',1.5e-5 * ones(1,3));
  54 insgps_ml('INSSetGyroVar',2e-4*ones(1,3));
  55
  56 accel_idx = 1;
  57 mag_idx = 1;
  58 baro_idx = 1;
  59 gps_idx = 1;
  60
  61 update_mag = false;
  62 update_baro = false;
  63 update_pos = false;
  64
  65 inited = false;
  66
  67 ned = zeros(3,length(Gyros.timestamp));
  68 gps_vel = zeros(3,length(Gyros.timestamp));
  69 a = zeros(13,length(Gyros.timestamp));
  70 for i = 1:length(Gyros.timestamp)-1000
  71     t = Gyros.timestamp(i);
  72     gyro = [Gyros.x(i) Gyros.y(i) Gyros.z(i)] * pi / 180;
  73
  74     accel_idx = accel_idx - 1 + find(Accels.timestamp(accel_idx:end) >= t,1,'first');
  75     accel = [Accels.x(accel_idx) Accels.y(accel_idx) Accels.z(accel_idx)-0.4];
  76
  77     dT = 0.0013;
  78     [a(:,i) b K] = insgps_ml('INSStatePrediction',gyro,accel,dT);
  79
  80     if Magnetometer.timestamp(mag_idx) <= t
  81         update_mag = true;
  82         mag_idx = mag_idx + 1;
  83         mag = [Magnetometer.x(mag_idx) Magnetometer.y(mag_idx) Magnetometer.z(mag_idx)];
  84     end
  85
  86     if BaroAltitude.timestamp(baro_idx) <= t
  87         baro_idx = baro_idx + 1;
  88         update_baro = true;
  89         baro = -BaroAltitude.Altitude(baro_idx) + baro_offset;
  90     end
  91
  92     if outdoor && GPSPosition.timestamp(gps_idx) <= t
  93         gps_idx = gps_idx + 1;
  94         lla = [GPSPosition.Latitude(gps_idx) GPSPosition.Longitude(gps_idx) GPSPosition.Altitude(gps_idx)];
  95         lla(1:2) = lla(1:2) / 10e6;
  96         pos = LLA2NED(lla, home);
  97         vel = [cos(GPSPosition.Heading(gps_idx) * pi / 180), sin(GPSPosition.Heading(gps_idx) * pi / 180) 0];
  98         vel = vel * GPSPosition.Groundspeed(gps_idx);
  99         update_pos = true;
 100     elseif ~outdoor
 101         pos = [0 0 baro];
 102         vel = [0 0 0];
 103         update_pos = true;
 104     end
 105
 106     if t > 4.1e5 & t < 4.2e5
 107         update_pos = false;
 108     end
 109
 110     ned(:,i) = pos;
 111     gps_vel(:,i) = vel;
 112     if ~inited && update_pos && update_baro && update_mag
 113         update_pos = false;
 114         update_baro = false;
 115         update_mag = false;
 116
 117         inited = true;
 118
 119         rpy(1) = atan2(accel(1), accel(3)) * 0;
 120         rpy(2) = atan2(accel(2), accel(3)) * 0;
 121         rpy(3) = atan2(mag(2),-mag(1));
 122
 123         %q1 = AttiudeFromVectors(mag',north',accel');
 124         q1 = RPY2Quaternion(rpy * 180 / pi)';
 125         insgps_ml('INSSetState',[pos(1) pos(2) baro vel q1' -mean(Gyros.x(1:100) * pi / 180) -mean(Gyros.y(1:100) * pi / 180) 0]);
 126     elseif inited && i < 100
 127         insgps_ml('INSSetMagVar',[5 5 5]);
 128         insgps_ml('INSSetPosVelVar',1 / 1000);
 129         % zero bias
 130         %insgps_ml('INSSetState',[a(1:10,i)' 0 0 0]);
 131         %state = insgps_ml('INSFullCorrection',mag,pos,vel,baro);
 132     elseif inited
 133         insgps_ml('INSSetMagVar',[5 5 5]*100);
 134
 135         if ~outdoor
 136             if update_mag
 137                 insgps_ml('INSMagCorrection',mag);
 138                 update_mag = false;
 139             elseif update_baro
 140                 insgps_ml('INSVelBaroCorrection',vel,baro);
 141                 update_baro = false;
 142             elseif update_pos
 143                 insgps_ml('INSGpsCorrection',[0 0 baro],[0 0 0]);
 144                 update_pos = false;
 145             end
 146         else % outdoor
 147             insgps_ml('INSSetPosVelVar',0.0001);
 148
 149             if update_mag
 150                 insgps_ml('INSMagCorrection',mag);
 151                 update_mag = false;
 152             elseif update_baro
 153                 insgps_ml('INSBaroCorrection',baro);
 154                 update_baro = false;
 155             elseif update_pos
 156                 insgps_ml('INSGpsCorrection',pos,vel);
 157                 update_pos = false;
 158             end
 159         end
 160     end
 161
 162     %state = insgps_ml('INSFullCorrection',mag,pos,vel,baro);
 163     if(mod(i,100) == 0)
 164         q = a(7:10,1:i);
 165         rpy = Quaternion2RPY(q');
 166         subplot(311);
 167         idx = find((AttitudeActual.timestamp < t) & (AttitudeActual.timestamp > Gyros.timestamp(1)));
 168         plot(Gyros.timestamp(1:i),rpy(:,1:2),AttitudeActual.timestamp(idx),AttitudeActual.Roll(idx),'k',AttitudeActual.timestamp(idx),AttitudeActual.Pitch(idx),'k');
 169         subplot(312);
 170         plot(Gyros.timestamp(1:i), a(1:2,1:i)',Gyros.timestamp(1:i), ned(1:2,1:i)','k');
 171         subplot(313);
 172         plot(Gyros.timestamp(1:i), a(4:5,1:i)',Gyros.timestamp(1:i), gps_vel(1:2,1:i)','k');
 173         drawnow
 174     end
 175 end