adsb_track_smoothing_filter.m

%******************************************************************************
% \details     : 
% \autor       : Simon Burkhardt
% \file        : adsb_track_smoothing_filter.m
% \date        : 2021-01-04
% \version     : 1.0
%******************************************************************************
clear all; close all; clc; format long;

% https://www.usna.edu/Users/oceano/pguth/md_help/html/approx_equivalents.htm
%
%     1° = 111 km  (or 60 nautical miles)
%     0.1° = 11.1 km
%     0.01° = 1.11 km (2 decimals, km accuracy)
%     0.001° =111 m
%     0.0001° = 11.1 m
%     0.00001° = 1.11 m
%     0.000001° = 0.11 m (7 decimals, cm accuracy)
% 
%     1' = 1.85 km  (or 1 nautical mile)
%     0.1' = 185 m
% 
%     3" = 90 m
%     1" = 30 m
%     1/3" = 10 m
%     1/27" = 1 m

% data is accurate to 0.00001° = 1.11m
Track=[45.80917,4.483795,46.323532,4.366998,46.328796,4.365257,46.374409,4.347427,46.37915,4.345294,48.312607,3.312308,48.316681,3.309702,48.344547,3.289899,48.348503,3.286791,48.377563,3.261249,48.383228,3.255784,50.113541,1.284461,50.116184,1.281318,50.120224,1.27651,50.127216,1.268104,50.13015,1.264615,50.133827,1.260236,50.136383,1.25714,50.140778,1.251935,50.142975,1.249333,50.148056,1.24319,50.189644,1.193575,50.194576,1.187637,50.197128,1.184572,50.199608,1.181624,50.201748,1.179026,50.211662,1.167149,50.2159,1.162101,50.221531,1.155346,50.233498,1.140945,50.235592,1.138421,50.238384,1.135081,50.240982,1.131953,50.273438,1.092991,50.27623,1.089651,50.279343,1.085912,50.281864,1.082821,50.287312,1.076363,50.291107,1.071817,50.296295,1.065525,50.298389,1.063001,50.300125,1.060976,50.415981,0.919585,50.419006,0.915206,50.426407,0.903254,50.428596,0.899468,50.435299,0.886701,50.43718,0.882882,50.442375,0.871112,50.444099,0.866806,50.448845,0.853593,50.450134,0.849633,50.453968,0.836371,50.454945,0.832586,50.461899,0.800629,50.462814,0.796003,50.487438,0.655171,50.48822,0.64935,50.506577,0.474436,50.507904,0.46504,50.513927,0.430842,50.514496,0.428105,50.522186,0.395652,50.523376,0.391316,50.857864,-0.707282,50.860741,-0.714111,50.862579,-0.717526,50.86441,-0.72057,50.866512,-0.723648,50.86734,-0.724726,50.869957,-0.727768,50.870705,-0.728455,50.87352,-0.730888,50.874664,-0.731779,50.877777,-0.733857,50.879173,-0.734634,50.887928,-0.739517,50.891094,-0.741119,51.008915,-0.796432,51.012314,-0.797119,51.0159,-0.797501,51.018318,-0.797501,51.020367,-0.797424,51.024372,-0.79689,51.028328,-0.796051,51.030704,-0.795593,51.099197,-0.779584,51.100487,-0.779419,51.101974,-0.779495,51.102951,-0.779584,51.103882,-0.7798,51.109268,-0.782182,51.121574,-0.802307,51.123154,-0.809555,51.133675,-0.881271,51.134262,-0.885439,51.135223,-0.892342,51.136917,-0.904665,51.137375,-0.907708,51.150192,-0.933467,51.151337,-0.93458,51.179714,-0.961609,51.188049,-0.967865,51.193356,-0.96817,51.195923,-0.967242,51.197708,-0.966351,51.202194,-0.963605,51.202854,-0.963135,51.209881,-0.957947,51.21114,-0.956726,51.211761,-0.956107,51.212494,-0.955217,51.227989,-0.923157];
Track=[47.283298,8.2974,47.2831,8.297,47.2869,8.2944,47.293701,8.2896,47.294899,8.2885,47.302101,8.2871,47.3032,8.2877,47.304401,8.288,47.305199,8.2894,47.306499,8.2912,47.307899,8.2926,47.309101,8.2942,47.309601,8.2988,47.309299,8.3012,47.309502,8.3045,47.3092,8.3077,47.309799,8.3084,47.309898,8.3112,47.309898,8.3128,47.310299,8.3141,47.310299,8.3177,47.310398,8.3202,47.310699,8.3218,47.3102,8.323,47.310299,8.3255,47.3106,8.3283,47.310799,8.3297,47.311199,8.3324,47.311001,8.3356,47.310799,8.3375,47.310799,8.3401,47.310799,8.3416,47.310699,8.3436,47.310699,8.3468,47.311001,8.3477,47.310299,8.3511,47.310699,8.3541,47.309898,8.357,47.309799,8.3584,47.310001,8.3607,47.309502,8.3619,47.3092,8.3655,47.3092,8.3681,47.3083,8.3717,47.308399,8.3744,47.308701,8.3769,47.3078,8.3784,47.307999,8.3805,47.307201,8.3861,47.3069,8.3889,47.306599,8.391,47.307098,8.3923,47.306801,8.3955,47.306499,8.3971,47.305801,8.403,47.305599,8.4062,47.304001,8.416,47.304401,8.4174,47.3036,8.4193,47.303299,8.4214,47.302299,8.4273,47.3018,8.4291,47.301498,8.4317,47.301899,8.4336,47.301701,8.4356,47.301399,8.4374,47.301201,8.4389,47.3008,8.4407,47.2995,8.4428,47.2994,8.4471,47.2995,8.4487,47.299702,8.4516,47.299301,8.4548,47.297901,8.457,47.298401,8.4591,47.298698,8.4609,47.2985,8.4623,47.298199,8.4649,47.298302,8.467,47.298,8.4694,47.297699,8.4712,47.2971,8.4727,47.297001,8.4747,47.2966,8.4765,47.296101,8.4784,47.294701,8.482,47.2948,8.4844,47.294399,8.4854,47.294498,8.4868,47.2934,8.4892,47.294201,8.4914,47.292702,8.494,47.293098,8.4959,47.291599,8.4975,47.291801,8.4985,47.291302,8.5008,47.291,8.5032,47.2901,8.5065,47.288898,8.5134,47.288399,8.5142,47.2878,8.517,47.287998,8.5179,47.286999,8.5242,47.286499,8.5256,47.285702,8.5271,47.2854,8.5287,47.284599,8.5316,47.283798,8.5322,47.283401,8.5351,47.288502,8.5357,47.2817,8.5381,47.280701,8.5403,47.279598,8.5416,47.276699,8.5461,47.275799,8.547,47.274101,8.5492,47.273201,8.5511,47.272499,8.5522,47.268902,8.5586,47.266201,8.5593,47.2659,8.5616,47.2645,8.5645,47.262199,8.5679,47.263599,8.5767,47.260502,8.5708,47.259899,8.5726,47.257801,8.5734,47.257198,8.5746,47.2565,8.5768,47.255402,8.5778,47.254398,8.5785,47.253201,8.5813,47.252102,8.5819,47.250801,8.5844,47.249599,8.5852,47.2495,8.5864,47.247898,8.5884,47.250198,8.5981,47.245602,8.5907,47.244202,8.5935,47.243198,8.5944,47.243,8.5969,47.241798,8.5975,47.240799,8.5991,47.2402,8.6019,47.238499,8.6042,47.238499,8.6064,47.237099,8.6077,47.236,8.6098,47.2351,8.6117,47.2342,8.613,47.233002,8.6147,47.233002,8.6181,47.230701,8.6188,47.2299,8.6206,47.2295,8.6232,47.227299,8.6246,47.226398,8.6277,47.225201,8.6275,47.2243,8.63,47.2239,8.6331,47.2215,8.6341,47.2248,8.6343,47.2206,8.6366,47.216301,8.6418,47.216499,8.6456,47.2225,8.6446,47.215,8.6521,47.215801,8.6516,47.212799,8.6537,47.212002,8.6566,47.211102,8.6577,47.210701,8.6587,47.204102,8.6629,47.210999,8.6631,47.206001,8.6631,47.204102,8.6628,47.2164,8.6633,47.204899,8.6707,47.202599,8.6716,47.199902,8.6729,47.199001,8.6737,47.210999,8.6733,47.210701,8.6738,47.198799,8.6833,47.198101,8.6868,47.198101,8.6878,47.194599,8.6916,47.193802,8.6933,47.1903,8.702,47.192699,8.6982,47.205101,8.6925,47.213699,8.6784,47.192001,8.7085,47.1894,8.7099,47.188301,8.7142,47.187099,8.7161,47.189499,8.7202,47.186699,8.7224,47.184502,8.7248,47.187801,8.7284,47.1936,8.7279,47.204498,8.72,47.1819,8.7437,47.186901,8.7427,47.208698,8.7226,47.1987,8.7369,47.1884,8.7507,47.179699,8.7659,47.194801,8.7496,47.171398,8.778,47.192501,8.7641,47.200298,8.7538,47.197601,8.7628,47.1931,8.7679,47.174702,8.7948,47.173901,8.7965,47.1856,8.7808,47.168598,8.8061,47.173901,8.8068,47.180302,8.7962,47.1842,8.7988,47.191799,8.7908,47.1922,8.791,47.1731,8.8148,47.136902,8.8719,47.176701,8.8224,47.1772,8.8251,47.1721,8.8383,47.162998,8.8505,47.1838,8.8231,47.177898,8.8269,47.178398,8.8281,47.184799,8.8272,47.197102,8.8027,47.165401,8.8659,47.194199,8.8169,47.1824,8.8406,47.1731,8.8635,47.181702,8.8532,47.212502,8.7613,47.1866,8.863];
lat = Track(1:2:end);
lon = Track(2:2:end);

% "sample rate" of ADSB is 1Hz (1 position per second)
d1 = designfilt('lowpassfir','FilterOrder',32, ...
    'HalfPowerFrequency',0.12);

lon2 = filtfilt(d1,lon);
lat2 = filtfilt(d1,lat);

lonm = lon./0.00001.*1.11;
latm = lat./0.00001.*1.11;
lon2m = lon2./0.00001.*1.11;
lat2m = lat2./0.00001.*1.11;

lonm = lonm - median(lonm);
latm = latm - median(latm);
lon2m = lon2m - median(lon2m);
lat2m = lat2m - median(lat2m);

%%
% plot in Degrees
plot(lon, lat, '-')
hold on
grid on
axis equal
plot(lon2, lat2, '-')

%%
% plot in meters
plot(lonm, latm, '-')
hold on
grid on
axis equal
plot(lon2m, lat2m, '-', 'LineWidth', 2)
xlabel("[m]")
ylabel("[m]")

%%
% Error in meters

error_lat = (lat2-lat)./0.00001.*1.11;
error_lon = (lon2-lon)./0.00001.*1.11;

plot(error_lat); hold on
plot(error_lon); grid on

%%
% Estimate if Filter is necessary by using the Spectrum
% snr() does not work as there is no fundamental Frequency
spec_lat = abs(fft(latm));
spec_lat2 = abs(fft(lat2m));
midpoint = floor(length(latm)/2);

plot(20.*log10(spec_lat (1:midpoint))); hold on;
plot(20.*log10(spec_lat2(1:midpoint))); grid on;

snr_lat = snr(latm);
snr_lat2 = snr(lat2m);

legend(num2str(snr_lat), num2str(snr_lat2))
title("Spectrum and SNR of Latitude")
ylabel("dB")
xlabel("Frequency")