matlab/ins/LLA2NED_symbolic.m

   1 function NED = LLA2NED_symbolic(lla, home)
   2
   3 DEG2RAD = pi / 180;
   4 a = 6378137.0;              % Equatorial Radius
   5 e = 8.1819190842622e-2; % Eccentricity
   6
   7 lat = home(1) / 10e6 * DEG2RAD;
   8 lon = home(2) / 10e6 * DEG2RAD;
   9 alt = home(3);
  10
  11 delta = (lla - home);
  12 delta(1:2) = delta(1:2) / 10e6 * DEG2RAD;
  13 delta = reshape(delta,[],1);
  14
  15 N = sym('a / sqrt(1.0 - e * e * sin(lat) * sin(lat))'); %prime vertical radius of curvature
  16
  17 ECEF = [sym('(N + alt) * cos(lat) * cos(lon)'); ...
  18  sym('(N + alt) * cos(lat) * sin(lon)'); ...
  19  sym('((1 - e * e) * N + alt) * sin(lat)')];
  20
  21 ECEF = subs(ECEF, 'N', N);
  22 ECEF = subs(ECEF, 'e', 0);
  23 ECEF = subs(ECEF, 'a', a);
  24
  25 J = [diff(ECEF,'lat') diff(ECEF,'lon') diff(ECEF,'alt')];
  26
  27 Rne(1,1) = sym('-sin(lat) * cos(lon)');
  28 Rne(1,2) = sym('-sin(lat) * sin(lon)');
  29 Rne(1,3) = sym('cos(lat)');
  30 Rne(2,1) = sym('-sin(lon)');
  31 Rne(2,2) = sym('cos(lon)');
  32 Rne(2,3) = 0;
  33 Rne(3,1) = sym('-cos(lat) * cos(lon)');
  34 Rne(3,2) = sym('-cos(lat) * sin(lon)');
  35 Rne(3,3) = sym('-sin(lat)');
  36
  37 ccode(simplify(Rne * J))
  38
  39 NEDsymb = simplify(Rne * J) * delta;
  40 NED = subs(subs(subs(NEDsymb,'lat',lat),'lon',lon),'alt',alt);
  41 %delta = subs(subs(subs(J * delta,'lat',lat),'lon',lon),'alt',alt)
  42 %Rne = subs(subs(subs(Rne,'lat',lat),'lon',lon),'alt',alt)