matlab/revo/async_attitude.m

   1 baro = fixTime(BaroAltitude);
   2 accel = fixTime(Accels);
   3 attitude = fixTime(AttitudeActual);
   4
   5 %accel.z(end/2:end) = accel.z(end/2:end) + 1;
   6 accel.z = accel.z+2;
   7
   8 Gamma = diag([1e-15 1e-15 1e-3 1e-5]); % state noise
   9 accel_sigma = 10;
  10 baro_sigma = 0.1;
  11 Nu = diag([10 10 10 1000]);
  12 z = zeros(length(accel.z),4);
  13 Nu_n = zeros([4 4 length(accel.z)]);
  14 Nu_n(:,:,1) = Nu;
  15
  16 t = max(accel.timestamp(1), baro.timestamp(1));
  17 last_t = t-1;
  18 last_accel_idx = 1;
  19 last_baro_idx = 1;
  20 i = 1;
  21 timestamp = [];
  22
  23 z(1) = baro.Altitude(1);
  24 z(1:5,4) = 0;
  25 timestamp(1) = t;
  26 log_accel = 0;
  27 while (last_accel_idx + 1) <= length(accel.z) && (last_baro_idx + 1) <= length(baro.Altitude)
  28
  29     update_baro = baro.timestamp(last_baro_idx + 1) < t;
  30     update_accel = accel.timestamp(last_accel_idx + 1) < t;
  31
  32     if 0 && update_accel
  33         [~,idx] = min(abs(attitude.timestamp - accel.timestamp(last_accel_idx+1)));
  34         Rbe = Quat2Rbe([attitude.q1(idx), attitude.q2(idx), attitude.q3(idx), attitude.q4(idx)]);
  35         idx = last_accel_idx + 1;
  36         accel_ned = Rbe * [accel.x(idx); accel.y(idx); accel.z(idx)];
  37         accel_ned = accel_ned(3);
  38 %         if(abs(accel_ned) < 1e-1)
  39 %             keyboard
  40 %         end
  41     else
  42         accel_ned = accel.z(last_accel_idx + 1);
  43     end
  44     log_accel(i) = accel_ned;
  45     if update_baro && update_accel;
  46         x = [baro.Altitude(last_baro_idx + 1); -accel_ned-9.81];
  47         last_baro_idx = last_baro_idx + 1;
  48         last_accel_idx = last_accel_idx + 1;
  49         C = [1 0 0 0; 0 0 1 1];
  50         Sigma = diag([baro_sigma; accel_sigma]);
  51     elseif update_accel
  52         x =  -accel_ned - 9.81;
  53         last_accel_idx = last_accel_idx + 1;
  54         C = [0 0 1 1];
  55         Sigma = [accel_sigma];
  56     elseif update_baro
  57         x = [baro.Altitude(last_baro_idx + 1)];
  58         last_baro_idx = last_baro_idx + 1;
  59         C = [1 0 0 0];
  60         Sigma = [baro_sigma];
  61     else
  62         % Take a timestep and look for advance
  63         t = t + 0.1;
  64         continue;
  65    end
  66     %[last_baro_idx last_accel_idx]
  67     t = max(baro.timestamp(last_baro_idx), accel.timestamp(last_accel_idx));
  68     dT = (t - last_t) / 1000;
  69     if(dT == 0)
  70         dT = 1.5 / 1000;
  71     end
  72     assert(dT ~= 0,'WTF');
  73     last_t = t;
  74
  75     i = i + 1;
  76
  77     A = [1 dT 0 0; 0 1 dT 0; 0 0 1 0; 0 0 0 1];
  78
  79     P = A * Nu * A' + Gamma;
  80     K = P*C'*(C*P*C'+Sigma)^-1;
  81
  82     z(i,:) = A * z(i-1,:)' + K * (x - C * A * z(i-1,:)');
  83     timestamp(i) = t;
  84     Nu = (eye(4) - K * C) * P;
  85     Nu_n(:,:,i) = Nu;
  86
  87     if mod(i,10000) == 0
  88         subplot(311)
  89         plot(baro.timestamp, baro.Altitude, '.', timestamp(1:i),z(1:i,1),'r','LineWidth',5)
  90         subplot(312)
  91         plot(timestamp(1:i),z(1:i,2),'k')
  92         subplot(313)
  93         plot(timestamp(1:i),z(1:i,3),'k');
  94         xlim(timestamp([1,i]))
  95         drawnow
  96     end
  97 end
  98
  99 subplot(311)
 100 plot(baro.timestamp, baro.Altitude, '.', timestamp(1:i),z(1:i,1),'r','LineWidth',5)
 101 subplot(312)
 102 plot(timestamp(1:i),z(1:i,2),'k')
 103 subplot(313)
 104 plot(timestamp(1:i),z(1:i,3),'k');
 105 xlim(timestamp([1,i]))
 106