matlab/revo/generate_altitude.m

   1 % Generate the symbolic code for the kalman filter on altitude
   2
   3 dT = sym('dT','real');
   4 A = [1 dT 0 0; 0 1 dT 0; 0 0 1 0; 0 0 0 1];
   5 %Nu = diag([sym('V[1]') sym('V[2]') sym('V[3]') sym('V[4]')]);
   6 %Nu = [sym('V[1][1]') 0              0              0; ...
   7 %      0              sym('V[2][2]') 0              0; ...
   8 %      0              0              sym('V[3][3]') sym('V[3][4]'); ...
   9 %      0              0              sym('V[4][3]') sym('V[4][4]')];
  10 Nu = [sym('V[1][1]') sym('V[1][2]') sym('V[1][3]') sym('V[1][4]'); ...
  11       sym('V[2][1]') sym('V[2][2]') sym('V[2][3]') sym('V[2][4]'); ...
  12       sym('V[3][1]') sym('V[3][2]') sym('V[3][3]') sym('V[3][4]'); ...
  13       sym('V[4][1]') sym('V[4][2]') sym('V[4][3]') sym('V[4][4]');];
  14
  15
  16 Gamma = diag([sym('G[1]') sym('G[2]') sym('G[3]') sym('G[4]')]);
  17 Sigma = diag([sym('S[1]') sym('S[2]')]);
  18 C = [1 0 0 0; 0 0 1 1];
  19 state = [sym('z[1]'); sym('z[2]'); sym('z[3]'); sym('z[4]')];
  20 measure = [sym('x[1]'); sym('x[2]')];
  21
  22 P = simplify(A * Nu * A' + Gamma);
  23 K = simplify(P*C'*(C*P*C'+Sigma)^-1);
  24
  25 % fill in the zeros from above equations to make next calculation sparse
  26 P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]') sym('P[1][4]'); ...
  27          sym('P[2][1]') sym('P[2][2]') sym('P[2][3]') sym('P[2][4]'); ...
  28          sym('P[3][1]') sym('P[3][2]') sym('P[3][3]') sym('P[3][4]'); ...
  29          sym('P[4][1]') sym('P[4][3]') sym('P[4][3]') sym('P[4][4]')];
  30 K_mat = [sym('K[1][1]') sym('K[1][2]'); ...
  31          sym('K[2][1]') sym('K[2][2]'); ...
  32          sym('K[3][1]') sym('K[3][2]'); ...
  33          sym('K[4][1]') sym('K[4][2]')];
  34
  35 z_new = A * state + K_mat * (measure - C * A * state);
  36 V = (eye(4) - K_mat * C) * P_mat;
  37
  38 ccode(P)
  39 ccode(K)
  40 ccode(z_new)
  41 ccode(V)
  42
  43
  44 %% For when there is no baro update
  45 % Generate the symbolic code for the kalman filter on altitude
  46 C = [0 0 1 1];
  47 Sigma = sym('S[2]');
  48 measure = [sym('x[2]')];
  49
  50 P = simplify(A * Nu * A' + Gamma);
  51 K = simplify(P*C'*(C*P*C'+Sigma)^-1);
  52
  53 % fill in the zeros from above equations to make next calculation sparse
  54 P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]') sym('P[1][4]'); ...
  55          sym('P[2][1]') sym('P[2][2]') sym('P[2][3]') sym('P[2][4]'); ...
  56          sym('P[3][1]') sym('P[3][2]') sym('P[3][3]') sym('P[3][4]'); ...
  57          sym('P[4][1]') sym('P[4][3]') sym('P[4][3]') sym('P[4][4]')];
  58 K_mat = [sym('K[1][1]'); ...
  59          sym('K[2][1]'); ...
  60          sym('K[3][1]'); ...
  61          sym('K[4][1]')];
  62
  63 z_new = A * state + K_mat * (measure - C * A * state);
  64 V = (eye(4) - K_mat * C) * P_mat;
  65
  66 ccode(P)
  67 ccode(K)
  68 ccode(z_new)
  69 ccode(V)