Closed hcab14 closed 5 years ago
Hi Christoph. Top-notch work as always. I've known the term Kalman filtering for a long time but have never looked into the details.
This looks like a bug in the GPS code as the time offsets are all exactly 20ms (with relative precision of < 5e-6)
I would like to know more about this. Do you mean the outliers are all multiples of 20ms? And are these the individual uncorrected PRN times? i.e. t_tx[] I looked into the contributions of the return value of GetClock() but didn't see an obvious problem. Even for values where my clock_correction() routine determines there is an outlier. The return value would be a multiple of 20ms if both "chips" and "ca_phase" were zero. But this doesn't seem to happen.
Hi John,
I would like to know more about this. Do you mean the outliers are all multiples of 20ms?
The difference between the outlier values and the expected values are multiples of 20ms
And are these the individual uncorrected PRN times? i.e. t_tx[]
Yes.
I looked into the contributions of the return value of GetClock() but didn't see an obvious problem. Even for values where my clock_correction() routine determines there is an outlier.
When an outlier is present, the position solution fails, so you will probably not see it in clock_correction(). You should be able to see these outliers in t_tx[] in epochs with >=4 satellites in which the position solution fails.
Below are all the outliers I found in the 10hours of data: z ... t_tx (sec) zp ... predicted t_tx (sec) dz=z-zp
Except in one case (marked with **), dz is a multiple of 0.02 seconds. As 0.02 sec = 1/50Hz I suspect that these outliers come from the contribution of bits / BPS
to t_tx.
outlier idx= 0 z=0.097945745 zp=0.077945701 dz=0.020000045 outlier idx= 3 z=0.093809096 zp=0.073809110 dz=0.019999986 outlier idx= 3 z=0.093973261 zp=0.073973199 dz=0.020000062 outlier idx= 3 z=0.093994771 zp=0.073994768 dz=0.020000003 outlier idx= 3 z=1.213997274 zp=0.073995905 dz=1.140001370 outlier idx= 3 z=0.094196655 zp=0.074196567 dz=0.020000089 outlier idx= 3 z=0.114218458 zp=0.074218374 dz=0.040000084 outlier idx= 3 z=0.094563995 zp=0.074563893 dz=0.020000102 outlier idx= 1 z=0.092564474 zp=0.072564498 dz=0.019999976 outlier idx= 1 z=0.132999413 zp=0.072999346 dz=0.060000067 outlier idx= 1 z=0.133568672 zp=0.073568520 dz=0.060000152 outlier idx= 0 z=0.115768668 zp=0.075768636 dz=0.040000032 outlier idx= 0 z=3.335774554 zp=0.075768581 dz=3.260005973 outlier idx= 0 z=0.095827999 zp=0.075828007 dz=0.019999992 outlier idx= 0 z=0.115833974 zp=0.075833954 dz=0.040000020 outlier idx= 0 z=4.775840008 zp=0.075831256 dz=4.700008753 outlier idx= 4 z=0.116575210 zp=0.076575279 dz=0.039999932 outlier idx= 4 z=3.096564977 zp=0.076573506 dz=3.019991471 outlier idx= 4 z=7.976554667 zp=0.076577080 dz=7.899977587 outlier idx= 4 z=0.096270285 zp=0.076270316 dz=0.019999968 outlier idx= 2 z=0.087745645 zp=0.067745593 dz=0.020000052 outlier idx= 3 z=0.087699058 zp=0.067699090 dz=0.019999968 outlier idx= 4 z=0.135828212 zp=0.075828340 dz=0.059999872 outlier idx= 5 z=0.100929845 zp=0.080929885 dz=0.019999960 outlier idx= 4 z=0.095589491 zp=0.075589644 dz=0.019999847 outlier idx= 2 z=0.087935245 zp=0.067935156 dz=0.020000089 outlier idx= 3 z=0.092224149 zp=0.072224113 dz=0.020000036 outlier idx= 3 z=0.092207493 zp=0.072207532 dz=0.019999962 outlier idx= 5 z=0.099451580 zp=0.079451619 dz=0.019999960 outlier idx= 5 z=0.099262274 zp=0.079262323 dz=0.019999950 outlier idx= 1 z=0.097466677 zp=0.077466677 dz=0.020000000 outlier idx= 1 z=0.097756990 zp=0.077756939 dz=0.020000052 outlier idx= 4 z=0.098677617 zp=0.078677658 dz=0.019999958 outlier idx= 0 z=0.098154831 zp=0.078154807 dz=0.020000025 outlier idx= 0 z=0.098334383 zp=0.078334318 dz=0.020000064 outlier idx= 0 z=0.098450984 zp=0.078451008 dz=0.019999975 outlier idx= 3 z=0.097773005 zp=0.077773016 dz=0.019999989 outlier idx= 0 z=0.117358765 zp=0.077358956 dz=0.039999809 outlier idx= 0 z=0.097078465 zp=0.077078557 dz=0.019999908 outlier idx= 1 z=0.089944859 zp=0.069944863 dz=0.019999996 outlier idx= 0 z=0.096939272 zp=0.076939316 dz=0.019999956 outlier idx= 0 z=0.136800580 zp=0.076800724 dz=0.059999856 outlier idx= 1 z=0.088204789 zp=0.068204694 dz=0.020000095 outlier idx= 0 z=0.099870989 zp=0.079871013 dz=0.019999976 outlier idx= 0 z=0.099529767 zp=0.079529880 dz=0.019999888 outlier idx= 0 z=0.599518757 zp=0.079520398 dz=0.519998359 outlier idx= 3 z=0.088414433 zp=0.068414387 dz=0.020000046 outlier idx= 0 z=0.098909155 zp=0.078909159 dz=0.019999996 outlier idx= 0 z=0.098510502 zp=0.078510467 dz=0.020000035 outlier idx= 0 z=0.098499559 zp=0.067601690 dz=0.030897869 outlier idx= 0 z=3.718488969 zp=0.067602243 dz=3.650886726 ** outlier idx= 0 z=0.078467499 zp=0.067606258 dz=0.010861241 outlier idx= 0 z=0.078456622 zp=0.067607806 dz=0.010848816 outlier idx= 0 z=0.078445854 zp=0.067609365 dz=0.010836489 outlier idx= 2 z=0.097166403 zp=0.077166352 dz=0.020000051 outlier idx= 2 z=0.097168340 zp=0.077168303 dz=0.020000037 outlier idx= 0 z=0.097526629 zp=0.077526672 dz=0.019999958 outlier idx= 2 z=0.087800141 zp=0.067800214 dz=0.019999927 outlier idx= 2 z=0.097417080 zp=0.077417029 dz=0.020000051 outlier idx= 2 z=0.097508969 zp=0.077508973 dz=0.019999996 outlier idx= 3 z=0.087700679 zp=0.067700680 dz=0.019999999 outlier idx= 3 z=0.087692081 zp=0.067692059 dz=0.020000022 outlier idx= 4 z=0.087645564 zp=0.067645509 dz=0.020000055 outlier idx= 4 z=0.094399141 zp=0.074399099 dz=0.020000042 outlier idx= 3 z=0.094222249 zp=0.074222234 dz=0.020000015 outlier idx= 3 z=0.093964011 zp=0.073964043 dz=0.019999968 outlier idx= 4 z=0.093652266 zp=0.073652313 dz=0.019999953 outlier idx= 3 z=0.093369262 zp=0.073369339 dz=0.019999923 outlier idx= 3 z=0.113234499 zp=0.073234600 dz=0.039999899 outlier idx= 1 z=0.089263452 zp=0.069263477 dz=0.019999975 outlier idx= 2 z=0.113082793 zp=0.073082813 dz=0.039999980 outlier idx= 2 z=0.153078597 zp=0.073078669 dz=0.079999928 outlier idx= 1 z=0.109116789 zp=0.069116853 dz=0.039999936 outlier idx= 3 z=0.092798754 zp=0.072798724 dz=0.020000030 outlier idx= 1 z=0.089046933 zp=0.069046824 dz=0.020000109 outlier idx= 1 z=0.089037383 zp=0.069037450 dz=0.019999933 outlier idx= 2 z=0.092667648 zp=0.072667645 dz=0.020000002 outlier idx= 2 z=0.092561744 zp=0.072561679 dz=0.020000065 outlier idx= 2 z=0.112411662 zp=0.072411758 dz=0.039999905 outlier idx= 1 z=0.134611985 zp=0.074611863 dz=0.060000122 outlier idx= 1 z=0.114773324 zp=0.074773224 dz=0.040000100 outlier idx= 1 z=0.092290319 zp=0.072290341 dz=0.019999979 outlier idx= 1 z=0.089385802 zp=0.069385714 dz=0.020000088 outlier idx= 1 z=0.109399177 zp=0.069399068 dz=0.040000109 outlier idx= 1 z=0.749399562 zp=0.069399475 dz=0.680000087 outlier idx= 1 z=5.869400103 zp=0.069399127 dz=5.800000976 outlier idx= 4 z=0.088954628 zp=0.068954655 dz=0.019999974 outlier idx= 1 z=0.089587298 zp=0.069587328 dz=0.019999970 outlier idx= 1 z=0.089615865 zp=0.069615901 dz=0.019999964 outlier idx= 2 z=0.097284889 zp=0.077284800 dz=0.020000089 outlier idx= 2 z=0.197398256 zp=0.077397925 dz=0.120000330 outlier idx= 2 z=0.097645499 zp=0.077645464 dz=0.020000035 outlier idx= 1 z=0.089776306 zp=0.069776273 dz=0.020000033 outlier idx= 1 z=0.089840727 zp=0.069840683 dz=0.020000043 outlier idx= 3 z=0.092741442 zp=0.072741403 dz=0.020000038 outlier idx= 1 z=0.089904654 zp=0.069904620 dz=0.020000034 outlier idx= 3 z=0.089649678 zp=0.069649610 dz=0.020000068 outlier idx= 3 z=0.092924081 zp=0.072923987 dz=0.020000093 outlier idx= 0 z=0.090021454 zp=0.070021500 dz=0.019999954 outlier idx= 2 z=0.092985727 zp=0.072985681 dz=0.020000045 outlier idx= 0 z=0.089612396 zp=0.069612408 dz=0.019999988 outlier idx= 0 z=0.089597756 zp=0.069597692 dz=0.020000064 outlier idx= 0 z=0.089579509 zp=0.069579523 dz=0.019999985 outlier idx= 0 z=0.089564064 zp=0.069564068 dz=0.019999996 outlier idx= 0 z=0.089558569 zp=0.069558444 dz=0.020000125 outlier idx= 3 z=0.094251142 zp=0.074251191 dz=0.019999951 outlier idx= 0 z=0.089510555 zp=0.069510590 dz=0.019999965 outlier idx= 2 z=0.094085708 zp=0.074085771 dz=0.019999937 outlier idx= 0 z=0.100112893 zp=0.080112720 dz=0.020000173 outlier idx= 1 z=0.113827583 zp=0.073827705 dz=0.039999878 outlier idx= 1 z=0.093623591 zp=0.073623660 dz=0.019999931 outlier idx= 0 z=0.091261142 zp=0.071261175 dz=0.019999968 outlier idx= 1 z=0.103027894 zp=0.083027841 dz=0.020000053 outlier idx= 0 z=0.091950093 zp=0.071950047 dz=0.020000046 outlier idx= 0 z=1.211955082 zp=0.071953457 dz=1.140001625 outlier idx= 0 z=0.093105775 zp=0.073105839 dz=0.019999936 outlier idx= 0 z=0.099284645 zp=0.079284711 dz=0.019999934 outlier idx= 0 z=0.133217738 zp=0.073217589 dz=0.060000149 outlier idx= 0 z=0.098904069 zp=0.078904146 dz=0.019999924 outlier idx= 1 z=0.118832366 zp=0.078832492 dz=0.039999875 outlier idx= 1 z=0.098678956 zp=0.078679075 dz=0.019999880 outlier idx= 0 z=0.178343124 zp=0.078343473 dz=0.099999651 outlier idx= 1 z=0.117849162 zp=0.077849153 dz=0.040000009 outlier idx= 1 z=0.088734252 zp=0.068734177 dz=0.020000075 outlier idx= 1 z=0.088668094 zp=0.068668161 dz=0.019999933 outlier idx= 1 z=0.088627843 zp=0.068627905 dz=0.019999937 outlier idx= 1 z=0.088584884 zp=0.068584944 dz=0.019999940 outlier idx= 2 z=0.088540312 zp=0.068540336 dz=0.019999976 outlier idx= 2 z=0.087592143 zp=0.067592169 dz=0.019999974 outlier idx= 2 z=0.087693288 zp=0.067693234 dz=0.020000055 outlier idx= 0 z=0.087797507 zp=0.067797415 dz=0.020000092 outlier idx= 0 z=0.087812320 zp=0.067812285 dz=0.020000034 outlier idx= 1 z=0.088451735 zp=0.068451693 dz=0.020000041 outlier idx= 1 z=0.089155129 zp=0.069155134 dz=0.019999995 outlier idx= 1 z=0.089077860 zp=0.069077891 dz=0.019999969 outlier idx= 1 z=0.089043047 zp=0.069043097 dz=0.019999950 outlier idx= 1 z=0.228995009 zp=0.068995201 dz=0.159999809 outlier idx= 1 z=0.096394410 zp=0.076394514 dz=0.019999896 outlier idx= 1 z=0.096111685 zp=0.076111831 dz=0.019999854 outlier idx= 1 z=0.095820420 zp=0.075820525 dz=0.019999894 outlier idx= 1 z=0.088702414 zp=0.068702520 dz=0.019999894 outlier idx= 1 z=0.148690639 zp=0.068690678 dz=0.079999960 outlier idx= 1 z=0.093514406 zp=0.073514452 dz=0.019999954 outlier idx= 1 z=0.113504477 zp=0.073504630 dz=0.039999846 outlier idx= 1 z=0.116418441 zp=0.076418602 dz=0.039999839 outlier idx= 2 z=0.096037913 zp=0.076038007 dz=0.019999906 outlier idx= 2 z=0.115952892 zp=0.075953052 dz=0.039999840 outlier idx= 1 z=0.092625082 zp=0.072625129 dz=0.019999953 outlier idx= 1 z=0.092549682 zp=0.072549807 dz=0.019999875 outlier idx= 2 z=0.128831227 zp=0.068831267 dz=0.059999960 outlier idx= 2 z=0.093114757 zp=0.073114773 dz=0.019999984 outlier idx= 1 z=0.092465398 zp=0.072465450 dz=0.019999948 outlier idx= 2 z=0.092784451 zp=0.072784426 dz=0.020000026 outlier idx= 1 z=0.151877284 zp=0.071877510 dz=0.079999774 outlier idx= 1 z=0.091724968 zp=0.071724973 dz=0.019999995 outlier idx= 1 z=0.091600705 zp=0.071600752 dz=0.019999953 outlier idx= 2 z=0.134463676 zp=0.074463859 dz=0.059999816 outlier idx= 2 z=0.094179760 zp=0.074179746 dz=0.020000015 outlier idx= 0 z=0.113875897 zp=0.073875800 dz=0.040000097 outlier idx= 3 z=0.114283080 zp=0.074282998 dz=0.040000082 outlier idx= 2 z=0.100756288 zp=0.080756258 dz=0.020000030 outlier idx= 4 z=0.094825955 zp=0.074825987 dz=0.019999968 outlier idx= 4 z=0.096486586 zp=0.076486542 dz=0.020000044 outlier idx= 4 z=0.096582846 zp=0.076582820 dz=0.020000025 outlier idx= 2 z=0.092142652 zp=0.072142628 dz=0.020000024 outlier idx= 4 z=0.116785811 zp=0.076785619 dz=0.040000192 outlier idx= 2 z=0.092009433 zp=0.072009457 dz=0.019999976 outlier idx= 1 z=0.087815154 zp=0.067815195 dz=0.019999959 outlier idx= 1 z=0.087725316 zp=0.067725416 dz=0.019999900 outlier idx= 2 z=0.100992056 zp=0.080992082 dz=0.019999974 outlier idx= 1 z=0.087664880 zp=0.067664861 dz=0.020000019 outlier idx= 2 z=0.131735941 zp=0.071735940 dz=0.060000001 outlier idx= 2 z=3.671732062 zp=0.071732866 dz=3.599999196 outlier idx= 1 z=0.087614544 zp=0.067614413 dz=0.020000131 outlier idx= 2 z=0.101054345 zp=0.081054419 dz=0.019999926 outlier idx= 1 z=0.167430607 zp=0.067430754 dz=0.099999853 outlier idx= 2 z=0.091028963 zp=0.071028951 dz=0.020000013 outlier idx= 2 z=0.091031309 zp=0.071031313 dz=0.019999996 outlier idx= 4 z=0.091344601 zp=0.071344581 dz=0.020000019 outlier idx= 4 z=0.111374163 zp=0.071374033 dz=0.040000130 outlier idx= 5 z=0.091472462 zp=0.071472388 dz=0.020000073 outlier idx= 5 z=0.091484655 zp=0.071484673 dz=0.019999982
Okay, I understand now -- thanks. The outliers I was referring to occur in the clock_correction() routine when the newly computed ADC clock value is outside the ~50 ppm window. This happens because the t_rx of the last GPS solution has an unexpected value. And if you print the lat/lon for the last solution the latitude is usually fine but the longitude is typically way off (tens of degrees).
So I wasn't looking at t_tx[] for failed position solutions. Let me see what I can find..
Here is some more information about the extended Kalman filter:
t_tx
measurements are detected and removed by the filterBelow is the filter output (in red) and the single point position solver output (blue) for the same data used for the plots in the 1st comment, now with adjusted process noise covariance: As you can see, the Kalman filter is able to provide updates even when there are less than four satellites available.
And here is the filter output for another set of data where a GPS week roll-over occurs
As you can see GPS reception is marginal with my setup but using the Kalman filter almost 100% efficiency can be reached.
As I have mentioned before I use the Eigen library for linear algebra. One possibility is to have it as a git submodule in Beagle_SDR_GPS/pkgs, see https://github.com/hcab14/Beagle_SDR_GPS (work in progress). Let me know what you think.
Hi Christoph.
Very sorry for the delayed reply. Lots of things going on, not all Kiwi related.
Unfortunately, my understanding of the GPS solution algorithm (MLAT?) is very limited. Can you clear up some terminology for me? Does SPP refer to the classic solution obtained with four or more sats? I understand how application of an EKF could help smooth out the variations in SPP solutions. What I don’t understand is how you can continue to generate valid position solutions based on subsequent pseudo-ranges from less than 4 sats (or even just one) in the intervening times between SPPs.
I think it’s fine to use the Eigen lib. My only concern is about referring to that package as a submodule within the Kiwi project instead of including a “snapshot” of it in the pkgs directory. Since all Kiwis get their updates from github if the Eigen guys make a faulty commit I wouldn’t want that to brick all Kiwis worldwide on the next Kiwi update. But I don’t understand the details of how git submodules work exactly.
Also, the code you added in your latest commit to your fork didn't seem to be called from gps/solve.cpp:Solve() anywhere. Was it just added in preparation for eventual integration? I guess so since you said "work in progress".
Unfortunately, my understanding of the GPS solution algorithm (MLAT?) is very limited. Can you clear up some terminology for me? Does SPP refer to the classic solution obtained with four or more sats?
Yes.
I understand how application of an EKF could help smooth out the variations in SPP solutions. What I don’t understand is how you can continue to generate valid position solutions based on subsequent pseudo-ranges from less than 4 sats (or even just one) in the intervening times between SPPs.
One way of looking at it is the following: suppose that the receiver position is known, then only the receiver time is unknown and for this one satellite is enough.
The EKF update step combines the knowledge about the filter state with the new measurements into a new state in two steps:
I think it’s fine to use the Eigen lib. My only concern is about referring to that package as a submodule within the Kiwi project instead of including a “snapshot” of it in the pkgs directory. Since all Kiwis get their updates from github if the Eigen guys make a faulty commit I wouldn’t want that to brick all Kiwis worldwide on the next Kiwi update. But I don’t understand the details of how git submodules work exactly.
I have to read the git submodule documentation in more detail but seem to recall that the submodule points to a specific commit. It still might be more safe make a snapshot in case the Eigen repository gets deleted or changes its name.
Also, the code you added in your latest commit to your fork didn't seem to be called from gps/solve.cpp:Solve() anywhere. Was it just added in preparation for eventual integration? I guess so since you said "work in progress".
Yes it is not yet integrated into the KiwiSDR software - I did not have time for this yet
One way of looking at it is the following: suppose that the receiver position is known, then only the receiver time is unknown and for this one satellite is enough.
Okay, so this is like the "position and hold" mode of GPS timing receivers where the user position becomes a constant.
... but seem to recall that the submodule points to a specific commit.
I see that now. That would work fine and makes it easier to switch to a new commit if they fix an essential bug.
I am still exploring the issue, but I think I may have found the cause of the occasional large outlier you noted above.
Andrew’s code takes care to “snapshot” the correct sat ephemeris data when the reception times for all sats are being computed. The largest contributor to the reception time is of course the TOW (time-of-week, in seconds) which is part of the ephemerides. These two processes, the decoding of the subframe data for each sat which stores the most recent TOW and the atomic computing of the reception time of all sats for each computed solution, are implemented as two separate asynchronous tasks (actually the processing of subframe data is a separate task per sat due to the bit-level processing involved). Therefore there has to be careful synchronization between them. If there is a problem updating the TOW for a sat, such as due to a parity error in the decode, the reception time code must be aware of that fact and prevent the sat from participating in the current positioning solution. Andrew’s code handles this case properly.
What seems not to be handled is the case where there is an error in the received 8-bit sync word (8 bits out of 300 per subframe) that causes the entire subframe not to be detected and hence no parity check even attempted. In this case the reception time code will use the TOW stored in the ephemerides from the last correctly decoded subframe which of course would lead to a huge error in the computed time.
Example: Suppose subframe 2 is decoded correctly and its TOW stored in the ephemerides. And then an asynchronous solution is attempted while subframe 3 is in the process of being received and decoded (NB: the solution task is run every 5 seconds and a subframe takes 6 seconds to transmit, i.e. 300 bits @50bps). This is the normal case and the code works fine. The reception time code uses the subframe 2 TOW and adds to it the exact amount of time that the sat has advanced into transmitting subframe 3 (by looking at how many bits have been received, the current position of the code generator in the FPGA etc.) If the decoding of subframe 3 turns up a parity error at any point then a flag is set (the “probation” flag in the code) which prevents the reception time code from using this sat in any solution computation for a few subframe times. But suppose one of the bits of the subframe 3 sync word is received incorrectly. The subframe 3 detection is missed entirely and its TOW will never be stored. And the reception time code is never told to not use the sat. So when a solution is attempted during the subframe 4 time the TOW from subframe 2 will be used instead of the correct subframe 3 which was never stored. The frequency of this situation is reduced somewhat because a sync pattern match can occur in the middle of normal subframe data. And that would trigger the parity checking code which would almost certainly fail and thus set the probation flag.
I discovered this while looking at the code in detail because Galileo, unlike Navstar, doesn’t transmit a TOW per subframe (page in Galileo terminology). And so the concept of current TOW will have to be handled differently for Galileo. The reception time code will possibly have to synthesize the current TOW based on the last one received and a local measurement of how long it’s been.
Hi @jks-prv,
as far as I am concerned this issue can be closed
Cheers Christoph
Hi @jks-prv,
this is not an issue but a possible enhancement.
As you have written here (https://github.com/jks-prv/Beagle_SDR_GPS/issues/146#issuecomment-361100778) it would be interesting to improve the accuracy of the GPS position+time solution.
I had a look at the GPS position solutions, placing the GPS patch antenna just outside my window on a short wooden boom at a position where more than half of the sky is obstructed with buildings. With this setup, GPS position solutions are available for much less than 50% of the time.
For about 10 hours I saved the GPS measurements, i.e.,
t_tx,X,Y,Z
of all tracked GPS satellites, and the ADC tick counter per epoch in a text file in order to test different methods of position solutions.The plot below shows the single point position (SPP) in blue and the output of an extended Kalman filter (EKF) in red: and here are the same plots with smaller ranges of the y-axis:
Note that is is very preliminary as
The EKF is used starting from the time a single point position solution (SPP) is first available. After this, it can update position, time, and frequency offset also with less than 4 satellites available.
I find that sometimes the GPS satellite transmit times
t_tx
contain outlier values, i.e., times corresponding to range offsets of more than 1000km relative to the expected times; the EKF can spot these outliers by comparing predicted and measured pseudo-ranges during the filter update and in the plots above these outliers have been removed.This looks like a bug in the GPS code as the time offsets are all exactly 20ms (with relative precision of < 5e-6); one possible location to start investigating this bug would be this one https://github.com/jks-prv/Beagle_SDR_GPS/blob/master/gps/solve.cpp#L157
For linear algebra I use the eigen library; I have checked that it compiles on the beagle-bone and does generate NEON SIMD code.
In terms of lines of code it would add ~200-300 lines and probably remove some of the lines for outlier filtering of the oscillator frequency offset.
Let me know what you think about this.